Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1961 → Rev 1962

/branches/dongfang_FC_rewrite/rc.c
80,20 → 80,20
cli();
 
// PPM-signal is connected to the Input Capture Pin (PD6) of timer 1
DDRD &= ~(1 << DDD6);
PORTD |= (1 << PORTD6);
DDRD &= ~(1<<1DDD6);
PORTD |= (1<<1PORTD6);
 
// Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5)
// set as output
DDRD |= (1 << DDD5) | (1 << DDD4) | (1 << DDD3);
DDRD |= (1<<DDD5) | (1<<DDD4) | (1<<DDD3);
// low level
PORTD &= ~((1 << PORTD5) | (1 << PORTD4) | (1 << PORTD3));
PORTD &= ~((1<<PORTD5) | (1<<PORTD4) | (1<<PORTD3));
 
// PD3 can't be used if 2nd UART is activated
// because TXD1 is at that port
if (CPUType != ATMEGA644P) {
DDRD |= (1 << PORTD3);
PORTD &= ~(1 << PORTD3);
DDRD |= (1<<PORTD3);
PORTD &= ~(1<<PORTD3);
}
 
// Timer/Counter1 Control Register A, B, C
116,8 → 116,8
// Enable Input Capture Interrupt (bit: ICIE1=1)
// Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0)
// Enable Overflow Interrupt (bit: TOIE1=0)
TIMSK1 &= ~((1 << OCIE1B) | (1 << OCIE1A) | (1 << TOIE1));
TIMSK1 |= (1 << ICIE1);
TIMSK1 &= ~((1<<OCIE1B) | (1<<OCIE1A) | (1<<TOIE1));
TIMSK1 |= (1<<ICIE1);
 
RC_Quality = 0;
 
135,323 → 135,323
___| |_| |_| |_| |_.............| |________________|
<-----><-------><------><--------> <------> <---
t0 t1 t2 t4 tn t0
 
The PPM-Frame length is 22.5 ms.
Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse.
The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms.
The maximum time delay of two events coding a chanel is ( 1.7 + 0.3) ms = 2 ms.
The minimum duration of all channels at minimum value is 8 * 1 ms = 8 ms.
The maximum duration of all channels at maximum value is 8 * 2 ms = 16 ms.
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 = 0, 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.52 ms < signal < 25.6 ms)
if ((signal > 1100) && (signal < 8000)) {
// if a sync gap happens and there where at least 4 channels decoded before
// then the NewPpmData flag is reset indicating valid data in the PPM_in[] array.
if (index >= 4) {
NewPpmData = 0; // Null means NewData for the first 4 channels
}
// synchronize channel index
index = 1;
} else { // within the PPM frame
if (index < MAX_CHANNELS - 1) { // PPM24 supports 12 channels
// check for valid signal length (0.8 ms < signal < 2.1984 ms)
// signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625
if ((signal > 250) && (signal < 687)) {
// shift signal to zero symmetric range -154 to 159
signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)
// check for stable signal
if (abs(signal - PPM_in[index]) < 6) {
if (RC_Quality < 200)
RC_Quality += 10;
else
RC_Quality = 200;
}
// If signal is the same as before +/- 1, just keep it there.
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;
// calculate signal difference on good signal level
if (RC_Quality >= 195)
PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
else
PPM_diff[index] = 0;
PPM_in[index] = tmp; // update channel value
}
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 RCDiff(dimension) PPM_diff[channelMap.channels[dimension]]
#define COMMAND_THRESHOLD 85
#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
 
// Internal.
uint8_t RC_getStickCommand(void) {
if (RCChannel(COMMAND_CHANNEL_VERTICAL) > COMMAND_THRESHOLD) {
// vertical is up
if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)
return COMMAND_GYROCAL;
if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)
return COMMAND_ACCCAL;
return COMMAND_NONE;
} else if (RCChannel(COMMAND_CHANNEL_VERTICAL) < -COMMAND_THRESHOLD) {
// vertical is down
if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)
return COMMAND_STOP;
if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)
return COMMAND_START;
return COMMAND_NONE;
}
// vertical is around center
return COMMAND_NONE;
}
 
