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 |