Subversion Repositories FlightCtrl

Rev

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

Rev 694 Rev 746
Line 75... Line 75...
75 
        if (x > 0) return (angle * m);  // 1st and 4th quadrant
 75 
        if (x > 0) return (angle * m);  // 1st and 4th quadrant
76 
        else if ((x < 0) && (m > 0)) return (180 - angle);      // 2nd quadrant
 76 
        else if ((x < 0) && (m > 0)) return (180 - angle);      // 2nd quadrant
77 
        else return (angle - 180); // ( (x < 0) && (y < 0))     3rd quadrant
 77 
        else return (angle - 180); // ( (x < 0) && (y < 0))     3rd quadrant
78 
}
 78 
}
Line -... Line 79...
- 
 
 79 
 
- 
 
 80 
 
- 
 
 81 
 
- 
 
 82 
// integer square root
- 
 
 83 
uint32_t c_sqrt(uint32_t number)
- 
 
 84 
{
- 
 
 85 
        uint32_t s1, s2;
- 
 
 86 
        uint8_t iter = 0;
- 
 
 87 
        // initialization of iteration
- 
 
 88 
        s2 = number;
- 
 
 89 
        do // iterative formula to solve x^2 - n = 0
- 
 
 90 
        {
- 
 
 91 
                s1 = s2;
- 
 
 92 
                s2 = number / s1;
- 
 
 93 
                s2 += s1;
- 
 
 94 
                s2 /= 2;
- 
 
 95 
                iter++;
- 
 
 96 
                //if(iter > 40) break;
- 
 
 97 
        }while( ( (s1-s2) > 1) && (iter < 40));
- 
 
 98 
        return s2;