/*
* This must be called (as the only thing) for each control loop cycle (488 Hz).
*/
void RC_update() {
int16_t tmp1, tmp2;
if (RC_Quality) {
RC_Quality--;
if (NewPpmData-- == 0) {
RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP
+ RCDiff(CH_PITCH) * staticParams.stickD;
RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.stickP
+ RCDiff(CH_ROLL) * staticParams.stickD;
RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE)
* staticParams.stickThrottleD + 120;
if (RC_PRTY[CONTROL_THROTTLE] < 0)
RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative.
tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW);
// exponential stick sensitivity in yawing rate
tmp2 = (int32_t) staticParams.stickYawP * ((int32_t) tmp1 * abs(tmp1))
/ 512L; // expo y = ax + bx^2
tmp2 += (staticParams.stickYawP * tmp1) >> 2;
RC_PRTY[CONTROL_YAW] = tmp2;
}
uint8_t command = RC_getStickCommand();
 
if (lastRCCommand == command) {
// Keep timer from overrunning.
if (commandTimer < COMMAND_TIMER)
commandTimer++;
} else {
// There was a change.
lastRCCommand = command;
commandTimer = 0;
}
} else { // Bad signal
RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE]
= RC_PRTY[CONTROL_YAW] = 0;
}
}
 
/*
* Get Pitch, Roll, Throttle, Yaw values
*/
int16_t* RC_getPRTY(void) {
return RC_PRTY;
}
 
/*
* Get other channel value
*/
int16_t RC_getVariable(uint8_t varNum) {
if (varNum < 4)
// 0th variable is 5th channel (1-based) etc.
return RCChannel(varNum + 4) + POT_OFFSET;
/*
* Let's just say:
* The RC variable 4 is hardwired to channel 5
* The RC variable 5 is hardwired to channel 6
* The RC variable 6 is hardwired to channel 7
* The RC variable 7 is hardwired to channel 8
* Alternatively, one could bind them to channel (4 + varNum) - or whatever...
*/
return PPM_in[varNum + 1] + POT_OFFSET;
}
 
uint8_t RC_getSignalQuality(void) {
if (RC_Quality >= 160)
return SIGNAL_GOOD;
if (RC_Quality >= 140)
return SIGNAL_OK;
if (RC_Quality >= 120)
return SIGNAL_BAD;
return SIGNAL_LOST;
}
 
/*
* To should fired only when the right stick is in the center position.
* This will cause the value of pitch and roll stick to be adjusted
* to zero (not just to near zero, as per the assumption in rc.c
* about the rc signal. I had values about 50..70 with a Futaba
* R617 receiver.) This calibration is not strictly necessary, but
* for control logic that depends on the exact (non)center position
* of a stick, it may be useful.
*/
void RC_calibrate(void) {
// Do nothing.
}
 
/*
if (staticParams.GlobalConfig & CFG_HEADING_HOLD) {
// In HH, it s OK to trim the R/C. The effect should not be conteracted here.
stickOffsetPitch = stickOffsetRoll = 0;
} else {
stickOffsetPitch = RCChannel(CH_PITCH) * staticParams.StickP;
stickOffsetRoll = RCChannel(CH_ROLL) * staticParams.StickP;
}
}
*/
 
uint8_t RC_getCommand(void) {
if (commandTimer == COMMAND_TIMER) {
// Stick has been held long enough; command committed.
return lastRCCommand;
}
// Not yet sure what the command is.
return COMMAND_NONE;
}
 
/*
* Command arguments on R/C:
* 2--3--4
* | | +
* 1 0 5 ^ 0
* | | |
* 8--7--6
*
* + <--
* 0
*
* Not in any of these positions: 0
*/
 
#define ARGUMENT_THRESHOLD 70
#define ARGUMENT_CHANNEL_VERTICAL CH_PITCH
#define ARGUMENT_CHANNEL_HORIZONTAL CH_ROLL
 
uint8_t RC_getArgument(void) {
if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) > ARGUMENT_THRESHOLD) {
// vertical is up
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
return 2;
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
return 4;
return 3;
} else if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) < -ARGUMENT_THRESHOLD) {
// vertical is down
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
return 8;
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
return 6;
return 7;
} else {
// vertical is around center
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
return 1;
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
return 5;
return 0;
}
}
 
