10,6 → 10,9 |
|
// The channel array is 0-based! |
volatile int16_t PPM_in[MAX_CHANNELS]; |
volatile uint16_t RC_buffer[MAX_CHANNELS]; |
volatile uint8_t inBfrPnt = 0; |
|
volatile uint8_t RCQuality; |
|
uint8_t lastRCCommand = COMMAND_NONE; |
55,6 → 58,21 |
SREG = sreg; |
} |
|
/* |
* This new and much faster interrupt handler should reduce servo jolts. |
*/ |
ISR(TIMER1_CAPT_vect) { |
static uint16_t oldICR1 = 0; |
uint16_t signal = (uint16_t)ICR1 - oldICR1; |
oldICR1 = ICR1; |
//sync gap? (3.5 ms < signal < 25.6 ms) |
if (signal > TIME(3.5)) { |
inBfrPnt = 0; |
} else if (inBfrPnt<MAX_CHANNELS) { |
RC_buffer[inBfrPnt++] = signal; |
} |
} |
|
/********************************************************************/ |
/* Every time a positive edge is detected at PD6 */ |
/********************************************************************/ |
76,66 → 94,26 |
The remaining time of (22.5 - 8 ms) ms = 14.5 ms to (22.5 - 16 ms) ms = 6.5 ms is |
the syncronization gap. |
*/ |
ISR(TIMER1_CAPT_vect) { // typical rate of 1 ms to 2 ms |
int16_t signal, tmp; |
static int16_t index; |
static uint16_t oldICR1 = 0; |
|
// 16bit Input Capture Register ICR1 contains the timer value TCNT1 |
// at the time the edge was detected |
|
// calculate the time delay to the previous event time which is stored in oldICR1 |
// calculatiing the difference of the two uint16_t and converting the result to an int16_t |
// implicit handles a timer overflow 65535 -> 0 the right way. |
signal = (uint16_t) ICR1 - oldICR1; |
oldICR1 = ICR1; |
|
//sync gap? (3.5 ms < signal < 25.6 ms) |
if (signal > TIME(3.5)) { |
index = 0; |
} else { // within the PPM frame |
if (index < MAX_CHANNELS) { // PPM24 supports 12 channels |
// check for valid signal length (0.8 ms < signal < 2.2 ms) |
void RC_process(void) { |
if (RCQuality) RCQuality--; |
for (uint8_t channel=0; channel<MAX_CHANNELS; channel++) { |
uint16_t signal = RC_buffer[channel]; |
if (signal != 0) { |
RC_buffer[channel] = 0; // reset to flag value already used. |
if ((signal >= TIME(0.8)) && (signal < TIME(2.2))) { |
// shift signal to zero symmetric range -154 to 159 |
//signal -= 3750; // theoretical value |
signal -= (TIME(1.5) - 128 + channelMap.trim-128); // best value with my Futaba in zero trim. |
// check for stable signal |
if (abs(signal - PPM_in[index]) < TIME(0.05)) { |
signal -= (TIME(1.5) - 128 + channelMap.HWTrim); |
if (abs(signal - PPM_in[channel]) < TIME(0.05)) { |
// With 7 channels and 50 frames/sec, we get 350 channel values/sec. |
if (RCQuality < 200) |
RCQuality += 10; |
else |
RCQuality = 200; |
RCQuality += 2; |
} |
// If signal is the same as before +/- 1, just keep it there. Naah lets get rid of this slimy sticy stuff. |
// if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) { |
// In addition, if the signal is very close to 0, just set it to 0. |
if (signal >= -1 && signal <= 1) { |
tmp = 0; |
//} else { |
// tmp = PPM_in[index]; |
// } |
} else |
tmp = signal; |
PPM_in[index] = tmp; // update channel value |
PPM_in[channel] = signal; |
} |
index++; // next channel |
// demux sum signal for channels 5 to 7 to J3, J4, J5 |
// TODO: General configurability of this R/C channel forwarding. Or remove it completely - the |
// channels are usually available at the receiver anyway. |
// if(index == 5) J3HIGH; else J3LOW; |
// if(index == 6) J4HIGH; else J4LOW; |
// if(CPUType != ATMEGA644P) // not used as TXD1 |
// { |
// if(index == 7) J5HIGH; else J5LOW; |
// } |
} |
} |
} |
|
#define RCChannel(dimension) PPM_in[channelMap.channels[dimension]] |
#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE |
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW |
|
uint8_t getControlModeSwitch(void) { |
int16_t channel = RCChannel(CH_MODESWITCH); |
160,11 → 138,13 |
int16_t channel = RCChannel(CH_THROTTLE); |
|
if (channel <= -TIME(0.55)) { |
lastRCCommand = COMMAND_GYROCAL; |
debugOut.analog[17] = 1; |
int16_t aux = RCChannel(COMMAND_CHANNEL_HORIZONTAL); |
if (abs(aux) >= TIME(0.3)) // If we pull on the stick, it is gyrocal. Else it is RC cal. |
lastRCCommand = COMMAND_GYROCAL; |
else |
lastRCCommand = COMMAND_RCCAL; |
} else { |
lastRCCommand = COMMAND_NONE; |
debugOut.analog[17] = 0; |
} |
return lastRCCommand; |
} |
177,13 → 157,12 |
* Get Pitch, Roll, Throttle, Yaw values |
*/ |
void RC_periodicTaskAndPRYT(int16_t* PRYT) { |
if (RCQuality) { |
RCQuality--; |
PRYT[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR); |
PRYT[CONTROL_AILERONS] = RCChannel(CH_AILERONS); |
PRYT[CONTROL_RUDDER] = RCChannel(CH_RUDDER); |
PRYT[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE); |
} // if RCQuality is no good, we just do nothing. |
RC_process(); |
|
PRYT[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR) - rcTrim.trim[CH_ELEVATOR]; |
PRYT[CONTROL_AILERONS] = RCChannel(CH_AILERONS) - rcTrim.trim[CH_AILERONS]; |
PRYT[CONTROL_RUDDER] = RCChannel(CH_RUDDER) - rcTrim.trim[CH_RUDDER]; |
PRYT[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE); // no trim on throttle! |
} |
|
/* |
192,12 → 171,12 |
int16_t RC_getVariable(uint8_t varNum) { |
if (varNum < 4) |
// 0th variable is 5th channel (1-based) etc. |
return (RCChannel(varNum + CH_POTS) >> 2) + channelMap.variableOffset; |
return (RCChannel(varNum + CH_POTS) >> 3) + channelMap.variableOffset; |
/* |
* Let's just say: |
* The RC variable i is hardwired to channel i, i>=4 |
*/ |
return (PPM_in[varNum] >> 2) + channelMap.variableOffset; |
return (PPM_in[varNum] >> 3) + channelMap.variableOffset; |
} |
|
uint8_t RC_getSignalQuality(void) { |
211,9 → 190,18 |
} |
|
void RC_calibrate(void) { |
// Do nothing. |
rcTrim.trim[CH_ELEVATOR] = RCChannel(CH_ELEVATOR); |
rcTrim.trim[CH_AILERONS] = RCChannel(CH_AILERONS); |
rcTrim.trim[CH_RUDDER] = RCChannel(CH_RUDDER); |
rcTrim.trim[CH_THROTTLE] = 0; |
} |
|
int16_t RC_getZeroThrottle() { |
int16_t RC_getZeroThrottle(void) { |
return TIME (-0.5); |
} |
|
void RC_setZeroTrim(void) { |
for (uint8_t i=0; i<MAX_CHANNELS; i++) { |
rcTrim.trim[i] = 0; |
} |
} |