Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1775 | - | 1 | #include <inttypes.h> |
| 2 | #include "attitude.h" |
||
| 3 | #include "uart0.h" |
||
| 4 | #include "configuration.h" |
||
| 5 | #include "dongfangMath.h" |
||
| 6 | |||
| 7 | // For scope debugging only! |
||
| 1872 | - | 8 | #include "output.h" |
| 1775 | - | 9 | |
| 10 | // = cos^2(45 degs). |
||
| 1887 | - | 11 | // const int32_t FACTORSQUARED = 1L << (MATH_UNIT_FACTOR_LOG * 2); |
| 12 | const int32_t MINPROJECTION = 1L << (MATH_UNIT_FACTOR_LOG * 2 - 9); |
||
| 1775 | - | 13 | |
| 14 | // Takes 380 - 400 usec. Way too slow. |
||
| 15 | // With static MINPROJECTION: 220 usec. |
||
| 16 | uint16_t AC_getThrottle(uint16_t throttle) { |
||
| 1877 | - | 17 | int32_t projection; |
| 1960 | - | 18 | uint8_t effect = dynamicParams.attitudeControl; // Userparam 3 |
| 1887 | - | 19 | int16_t deltaThrottle, y; |
| 20 | |||
| 2045 | - | 21 | int16_t rollAngleInDegrees = angle[ROLL] / GYRO_DEG_FACTOR_PITCHROLL; |
| 22 | int16_t pitchAngleInDegrees = angle[PITCH] / GYRO_DEG_FACTOR_PITCHROLL; |
||
| 23 | |||
| 24 | projection = (int32_t) cos_360(pitchAngleInDegrees) * (int32_t) cos_360(rollAngleInDegrees); |
||
| 1887 | - | 25 | projection >>= 8; |
| 26 | |||
| 27 | if (projection < 0) { |
||
| 28 | // Case not yet considered! |
||
| 29 | y = 0; |
||
| 30 | } else { |
||
| 1877 | - | 31 | if (projection < MINPROJECTION && projection >= 0) { |
| 1887 | - | 32 | projection = MINPROJECTION; |
| 1877 | - | 33 | } else if (projection > -MINPROJECTION && projection < 0) { |
| 1887 | - | 34 | projection = -MINPROJECTION; |
| 1877 | - | 35 | } else { |
| 1887 | - | 36 | } |
| 37 | y = ((int32_t) throttle << (MATH_UNIT_FACTOR_LOG * 2 - 8)) / projection |
||
| 38 | - throttle; |
||
| 39 | } |
||
| 40 | deltaThrottle = ((int32_t)y * effect) >> 6; |
||
| 1978 | - | 41 | // debugOut.analog[8] = deltaThrottle; |
| 1877 | - | 42 | return throttle + deltaThrottle; |
| 43 | } |