/*
uint8_t RC_getLooping(uint8_t looping) {
// static uint8_t looping = 0;
 
if (RCChannel(CH_ROLL) > staticParams.LoopThreshold && staticParams.BitConfig
& CFG_LOOP_LEFT) {
looping |= (LOOPING_ROLL_AXIS | LOOPING_LEFT);
} else if ((looping & LOOPING_LEFT) && RCChannel(CH_ROLL)
< staticParams.LoopThreshold - staticParams.LoopHysteresis) {
looping &= (~(LOOPING_ROLL_AXIS | LOOPING_LEFT));
}
 
if (RCChannel(CH_ROLL) < -staticParams.LoopThreshold
&& staticParams.BitConfig & CFG_LOOP_RIGHT) {
looping |= (LOOPING_ROLL_AXIS | LOOPING_RIGHT);
} else if ((looping & LOOPING_RIGHT) && RCChannel(CH_ROLL)
> -staticParams.LoopThreshold - staticParams.LoopHysteresis) {
looping &= (~(LOOPING_ROLL_AXIS | LOOPING_RIGHT));
}
 
if (RCChannel(CH_PITCH) > staticParams.LoopThreshold
&& staticParams.BitConfig & CFG_LOOP_UP) {
looping |= (LOOPING_PITCH_AXIS | LOOPING_UP);
} else if ((looping & LOOPING_UP) && RCChannel(CH_PITCH)
< staticParams.LoopThreshold - staticParams.LoopHysteresis) {
looping &= (~(LOOPING_PITCH_AXIS | LOOPING_UP));
}
 
if (RCChannel(CH_PITCH) < -staticParams.LoopThreshold
&& staticParams.BitConfig & CFG_LOOP_DOWN) {
looping |= (LOOPING_PITCH_AXIS | LOOPING_DOWN);
} else if ((looping & LOOPING_DOWN) && RCChannel(CH_PITCH)
> -staticParams.LoopThreshold - staticParams.LoopHysteresis) {
looping &= (~(LOOPING_PITCH_AXIS | LOOPING_DOWN));
}
 
return looping;
}
*/
 
uint8_t RC_testCompassCalState(void) {
static uint8_t stick = 1;
// if pitch is centered or top set stick to zero
if (RCChannel(CH_PITCH) > -20)
stick = 0;
// if pitch is down trigger to next cal state
if ((RCChannel(CH_PITCH) < -70) && !stick) {
stick = 1;
return 1;
}
return 0;
}
/*
* Abstract controls are not used at the moment.
t_control rc_control = {
RC_getPitch,
RC_getRoll,
RC_getYaw,
RC_getThrottle,
RC_getSignalQuality,
RC_calibrate
};
*/
 
The PPM-Frame length is 22.5 ms.
Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse.
The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms.
The maximum time delay of two events coding a channel is ( 1.7 + 0.3) ms = 2 ms.
The minimum duration of all channels at minimum value is 8 * 1 ms = 8 ms.
The maximum duration of all channels at maximum value is 8 * 2 ms = 16 ms.
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 = 0, 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.52 ms < signal < 25.6 ms)
if ((signal > 1100) && (signal < 8000)) {
// if a sync gap happens and there where at least 4 channels decoded before
// then the NewPpmData flag is reset indicating valid data in the PPM_in[] array.
if (index >= 4) {
NewPpmData = 0; // Null means NewData for the first 4 channels
}
// synchronize channel index
index = 1;
} else { // within the PPM frame
if (index < MAX_CHANNELS - 1) { // PPM24 supports 12 channels
// check for valid signal length (0.8 ms < signal < 2.1984 ms)
// signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625
if ((signal > 250) && (signal < 687)) {
// shift signal to zero symmetric range -154 to 159
signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms)
// check for stable signal
if (abs(signal - PPM_in[index]) < 6) {
if (RC_Quality < 200)
RC_Quality += 10;
else
RC_Quality = 200;
}
// If signal is the same as before +/- 1, just keep it there.
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;
// calculate signal difference on good signal level
if (RC_Quality >= 195)
PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction
else
PPM_diff[index] = 0;
PPM_in[index] = tmp; // update channel value
}
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 RCDiff(dimension) PPM_diff[channelMap.channels[dimension]]
#define COMMAND_THRESHOLD 85
#define COMMAND_CHANNEL_VERTICAL CH_THROTTLE
#define COMMAND_CHANNEL_HORIZONTAL CH_YAW
 
// Internal.
uint8_t RC_getStickCommand(void) {
if (RCChannel(COMMAND_CHANNEL_VERTICAL) > COMMAND_THRESHOLD) {
// vertical is up
if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)
return COMMAND_GYROCAL;
if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)
return COMMAND_ACCCAL;
return COMMAND_NONE;
} else if (RCChannel(COMMAND_CHANNEL_VERTICAL) < -COMMAND_THRESHOLD) {
// vertical is down
if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) > COMMAND_THRESHOLD)
return COMMAND_STOP;
if (RCChannel(COMMAND_CHANNEL_HORIZONTAL) < -COMMAND_THRESHOLD)
return COMMAND_START;
return COMMAND_NONE;
}
// vertical is around center
return COMMAND_NONE;
}
 
