Rev 1635 | Rev 1775 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1635 | Rev 1645 | ||
|---|---|---|---|
| Line 54... | Line 54... | ||
| 54 | 54 | ||
| 55 | #include "rc.h" |
55 | #include "rc.h" |
| 56 | #include "controlMixer.h" |
56 | #include "controlMixer.h" |
| Line 57... | Line -... | ||
| 57 | #include "configuration.h" |
- | |
| 58 | - | ||
| 59 | // for DebugOut only. |
- | |
| 60 | #include "uart0.h" |
- | |
| 61 | 57 | #include "configuration.h" |
|
| 62 | 58 | ||
| 63 | // The channel array is 1-based. The 0th entry is not used. |
59 | // The channel array is 1-based. The 0th entry is not used. |
| 64 | volatile int16_t PPM_in[MAX_CHANNELS]; |
60 | volatile int16_t PPM_in[MAX_CHANNELS]; |
| 65 | volatile int16_t PPM_diff[MAX_CHANNELS]; |
61 | volatile int16_t PPM_diff[MAX_CHANNELS]; |
| Line 120... | Line 116... | ||
| 120 | RC_Quality = 0; |
116 | RC_Quality = 0; |
| Line 121... | Line 117... | ||
| 121 | 117 | ||
| 122 | SREG = sreg; |
118 | SREG = sreg; |
| Line 123... | Line -... | ||
| 123 | } |
- | |
| 124 | 119 | } |
|
| 125 | 120 | ||
| 126 | /********************************************************************/ |
121 | /********************************************************************/ |
| 127 | /* Every time a positive edge is detected at PD6 */ |
122 | /* Every time a positive edge is detected at PD6 */ |
| 128 | /********************************************************************/ |
123 | /********************************************************************/ |
| Line 214... | Line 209... | ||
| 214 | void RC_update() { |
209 | void RC_update() { |
| 215 | int16_t tmp1, tmp2; |
210 | int16_t tmp1, tmp2; |
| 216 | if(RC_Quality) { |
211 | if(RC_Quality) { |
| 217 | RC_Quality--; |
212 | RC_Quality--; |
| 218 | if (NewPpmData-- == 0) { |
213 | if (NewPpmData-- == 0) { |
| 219 | RC_PRTY[CONTROL_PITCH] = (RCChannel(CH_PITCH) - stickOffsetPitch) * staticParams.StickP + RCDiff(CH_PITCH) * staticParams.StickD; |
214 | RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.StickP - stickOffsetPitch + RCDiff(CH_PITCH) * staticParams.StickD; |
| 220 | RC_PRTY[CONTROL_ROLL] = (RCChannel(CH_ROLL) - stickOffsetRoll) * staticParams.StickP + RCDiff(CH_ROLL) * staticParams.StickD; |
215 | RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP - stickOffsetRoll + RCDiff(CH_ROLL) * staticParams.StickD; |
| 221 | RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + PPM_diff[staticParams.ChannelAssignment[CH_THROTTLE]] * dynamicParams.UserParams[3] + 120; |
216 | RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + PPM_diff[staticParams.ChannelAssignment[CH_THROTTLE]] * dynamicParams.UserParams[3] + 120; |
| 222 | if (RC_PRTY[CONTROL_THROTTLE] < 0) RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
217 | if (RC_PRTY[CONTROL_THROTTLE] < 0) RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
| 223 | tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
218 | tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
| 224 | // exponential stick sensitivity in yawring rate |
219 | // exponential stick sensitivity in yawring rate |
| 225 | tmp2 = (int32_t) staticParams.StickYawP * ((int32_t)tmp1 * abs(tmp1)) / 512L; // expo y = ax + bx^2 |
220 | tmp2 = (int32_t) staticParams.StickYawP * ((int32_t)tmp1 * abs(tmp1)) / 512L; // expo y = ax + bx^2 |
| Line 278... | Line 273... | ||
| 278 | void RC_calibrate(void) { |
273 | void RC_calibrate(void) { |
| 279 | if (staticParams.GlobalConfig & CFG_HEADING_HOLD) { |
274 | if (staticParams.GlobalConfig & CFG_HEADING_HOLD) { |
| 280 | // In HH, it s OK to trim the R/C. The effect should not be conteracted here. |
275 | // In HH, it s OK to trim the R/C. The effect should not be conteracted here. |
| 281 | stickOffsetPitch = stickOffsetRoll = 0; |
276 | stickOffsetPitch = stickOffsetRoll = 0; |
| 282 | } else { |
277 | } else { |
| 283 | stickOffsetPitch = RCChannel(CH_PITCH); |
278 | stickOffsetPitch = RCChannel(CH_PITCH) * staticParams.StickP; |
| 284 | stickOffsetRoll = RCChannel(CH_ROLL); |
279 | stickOffsetRoll = RCChannel(CH_ROLL) * staticParams.StickP; |
| 285 | } |
280 | } |
| 286 | } |
281 | } |
| Line 287... | Line 282... | ||
| 287 | 282 | ||
| 288 | #define COMMAND_THRESHOLD 85 |
283 | #define COMMAND_THRESHOLD 85 |