NavX MXP Continuous Angle to Calculate Derivative

[Ether]

Originally Posted by dougwilliams

You should be able to use some modulo arithmetic operations to solve the jump from 0 / 360.

Originally Posted by JamesTerm

Would x=x%180.... (modulo operator) work?

modulo is not what you want here.

what you want is the shortest angle from the previous reading to the present reading, with the correct sign. The IEEERemainder function¹ does this with one line of code:

shortest_angle = IEEERemainder(present-previous,360);

C# and Java both support IEEERemainder.


If your language does not support IEEERemainder, you can use this one-line function instead:

shortest_angle = (present-previous) - 360*floor(0.5+(present-previous)/360);

EDIT:

Test code:

#include <math.h>
#include <stdio.h>

double shortest_angle(double previous, double present){
	return (present-previous) - 360.0*floor(0.5+(present-previous)/360.0);}

void test(double previous, double present){
	printf("previous=%f\tpresent=%f\tshortest_angle=%f\n",previous,present,shortest_angle(previous,present));
	}

void main(void){
	test(360,0);
	test(0,360);
	test(360,1);
	test(1,360);
	test(359,0);
	test(0,359);
	test(1,359);
	test(359,1);
	test(359,-1);
	test(-1,359);
	test(3*360+1,-1);
	test(-1,3*360+1);
	}

Output:

previous=360.000000     present=0.000000        shortest_angle=0.000000
previous=0.000000       present=360.000000      shortest_angle=0.000000
previous=360.000000     present=1.000000        shortest_angle=1.000000
previous=1.000000       present=360.000000      shortest_angle=-1.000000
previous=359.000000     present=0.000000        shortest_angle=1.000000
previous=0.000000       present=359.000000      shortest_angle=-1.000000
previous=1.000000       present=359.000000      shortest_angle=-2.000000
previous=359.000000     present=1.000000        shortest_angle=2.000000
previous=359.000000     present=-1.000000       shortest_angle=0.000000
previous=-1.000000      present=359.000000      shortest_angle=0.000000
previous=1081.000000    present=-1.000000       shortest_angle=-2.000000
previous=-1.000000      present=1081.000000     shortest_angle=2.000000

¹ REMAINDER function "x REM y" per IEC 60559 as specified on Page 235 Section 7.12.10.2 of ISO/IEC 9899:TC3