/*
* This must be called (as the only thing) for each control loop cycle (488 Hz).
*/
void RC_update() {
int16_t tmp1, tmp2;
if (RC_Quality) {
RC_Quality--;
if (NewPpmData-- == 0) {
RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.stickP
+ RCDiff(CH_PITCH) * staticParams.stickD;
RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.stickP
+ RCDiff(CH_ROLL) * staticParams.stickD;
RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE)
* staticParams.stickThrottleD + 120;
if (RC_PRTY[CONTROL_THROTTLE] < 0)
RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative.
tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW);
// exponential stick sensitivity in yawing rate
tmp2 = (int32_t) staticParams.stickYawP * ((int32_t) tmp1 * abs(tmp1))
/ 512L; // expo y = ax + bx^2
tmp2 += (staticParams.stickYawP * tmp1) >> 2;
RC_PRTY[CONTROL_YAW] = tmp2;
}
uint8_t command = RC_getStickCommand();
 
if (lastRCCommand == command) {
// Keep timer from overrunning.
if (commandTimer < COMMAND_TIMER)
commandTimer++;
} else {
// There was a change.
lastRCCommand = command;
commandTimer = 0;
}
} else { // Bad signal
RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE]
= RC_PRTY[CONTROL_YAW] = 0;
}
}
 
/*
* Get Pitch, Roll, Throttle, Yaw values
*/
int16_t* RC_getPRTY(void) {
return RC_PRTY;
}
 
/*
* Get other channel value
*/
int16_t RC_getVariable(uint8_t varNum) {
if (varNum < 4)
// 0th variable is 5th channel (1-based) etc.
return RCChannel(varNum + 4) + POT_OFFSET;
/*
* Let's just say:
* The RC variable 4 is hardwired to channel 5
* The RC variable 5 is hardwired to channel 6
* The RC variable 6 is hardwired to channel 7
* The RC variable 7 is hardwired to channel 8
* Alternatively, one could bind them to channel (4 + varNum) - or whatever...
*/
return PPM_in[varNum + 1] + POT_OFFSET;
}
 
uint8_t RC_getSignalQuality(void) {
if (RC_Quality >= 160)
return SIGNAL_GOOD;
if (RC_Quality >= 140)
return SIGNAL_OK;
if (RC_Quality >= 120)
return SIGNAL_BAD;
return SIGNAL_LOST;
}
 
/*
* To should fired only when the right stick is in the center position.
* This will cause the value of pitch and roll stick to be adjusted
* to zero (not just to near zero, as per the assumption in rc.c
* about the rc signal. I had values about 50..70 with a Futaba
* R617 receiver.) This calibration is not strictly necessary, but
* for control logic that depends on the exact (non)center position
* of a stick, it may be useful.
*/
void RC_calibrate(void) {
// Do nothing.
}
 
/*
if (staticParams.GlobalConfig & CFG_HEADING_HOLD) {
// In HH, it s OK to trim the R/C. The effect should not be conteracted here.
stickOffsetPitch = stickOffsetRoll = 0;
} else {
stickOffsetPitch = RCChannel(CH_PITCH) * staticParams.StickP;
stickOffsetRoll = RCChannel(CH_ROLL) * staticParams.StickP;
}
}
*/
 
