Rev 2017 | Rev 2026 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2017 | Rev 2019 | ||
---|---|---|---|
Line 60... | Line 60... | ||
60 | 60 | ||
61 | // The channel array is now 0-based. |
61 | // The channel array is now 0-based. |
62 | volatile int16_t PPM_in[MAX_CHANNELS]; |
62 | volatile int16_t PPM_in[MAX_CHANNELS]; |
63 | volatile int16_t PPM_diff[MAX_CHANNELS]; |
63 | volatile int16_t PPM_diff[MAX_CHANNELS]; |
64 | volatile uint8_t NewPpmData = 1; |
64 | volatile uint8_t NewPpmData = 1; |
65 | volatile uint8_t RC_Quality = 0; |
65 | volatile uint8_t RCQuality = 0; |
66 | int16_t RC_PRTY[4]; |
66 | int16_t RC_PRTY[4]; |
67 | uint8_t lastRCCommand = COMMAND_NONE; |
67 | uint8_t lastRCCommand = COMMAND_NONE; |
Line 68... | Line 68... | ||
68 | uint8_t commandTimer = 0; |
68 | uint8_t commandTimer = 0; |
Line 112... | Line 112... | ||
112 | // Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0) |
112 | // Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0) |
113 | // Enable Overflow Interrupt (bit: TOIE1=0) |
113 | // Enable Overflow Interrupt (bit: TOIE1=0) |
114 | TIMSK1 &= ~((1<<OCIE1B) | (1<<OCIE1A) | (1<<TOIE1)); |
114 | TIMSK1 &= ~((1<<OCIE1B) | (1<<OCIE1A) | (1<<TOIE1)); |
115 | TIMSK1 |= (1<<ICIE1); |
115 | TIMSK1 |= (1<<ICIE1); |
Line 116... | Line 116... | ||
116 | 116 | ||
Line 117... | Line 117... | ||
117 | RC_Quality = 0; |
117 | RCQuality = 0; |
118 | 118 | ||
Line 119... | Line 119... | ||
119 | SREG = sreg; |
119 | SREG = sreg; |
Line 171... | Line 171... | ||
171 | if ((signal > 250) && (signal < 687)) { |
171 | if ((signal > 250) && (signal < 687)) { |
172 | // shift signal to zero symmetric range -154 to 159 |
172 | // shift signal to zero symmetric range -154 to 159 |
173 | signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2us = 1.5008 ms) |
173 | signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2us = 1.5008 ms) |
174 | // check for stable signal |
174 | // check for stable signal |
175 | if (abs(signal - PPM_in[index]) < 6) { |
175 | if (abs(signal - PPM_in[index]) < 6) { |
176 | if (RC_Quality < 200) |
176 | if (RCQuality < 200) |
177 | RC_Quality += 10; |
177 | RCQuality += 10; |
178 | else |
178 | else |
179 | RC_Quality = 200; |
179 | RCQuality = 200; |
180 | } |
180 | } |
181 | // If signal is the same as before +/- 1, just keep it there. |
181 | // If signal is the same as before +/- 1, just keep it there. |
182 | if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) { |
182 | if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) { |
183 | // In addition, if the signal is very close to 0, just set it to 0. |
183 | // In addition, if the signal is very close to 0, just set it to 0. |
184 | if (signal >= -1 && signal <= 1) { |
184 | if (signal >= -1 && signal <= 1) { |
Line 187... | Line 187... | ||
187 | tmp = PPM_in[index]; |
187 | tmp = PPM_in[index]; |
188 | } |
188 | } |
189 | } else |
189 | } else |
190 | tmp = signal; |
190 | tmp = signal; |
191 | // calculate signal difference on good signal level |
191 | // calculate signal difference on good signal level |
192 | if (RC_Quality >= 195) |
192 | if (RCQuality >= 195) |
193 | PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction |
193 | PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction |
194 | else |
194 | else |
195 | PPM_diff[index] = 0; |
195 | PPM_diff[index] = 0; |
196 | PPM_in[index] = tmp; // update channel value |
196 | PPM_in[index] = tmp; // update channel value |
197 | } |
197 | } |
Line 239... | Line 239... | ||
239 | /* |
239 | /* |
240 | * This must be called (as the only thing) for each control loop cycle (488 Hz). |
240 | * This must be called (as the only thing) for each control loop cycle (488 Hz). |
241 | */ |
241 | */ |
242 | void RC_update() { |
242 | void RC_update() { |
243 | int16_t tmp1, tmp2; |
243 | int16_t tmp1, tmp2; |
244 | if (RC_Quality) { |
244 | if (RCQuality) { |
245 | RC_Quality--; |
245 | RCQuality--; |
246 | if (NewPpmData-- == 0) { |
246 | if (NewPpmData-- == 0) { |
247 | RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP |
247 | RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP |
248 | + RCDiff(CH_PITCH) * staticParams.stickD; |
248 | + RCDiff(CH_PITCH) * staticParams.stickD; |
249 | RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.stickP |
249 | RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.stickP |
250 | + RCDiff(CH_ROLL) * staticParams.stickD; |
250 | + RCDiff(CH_ROLL) * staticParams.stickD; |
Line 272... | Line 272... | ||
272 | } |
272 | } |
273 | } else { // Bad signal |
273 | } else { // Bad signal |
274 | RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE] |
274 | RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE] |
275 | = RC_PRTY[CONTROL_YAW] = 0; |
275 | = RC_PRTY[CONTROL_YAW] = 0; |
276 | } |
276 | } |
- | 277 | ||
- | 278 | debugOut.analog[21] = RCQuality; |
|
277 | } |
279 | } |
Line 278... | Line 280... | ||
278 | 280 | ||
279 | /* |
281 | /* |
280 | * Get Pitch, Roll, Throttle, Yaw values |
282 | * Get Pitch, Roll, Throttle, Yaw values |
Line 296... | Line 298... | ||
296 | */ |
298 | */ |
297 | return PPM_in[varNum] + POT_OFFSET; |
299 | return PPM_in[varNum] + POT_OFFSET; |
298 | } |
300 | } |
Line 299... | Line 301... | ||
299 | 301 | ||
300 | uint8_t RC_getSignalQuality(void) { |
302 | uint8_t RC_getSignalQuality(void) { |
301 | if (RC_Quality >= 160) |
303 | if (RCQuality >= 160) |
302 | return SIGNAL_GOOD; |
304 | return SIGNAL_GOOD; |
303 | if (RC_Quality >= 140) |
305 | if (RCQuality >= 140) |
304 | return SIGNAL_OK; |
306 | return SIGNAL_OK; |
305 | if (RC_Quality >= 120) |
307 | if (RCQuality >= 120) |
306 | return SIGNAL_BAD; |
308 | return SIGNAL_BAD; |
307 | return SIGNAL_LOST; |
309 | return SIGNAL_LOST; |
Line 308... | Line 310... | ||
308 | } |
310 | } |