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 |