Subversion Repositories FlightCtrl

Rev

Rev 684 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 684 Rev 694
Line -... Line 1...
-
 
1
#include <stdlib.h>
1
#include <avr/pgmspace.h>
2
#include <avr/pgmspace.h>
2
#include "mymath.h"
3
#include "mymath.h"
Line 3... Line 4...
3
 
4
 
Line 13... Line 14...
13
 
14
 
14
        // avoid negative angles
15
        // avoid negative angles
15
        if (angle < 0)
16
        if (angle < 0)
16
        {
17
        {
17
                m = -1;
18
                m = -1;
18
                angle = -angle;
19
                angle = abs(angle);
19
        }
20
        }
Line 20... Line 21...
20
        else m = 1;
21
        else m = +1;
21
 
22
 
Line 22... Line 23...
22
        // fold angle to intervall 0 to 359
23
        // fold angle to intervall 0 to 359
23
        angle %= 360;
24
        angle %= 360;
24
 
25
 
25
        // queck quadrant
26
        // check quadrant
26
        if (angle <= 90) n=1; // first quadrant
27
        if (angle <= 90) n=1; // first quadrant
27
        else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant
28
        else if ((angle > 90) && (angle <= 180)) {angle = 180 - angle; n = 1;} // second quadrant