Rev 1872 | Rev 1955 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1872 | Rev 1908 | ||
|---|---|---|---|
| Line 58... | Line 58... | ||
| 58 | #include "configuration.h" |
58 | #include "configuration.h" |
| 59 | #include "attitude.h" |
59 | #include "attitude.h" |
| 60 | #include "commands.h" |
60 | #include "commands.h" |
| 61 | #include "output.h" |
61 | #include "output.h" |
| Line 62... | Line 62... | ||
| 62 | 62 | ||
| - | 63 | // uint16_t maxControl[2] = { 0, 0 }; |
|
| 63 | uint16_t maxControl[2] = { 0, 0 }; |
64 | uint16_t controlActivity = 0; |
| 64 | int16_t control[2] = { 0, 0 }; |
65 | int16_t controls[4] = { 0, 0, 0, 0 }; |
| 65 | int16_t controlYaw = 0, controlThrottle = 0; |
66 | //int16_t controlYaw = 0, controlThrottle = 0; |
| Line 66... | Line 67... | ||
| 66 | uint8_t looping = 0; |
67 | uint8_t looping = 0; |
| 67 | 68 | ||
| 68 | // Internal variables for reading commands made with an R/C stick. |
69 | // Internal variables for reading commands made with an R/C stick. |
| Line 134... | Line 135... | ||
| 134 | uint8_t ecQ = EC_getSignalQuality(); |
135 | uint8_t ecQ = EC_getSignalQuality(); |
| 135 | // This needs not be the only correct solution... |
136 | // This needs not be the only correct solution... |
| 136 | return rcQ > ecQ ? rcQ : ecQ; |
137 | return rcQ > ecQ ? rcQ : ecQ; |
| 137 | } |
138 | } |
| Line -... | Line 139... | ||
| - | 139 | ||
| - | 140 | void updateControlAndMeasureControlActivity(uint8_t index, int16_t newValue) { |
|
| - | 141 | int16_t tmp = controls[index]; |
|
| - | 142 | controls[index] = newValue; |
|
| - | 143 | tmp -= newValue; |
|
| - | 144 | tmp /= 2; |
|
| - | 145 | tmp = tmp * tmp; |
|
| - | 146 | // tmp += (newValue >= 0) ? newValue : -newValue; |
|
| - | 147 | if (controlActivity + (uint16_t)tmp >= controlActivity) |
|
| - | 148 | controlActivity += tmp; |
|
| - | 149 | else controlActivity = 0xffff; |
|
| - | 150 | } |
|
| - | 151 | ||
| - | 152 | #define CADAMPING 10 |
|
| - | 153 | void dampenControlActivity(void) { |
|
| - | 154 | uint32_t tmp = controlActivity; |
|
| - | 155 | tmp *= ((1<<CADAMPING)-1); |
|
| - | 156 | tmp >>= CADAMPING; |
|
| - | 157 | controlActivity = tmp; |
|
| - | 158 | } |
|
| 138 | 159 | ||
| 139 | /* |
160 | /* |
| 140 | * Update the variables indicating stick position from the sum of R/C, GPS and external control. |
161 | * Update the variables indicating stick position from the sum of R/C, GPS and external control. |
| 141 | */ |
162 | */ |
| 142 | void controlMixer_update(void) { |
163 | void controlMixer_update(void) { |
| 143 | // calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
164 | // calculate Stick inputs by rc channels (P) and changing of rc channels (D) |
| 144 | // TODO: If no signal --> zero. |
- | |
| 145 | uint8_t axis; |
165 | // TODO: If no signal --> zero. |
| Line 146... | Line 166... | ||
| 146 | int16_t tempThrottle; |
166 | int16_t tempThrottle; |
| 147 | 167 | ||
| Line 152... | Line 172... | ||
| 152 | EC_update(); |
172 | EC_update(); |
| Line 153... | Line 173... | ||
| 153 | 173 | ||
| 154 | // takes about 80 usec. |
174 | // takes about 80 usec. |
| Line 155... | Line 175... | ||
| 155 | HC_update(); |
175 | HC_update(); |
| 156 | 176 | ||
| Line 157... | Line 177... | ||
| 157 | int16_t* RC_PRTY = RC_getPRTY(); |
177 | int16_t* RC_PRTY = RC_getPRTY(); |
| 158 | int16_t* EC_PRTY = EC_getPRTY(); |
178 | int16_t* EC_PRTY = EC_getPRTY(); |
| - | 179 | ||
| - | 180 | updateControlAndMeasureControlActivity(CONTROL_PITCH, RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH]); |
|
| Line 159... | Line -... | ||
| 159 | - | ||
| 160 | control[PITCH] = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH]; |
181 | updateControlAndMeasureControlActivity(CONTROL_ROLL, RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]); |
| Line 161... | Line 182... | ||
| 161 | control[ROLL] = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]; |
182 | updateControlAndMeasureControlActivity(CONTROL_YAW, RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]); |
| - | 183 | dampenControlActivity(); |
|
| Line 162... | Line -... | ||
| 162 | - | ||
| 163 | tempThrottle = HC_getThrottle(RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE]); |
184 | |
| Line 164... | Line 185... | ||
| 164 | controlThrottle = AC_getThrottle(tempThrottle); |
185 | DebugOut.Analog[14] = controlActivity/10; |
| 165 | 186 | ||
| 166 | controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]; |
187 | tempThrottle = HC_getThrottle(RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE]); |
| 167 | 188 | controls[CONTROL_THROTTLE] = AC_getThrottle(tempThrottle); |
|
| Line 188... | Line 209... | ||
| 188 | } |
209 | } |
| 189 | */ |
210 | */ |
| Line 190... | Line 211... | ||
| 190 | 211 | ||
| 191 | /* |
212 | /* |
| 192 | * Record maxima |
- | |
| 193 | */ |
213 | * Record maxima |
| 194 | for (axis = PITCH; axis <= ROLL; axis++) { |
214 | for (axis = PITCH; axis <= ROLL; axis++) { |
| 195 | if (abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) { |
215 | if (abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) { |
| 196 | maxControl[axis] = abs(control[axis]) / CONTROL_SCALING; |
216 | maxControl[axis] = abs(control[axis]) / CONTROL_SCALING; |
| 197 | if (maxControl[axis] > 100) |
217 | if (maxControl[axis] > 100) |
| 198 | maxControl[axis] = 100; |
218 | maxControl[axis] = 100; |
| 199 | } else if (maxControl[axis]) |
219 | } else if (maxControl[axis]) |
| 200 | maxControl[axis]--; |
220 | maxControl[axis]--; |
| - | 221 | } |
|
| Line 201... | Line 222... | ||
| 201 | } |
222 | */ |
| 202 | 223 | ||
| 203 | uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() |
224 | uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() |
| 204 | : COMMAND_NONE; |
225 | : COMMAND_NONE; |