Rev 1612 | Rev 1635 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1612 | Rev 1634 | ||
---|---|---|---|
Line 54... | Line 54... | ||
54 | 54 | ||
55 | #include "rc.h" |
55 | #include "rc.h" |
56 | #include "controlMixer.h" |
56 | #include "controlMixer.h" |
Line -... | Line 57... | ||
- | 57 | #include "configuration.h" |
|
- | 58 | ||
- | 59 | // for DebugOut only. |
|
- | 60 | #include "uart0.h" |
|
57 | #include "configuration.h" |
61 | |
58 | 62 | ||
59 | // The channel array is 1-based. The 0th entry is not used. |
63 | // The channel array is 1-based. The 0th entry is not used. |
60 | volatile int16_t PPM_in[MAX_CHANNELS]; |
64 | volatile int16_t PPM_in[MAX_CHANNELS]; |
61 | volatile int16_t PPM_diff[MAX_CHANNELS]; |
65 | volatile int16_t PPM_diff[MAX_CHANNELS]; |
Line 125... | Line 129... | ||
125 | /* t-Frame |
129 | /* t-Frame |
126 | <-----------------------------------------------------------------------> |
130 | <-----------------------------------------------------------------------> |
127 | ____ ______ _____ ________ ______ sync gap ____ |
131 | ____ ______ _____ ________ ______ sync gap ____ |
128 | | | | | | | | | | | | |
132 | | | | | | | | | | | | |
129 | | | | | | | | | | | | |
133 | | | | | | | | | | | | |
130 | ___| |_| |_| |_| |_.............| |________________| |
134 | ___| |_| |_| |_| |_.............| |________________| |
131 | <-----><-------><------><--------> <------> <--- |
135 | <-----><-------><------><--------> <------> <--- |
132 | t0 t1 t2 t4 tn t0 |
136 | t0 t1 t2 t4 tn t0 |
Line 133... | Line 137... | ||
133 | 137 | ||
134 | The PPM-Frame length is 22.5 ms. |
138 | The PPM-Frame length is 22.5 ms. |
Line 210... | Line 214... | ||
210 | void RC_update() { |
214 | void RC_update() { |
211 | int16_t tmp1, tmp2; |
215 | int16_t tmp1, tmp2; |
212 | if(RC_Quality) { |
216 | if(RC_Quality) { |
213 | RC_Quality--; |
217 | RC_Quality--; |
214 | if (NewPpmData-- == 0) { |
218 | if (NewPpmData-- == 0) { |
- | 219 | ||
- | 220 | DebugOut.Analog[12] = stickOffsetPitch; |
|
- | 221 | DebugOut.Analog[13] = stickOffsetRoll; |
|
- | 222 | ||
215 | RC_PRTY[CONTROL_PITCH] = (RCChannel(CH_PITCH) - stickOffsetPitch) * staticParams.StickP + RCDiff(CH_PITCH) * staticParams.StickD; |
223 | RC_PRTY[CONTROL_PITCH] = (RCChannel(CH_PITCH) - stickOffsetPitch) * staticParams.StickP + RCDiff(CH_PITCH) * staticParams.StickD; |
216 | RC_PRTY[CONTROL_ROLL] = (RCChannel(CH_ROLL) - stickOffsetRoll) * staticParams.StickP + RCDiff(CH_ROLL) * staticParams.StickD; |
224 | RC_PRTY[CONTROL_ROLL] = (RCChannel(CH_ROLL) - stickOffsetRoll) * staticParams.StickP + RCDiff(CH_ROLL) * staticParams.StickD; |
217 | RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + PPM_diff[staticParams.ChannelAssignment[CH_THROTTLE]] * dynamicParams.UserParams[6] + 120; |
225 | RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + PPM_diff[staticParams.ChannelAssignment[CH_THROTTLE]] * dynamicParams.UserParams[6] + 120; |
218 | if (RC_PRTY[CONTROL_THROTTLE] < 0) RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
226 | if (RC_PRTY[CONTROL_THROTTLE] < 0) RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
219 | tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
227 | tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
220 | // exponential stick sensitivity in yawring rate |
228 | // exponential stick sensitivity in yawring rate |
221 | tmp2 = (int32_t) staticParams.StickYawP * ((int32_t)tmp1 * abs(tmp1)) / 512L; // expo y = ax + bx^2 |
229 | tmp2 = (int32_t) staticParams.StickYawP * ((int32_t)tmp1 * abs(tmp1)) / 512L; // expo y = ax + bx^2 |
Line 274... | Line 282... | ||
274 | void RC_calibrate(void) { |
282 | void RC_calibrate(void) { |
275 | if (staticParams.GlobalConfig & CFG_HEADING_HOLD) { |
283 | if (staticParams.GlobalConfig & CFG_HEADING_HOLD) { |
276 | // In HH, it s OK to trim the R/C. The effect should not be conteracted here. |
284 | // In HH, it s OK to trim the R/C. The effect should not be conteracted here. |
277 | stickOffsetPitch = stickOffsetRoll = 0; |
285 | stickOffsetPitch = stickOffsetRoll = 0; |
278 | } else { |
286 | } else { |
279 | stickOffsetPitch += RCChannel(CH_PITCH); |
287 | stickOffsetPitch = RCChannel(CH_PITCH); |
280 | stickOffsetRoll += RCChannel(CH_ROLL); |
288 | stickOffsetRoll = RCChannel(CH_ROLL); |
281 | } |
289 | } |
282 | } |
290 | } |
Line 283... | Line 291... | ||
283 | 291 | ||
284 | #define COMMAND_THRESHOLD 85 |
292 | #define COMMAND_THRESHOLD 85 |