Rev 2099 | Rev 2110 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2099 | Rev 2103 | ||
|---|---|---|---|
| Line 26... | Line 26... | ||
| 26 | * things functionally without side effects) but this is shorter and probably |
26 | * things functionally without side effects) but this is shorter and probably |
| 27 | * faster too. |
27 | * faster too. |
| 28 | * The variables are overwritten at each attitude calculation invocation - the values |
28 | * The variables are overwritten at each attitude calculation invocation - the values |
| 29 | * are not preserved or reused. |
29 | * are not preserved or reused. |
| 30 | */ |
30 | */ |
| 31 | int16_t rate_ATT[3]; |
- | |
| 32 | - | ||
| 33 | // With different (less) filtering |
- | |
| 34 | int16_t rate_PID[3]; |
- | |
| 35 | int16_t differential[3]; |
- | |
| Line 36... | Line 31... | ||
| 36 | 31 | ||
| 37 | /* |
32 | /* |
| 38 | * Gyro integrals. These are the rotation angles of the airframe compared to the |
33 | * Gyro integrals. These are the rotation angles of the airframe compared to the |
| 39 | * horizontal plane, yaw relative to yaw at start. |
34 | * horizontal plane, yaw relative to yaw at start. |
| Line 84... | Line 79... | ||
| 84 | * the values into variables). |
79 | * the values into variables). |
| 85 | * The rate variable end up in a range of about [-1024, 1023]. |
80 | * The rate variable end up in a range of about [-1024, 1023]. |
| 86 | *************************************************************************/ |
81 | *************************************************************************/ |
| 87 | void getAnalogData(void) { |
82 | void getAnalogData(void) { |
| 88 | uint8_t axis; |
83 | uint8_t axis; |
| 89 | - | ||
| 90 | analog_update(); |
84 | analog_update(); |
| 91 | - | ||
| 92 | for (axis = PITCH; axis <= YAW; axis++) { |
85 | for (axis = PITCH; axis <= YAW; axis++) { |
| 93 | rate_PID[axis] = gyro_PID[axis]; |
- | |
| 94 | rate_ATT[axis] = gyro_ATT[axis]; |
- | |
| 95 | differential[axis] = gyroD[axis]; |
- | |
| 96 | } |
86 | } |
| 97 | } |
87 | } |
| Line 98... | Line 88... | ||
| 98 | 88 | ||
| 99 | void integrate(void) { |
89 | void integrate(void) { |
| Line 108... | Line 98... | ||
| 108 | /* |
98 | /* |
| 109 | * Pitch axis integration and range boundary wrap. |
99 | * Pitch axis integration and range boundary wrap. |
| 110 | */ |
100 | */ |
| Line 111... | Line 101... | ||
| 111 | 101 | ||
| 112 | for (axis = PITCH; axis <= YAW; axis++) { |
102 | for (axis = PITCH; axis <= YAW; axis++) { |
| 113 | attitude[axis] += rate_ATT[axis]; |
103 | attitude[axis] += gyro_ATT[axis]; |
| 114 | if (attitude[axis] > OVER180) { |
104 | if (attitude[axis] > OVER180) { |
| 115 | attitude[axis] -= OVER360; |
105 | attitude[axis] -= OVER360; |
| 116 | } else if (attitude[axis] <= -OVER180) { |
106 | } else if (attitude[axis] <= -OVER180) { |
| 117 | attitude[axis] += OVER360; |
107 | attitude[axis] += OVER360; |