Rev 1645 | Rev 1821 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1645 | Rev 1775 | ||
---|---|---|---|
Line 51... | Line 51... | ||
51 | #include <stdlib.h> |
51 | #include <stdlib.h> |
52 | #include <avr/io.h> |
52 | #include <avr/io.h> |
53 | #include <avr/interrupt.h> |
53 | #include <avr/interrupt.h> |
Line 54... | Line 54... | ||
54 | 54 | ||
- | 55 | #include "rc.h" |
|
55 | #include "rc.h" |
56 | #include "uart0.h" |
56 | #include "controlMixer.h" |
57 | #include "controlMixer.h" |
- | 58 | #include "configuration.h" |
|
Line 57... | Line 59... | ||
57 | #include "configuration.h" |
59 | #include "commands.h" |
58 | 60 | ||
59 | // The channel array is 1-based. The 0th entry is not used. |
61 | // The channel array is 1-based. The 0th entry is not used. |
60 | volatile int16_t PPM_in[MAX_CHANNELS]; |
62 | volatile int16_t PPM_in[MAX_CHANNELS]; |
61 | volatile int16_t PPM_diff[MAX_CHANNELS]; |
63 | volatile int16_t PPM_diff[MAX_CHANNELS]; |
62 | volatile uint8_t NewPpmData = 1; |
64 | volatile uint8_t NewPpmData = 1; |
- | 65 | volatile int16_t RC_Quality = 0; |
|
- | 66 | int16_t RC_PRTY[4]; |
|
Line -... | Line 67... | ||
- | 67 | uint8_t lastRCCommand = COMMAND_NONE; |
|
63 | volatile int16_t RC_Quality = 0; |
68 | uint8_t commandTimer = 0; |
Line 64... | Line 69... | ||
64 | int16_t RC_PRTY[4]; |
69 | |
65 | 70 | // Useless. Just trim on the R/C instead. |
|
66 | int16_t stickOffsetPitch = 0, stickOffsetRoll = 0; |
71 | // int16_t stickOffsetPitch = 0, stickOffsetRoll = 0; |
67 | 72 | ||
Line 78... | Line 83... | ||
78 | DDRD &= ~(1<<DDD6); |
83 | DDRD &= ~(1<<DDD6); |
79 | PORTD |= (1<<PORTD6); |
84 | PORTD |= (1<<PORTD6); |
Line 80... | Line 85... | ||
80 | 85 | ||
81 | // Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5) |
86 | // Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5) |
82 | // set as output |
87 | // set as output |
83 | DDRD |= /*(1<<DDD5)|*/(1<<DDD4); |
88 | DDRD |= (1<<DDD5)| (1<<DDD4) | (1<<DDD3); |
84 | // low level |
89 | // low level |
Line 85... | Line 90... | ||
85 | PORTD &= ~(/*(1<<PORTD5)|*/(1<<PORTD4)); |
90 | PORTD &= ~((1<<PORTD5) | (1<<PORTD4) | (1<<PORTD3)); |
86 | 91 | ||
87 | // PD3 can't be used if 2nd UART is activated |
92 | // PD3 can't be used if 2nd UART is activated |
88 | // because TXD1 is at that port |
93 | // because TXD1 is at that port |
Line 165... | Line 170... | ||
165 | } else { // within the PPM frame |
170 | } else { // within the PPM frame |
166 | if(index < MAX_CHANNELS-1) // PPM24 supports 12 channels |
171 | if(index < MAX_CHANNELS-1) // PPM24 supports 12 channels |
167 | { |
172 | { |
168 | // check for valid signal length (0.8 ms < signal < 2.1984 ms) |
173 | // check for valid signal length (0.8 ms < signal < 2.1984 ms) |
169 | // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625 |
174 | // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625 |
170 | if((signal > 250) && (signal < 687)) |
175 | if((signal > 250) && (signal < 687)) { |
171 | { |
- | |
172 | // shift signal to zero symmetric range -154 to 159 |
176 | // shift signal to zero symmetric range -154 to 159 |
173 | signal -= 466; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms) |
177 | signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms) |
174 | // check for stable signal |
178 | // check for stable signal |
175 | if(abs(signal - PPM_in[index]) < 6) { |
179 | if(abs(signal - PPM_in[index]) < 6) { |
176 | if(RC_Quality < 200) RC_Quality +=10; |
180 | if(RC_Quality < 200) RC_Quality +=10; |
177 | else RC_Quality = 200; |
181 | else RC_Quality = 200; |
178 | } |
182 | } |
179 | // calculate exponential history for signal |
183 | // If signal is the same as before +/- 1, just keep it there. |
180 | tmp = (3 * PPM_in[index] + signal) / 4; |
184 | if (signal>=PPM_in[index]-1 && signal<=PPM_in[index]+1) { |
- | 185 | // In addition, if the signal is very close to 0, just set it to 0. |
|
181 | if(tmp > signal + 1) tmp--; else |
186 | if (signal >=-1 && signal <= 1) { |
- | 187 | tmp = 0; |
|
- | 188 | } else { |
|
- | 189 | tmp = PPM_in[index]; |
|
- | 190 | } |
|
- | 191 | } |
|
- | 192 | else |
|
182 | if(tmp < signal-1) tmp++; |
193 | tmp = signal; |
183 | // calculate signal difference on good signal level |
194 | // calculate signal difference on good signal level |
184 | if(RC_Quality >= 195) |
195 | if(RC_Quality >= 195) |
185 | PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction |
196 | PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction |
186 | else PPM_diff[index] = 0; |
197 | else PPM_diff[index] = 0; |
187 | PPM_in[index] = tmp; // update channel value |
198 | PPM_in[index] = tmp; // update channel value |
Line 200... | Line 211... | ||
200 | } |
211 | } |
201 | } |
212 | } |
Line 202... | Line 213... | ||
202 | 213 | ||
203 | #define RCChannel(dimension) PPM_in[staticParams.ChannelAssignment[dimension]] |
214 | #define RCChannel(dimension) PPM_in[staticParams.ChannelAssignment[dimension]] |
- | 215 | #define RCDiff(dimension) PPM_diff[staticParams.ChannelAssignment[dimension]] |
|
- | 216 | #define COMMAND_THRESHOLD 85 |
|
- | 217 | #define COMMAND_CHANNEL_VERTICAL CH_THROTTLE |
|
- | 218 | #define COMMAND_CHANNEL_HORIZONTAL CH_YAW |
|
- | 219 | ||
- | 220 | // Internal. |
|
- | 221 | uint8_t RC_getStickCommand(void) { |
|
- | 222 | if(RCChannel(COMMAND_CHANNEL_VERTICAL) > COMMAND_THRESHOLD) { |
|
- | 223 | // vertical is up |
|
- | 224 | if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD) |
|
- | 225 | return COMMAND_GYROCAL; |
|
- | 226 | if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD) |
|
- | 227 | return COMMAND_ACCCAL; |
|
- | 228 | return COMMAND_NONE; |
|
- | 229 | } else if(RCChannel(COMMAND_CHANNEL_VERTICAL) < -COMMAND_THRESHOLD) { |
|
- | 230 | // vertical is down |
|
- | 231 | if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD) |
|
- | 232 | return COMMAND_STOP; |
|
- | 233 | if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD) |
|
- | 234 | return COMMAND_START; |
|
- | 235 | return COMMAND_NONE; |
|
- | 236 | } |
|
- | 237 | // vertical is around center |
|
- | 238 | return COMMAND_NONE; |
|
Line 204... | Line 239... | ||
204 | #define RCDiff(dimension) PPM_diff[staticParams.ChannelAssignment[dimension]] |
239 | } |
205 | 240 | ||
206 | /* |
241 | /* |
207 | * This must be called (as the only thing) for each control loop cycle (488 Hz). |
242 | * This must be called (as the only thing) for each control loop cycle (488 Hz). |
208 | */ |
243 | */ |
209 | void RC_update() { |
244 | void RC_update() { |
210 | int16_t tmp1, tmp2; |
245 | int16_t tmp1, tmp2; |
211 | if(RC_Quality) { |
246 | if(RC_Quality) { |
212 | RC_Quality--; |
247 | RC_Quality--; |
213 | if (NewPpmData-- == 0) { |
248 | if (NewPpmData-- == 0) { |
214 | RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.StickP - stickOffsetPitch + RCDiff(CH_PITCH) * staticParams.StickD; |
249 | RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.StickP - /* stickOffsetPitch */ + RCDiff(CH_PITCH) * staticParams.StickD; |
215 | RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP - stickOffsetRoll + RCDiff(CH_ROLL) * staticParams.StickD; |
250 | RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP - /* stickOffsetRoll */ + RCDiff(CH_ROLL) * staticParams.StickD; |
216 | RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + PPM_diff[staticParams.ChannelAssignment[CH_THROTTLE]] * dynamicParams.UserParams[3] + 120; |
251 | RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + PPM_diff[staticParams.ChannelAssignment[CH_THROTTLE]] * dynamicParams.UserParams[3] + 120; |
217 | if (RC_PRTY[CONTROL_THROTTLE] < 0) RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
252 | if (RC_PRTY[CONTROL_THROTTLE] < 0) RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
218 | tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
253 | tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
219 | // exponential stick sensitivity in yawring rate |
254 | // exponential stick sensitivity in yawing rate |
220 | tmp2 = (int32_t) staticParams.StickYawP * ((int32_t)tmp1 * abs(tmp1)) / 512L; // expo y = ax + bx^2 |
255 | tmp2 = (int32_t) staticParams.StickYawP * ((int32_t)tmp1 * abs(tmp1)) / 512L; // expo y = ax + bx^2 |
221 | tmp2 += (staticParams.StickYawP * tmp1) / 4; |
256 | tmp2 += (staticParams.StickYawP * tmp1) / 4; |
- | 257 | RC_PRTY[CONTROL_YAW] = tmp2; |
|
- | 258 | } |
|
- | 259 | uint8_t command = RC_getStickCommand(); |
|
- | 260 | if (lastRCCommand == command) { |
|
- | 261 | // Keep timer from overrunning. |
|
- | 262 | if (commandTimer < COMMAND_TIMER) commandTimer++; |
|
- | 263 | } else { |
|
- | 264 | // There was a change. |
|
- | 265 | lastRCCommand = command; |
|
222 | RC_PRTY[CONTROL_YAW] = tmp2; |
266 | commandTimer = 0; |
223 | } |
267 | } |
224 | } else { // Bad signal |
268 | } else { // Bad signal |
225 | RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE] = RC_PRTY[CONTROL_YAW] = 0; |
269 | RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE] = RC_PRTY[CONTROL_YAW] = 0; |
Line 234... | Line 278... | ||
234 | } |
278 | } |
Line 235... | Line 279... | ||
235 | 279 | ||
236 | /* |
280 | /* |
237 | * Get other channel value |
281 | * Get other channel value |
238 | */ |
282 | */ |
239 | int16_t RC_getVariable(uint8_t varNum) { |
283 | int16_t RC_getVariable (uint8_t varNum) { |
240 | if (varNum < 4) |
284 | if (varNum < 4) |
241 | // 0th variable is 5th channel (1-based) etc. |
285 | // 0th variable is 5th channel (1-based) etc. |
242 | return RCChannel(varNum + 5); |
286 | return RCChannel(varNum + 4) + POT_OFFSET; |
243 | /* |
287 | /* |
244 | * Let's just say: |
288 | * Let's just say: |
245 | * The RC variable 4 is hardwired to channel 5 |
289 | * The RC variable 4 is hardwired to channel 5 |
246 | * The RC variable 5 is hardwired to channel 6 |
290 | * The RC variable 5 is hardwired to channel 6 |
247 | * The RC variable 6 is hardwired to channel 7 |
291 | * The RC variable 6 is hardwired to channel 7 |
248 | * The RC variable 7 is hardwired to channel 8 |
292 | * The RC variable 7 is hardwired to channel 8 |
249 | * Alternatively, one could bind them to channel (4 + varNum) - or whatever... |
293 | * Alternatively, one could bind them to channel (4 + varNum) - or whatever... |
250 | */ |
294 | */ |
251 | return PPM_in[varNum + 1]; |
295 | return PPM_in[varNum + 1] + POT_OFFSET; |
Line 252... | Line 296... | ||
252 | } |
296 | } |
253 | 297 | ||
254 | uint8_t RC_getSignalQuality(void) { |
298 | uint8_t RC_getSignalQuality(void) { |
Line 269... | Line 313... | ||
269 | * R617 receiver.) This calibration is not strictly necessary, but |
313 | * R617 receiver.) This calibration is not strictly necessary, but |
270 | * for control logic that depends on the exact (non)center position |
314 | * for control logic that depends on the exact (non)center position |
271 | * of a stick, it may be useful. |
315 | * of a stick, it may be useful. |
272 | */ |
316 | */ |
273 | void RC_calibrate(void) { |
317 | void RC_calibrate(void) { |
- | 318 | // Do nothing. |
|
- | 319 | } |
|
- | 320 | ||
- | 321 | /* |
|
274 | if (staticParams.GlobalConfig & CFG_HEADING_HOLD) { |
322 | if (staticParams.GlobalConfig & CFG_HEADING_HOLD) { |
275 | // In HH, it s OK to trim the R/C. The effect should not be conteracted here. |
323 | // In HH, it s OK to trim the R/C. The effect should not be conteracted here. |
276 | stickOffsetPitch = stickOffsetRoll = 0; |
324 | stickOffsetPitch = stickOffsetRoll = 0; |
277 | } else { |
325 | } else { |
278 | stickOffsetPitch = RCChannel(CH_PITCH) * staticParams.StickP; |
326 | stickOffsetPitch = RCChannel(CH_PITCH) * staticParams.StickP; |
279 | stickOffsetRoll = RCChannel(CH_ROLL) * staticParams.StickP; |
327 | stickOffsetRoll = RCChannel(CH_ROLL) * staticParams.StickP; |
280 | } |
328 | } |
281 | } |
329 | } |
282 | 330 | */ |
|
283 | #define COMMAND_THRESHOLD 85 |
- | |
284 | #define COMMAND_CHANNEL_VERTICAL CH_THROTTLE |
- | |
285 | #define COMMAND_CHANNEL_HORIZONTAL CH_YAW |
- | |
Line 286... | Line 331... | ||
286 | 331 | ||
287 | uint8_t RC_getCommand(void) { |
- | |
288 | if(RCChannel(COMMAND_CHANNEL_VERTICAL) > COMMAND_THRESHOLD) { |
- | |
289 | // vertical is up |
- | |
290 | if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD) |
- | |
291 | return COMMAND_GYROCAL; |
- | |
292 | if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD) |
- | |
293 | return COMMAND_ACCCAL; |
- | |
294 | return COMMAND_NONE; |
- | |
295 | } else if(RCChannel(COMMAND_CHANNEL_VERTICAL) < -COMMAND_THRESHOLD) { |
- | |
296 | // vertical is down |
- | |
297 | if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD) |
- | |
298 | return COMMAND_STOP; |
- | |
299 | if(RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD) |
332 | uint8_t RC_getCommand(void) { |
300 | return COMMAND_START; |
- | |
301 | return COMMAND_NONE; |
- | |
302 | } else { |
333 | if (commandTimer == COMMAND_TIMER) { |
303 | // vertical is around center |
334 | // Stick has been held long enough; command committed. |
304 | return COMMAND_NONE; |
335 | return lastRCCommand; |
- | 336 | } |
|
- | 337 | // Not yet sure what the command is. |
|
305 | } |
338 | return COMMAND_NONE; |
Line 306... | Line 339... | ||
306 | } |
339 | } |
307 | 340 | ||
308 | /* |
341 | /* |
Line 376... | Line 409... | ||
376 | } |
409 | } |
Line 377... | Line 410... | ||
377 | 410 | ||
378 | return looping; |
411 | return looping; |
Line -... | Line 412... | ||
- | 412 | } |
|
- | 413 | ||
- | 414 | uint8_t RC_testCompassCalState(void) { |
|
- | 415 | static uint8_t stick = 1; |
|
- | 416 | // if pitch is centered or top set stick to zero |
|
- | 417 | if(RCChannel(CH_PITCH) > -20) stick = 0; |
|
- | 418 | // if pitch is down trigger to next cal state |
|
- | 419 | if((RCChannel(CH_PITCH) < -70) && !stick) { |
|
- | 420 | stick = 1; |
|
- | 421 | return 1; |
|
- | 422 | } |
|
379 | } |
423 | return 0; |
380 | 424 | } |
|
381 | /* |
425 | /* |
382 | * Abstract controls are not used at the moment. |
426 | * Abstract controls are not used at the moment. |
383 | t_control rc_control = { |
427 | t_control rc_control = { |