uint8_t RC_getCommand(void) {
if (commandTimer == COMMAND_TIMER) {
// Stick has been held long enough; command committed.
return lastRCCommand;
}
// Not yet sure what the command is.
return COMMAND_NONE;
}
 
/*
* Command arguments on R/C:
* 2--3--4
* | | +
* 1 0 5 ^ 0
* | | |
* 8--7--6
*
* + <--
* 0
*
* Not in any of these positions: 0
*/
 
#define ARGUMENT_THRESHOLD 70
#define ARGUMENT_CHANNEL_VERTICAL CH_PITCH
#define ARGUMENT_CHANNEL_HORIZONTAL CH_ROLL
 
uint8_t RC_getArgument(void) {
if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) > ARGUMENT_THRESHOLD) {
// vertical is up
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
return 2;
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
return 4;
return 3;
} else if (RCChannel(ARGUMENT_CHANNEL_VERTICAL) < -ARGUMENT_THRESHOLD) {
// vertical is down
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
return 8;
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
return 6;
return 7;
} else {
// vertical is around center
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) > ARGUMENT_THRESHOLD)
return 1;
if (RCChannel(ARGUMENT_CHANNEL_HORIZONTAL) < -ARGUMENT_THRESHOLD)
return 5;
return 0;
}
}
 
/*
uint8_t RC_getLooping(uint8_t looping) {
// static uint8_t looping = 0;
 
if (RCChannel(CH_ROLL) > staticParams.LoopThreshold && staticParams.BitConfig
& CFG_LOOP_LEFT) {
looping |= (LOOPING_ROLL_AXIS | LOOPING_LEFT);
} else if ((looping & LOOPING_LEFT) && RCChannel(CH_ROLL)
< staticParams.LoopThreshold - staticParams.LoopHysteresis) {
looping &= (~(LOOPING_ROLL_AXIS | LOOPING_LEFT));
}
 
if (RCChannel(CH_ROLL) < -staticParams.LoopThreshold
&& staticParams.BitConfig & CFG_LOOP_RIGHT) {
looping |= (LOOPING_ROLL_AXIS | LOOPING_RIGHT);
} else if ((looping & LOOPING_RIGHT) && RCChannel(CH_ROLL)
> -staticParams.LoopThreshold - staticParams.LoopHysteresis) {
looping &= (~(LOOPING_ROLL_AXIS | LOOPING_RIGHT));
}
 
if (RCChannel(CH_PITCH) > staticParams.LoopThreshold
&& staticParams.BitConfig & CFG_LOOP_UP) {
looping |= (LOOPING_PITCH_AXIS | LOOPING_UP);
} else if ((looping & LOOPING_UP) && RCChannel(CH_PITCH)
< staticParams.LoopThreshold - staticParams.LoopHysteresis) {
looping &= (~(LOOPING_PITCH_AXIS | LOOPING_UP));
}
 
if (RCChannel(CH_PITCH) < -staticParams.LoopThreshold
&& staticParams.BitConfig & CFG_LOOP_DOWN) {
looping |= (LOOPING_PITCH_AXIS | LOOPING_DOWN);
} else if ((looping & LOOPING_DOWN) && RCChannel(CH_PITCH)
> -staticParams.LoopThreshold - staticParams.LoopHysteresis) {
looping &= (~(LOOPING_PITCH_AXIS | LOOPING_DOWN));
}
 
return looping;
}
*/
 
uint8_t RC_testCompassCalState(void) {
static uint8_t stick = 1;
// if pitch is centered or top set stick to zero
if (RCChannel(CH_PITCH) > -20)
stick = 0;
// if pitch is down trigger to next cal state
if ((RCChannel(CH_PITCH) < -70) && !stick) {
stick = 1;
return 1;
}
return 0;
}
/*
* Abstract controls are not used at the moment.
t_control rc_control = {
RC_getPitch,
RC_getRoll,
RC_getYaw,
RC_getThrottle,
RC_getSignalQuality,
RC_calibrate
};
*/