/// TODO: Be able to tell if the RC has become disconnected during flight
bool check_failsafe(void)
{
for(int8_t i = 0; i<get_param_int(PARAM_RC_NUM_CHANNELS); i++)
{
if(pwmRead(i) < 900 || pwmRead(i) > 2100)
{
if(_armed_state == ARMED || _armed_state == DISARMED)
{
_armed_state = FAILSAFE_DISARMED;
}
// blink LED
static uint8_t count = 0;
if(count > 25)
{
LED1_TOGGLE;
count = 0;
}
count++;
return true;
}
}
// we got a valid RC measurement for all channels
if(_armed_state == FAILSAFE_ARMED || _armed_state == FAILSAFE_DISARMED)
{
// return to appropriate mode
_armed_state = (_armed_state == FAILSAFE_ARMED) ? ARMED : DISARMED;
}
return false;
}
uint16_t pwmReadRawRC(uint8_t chan)
{
uint16_t data;
data = pwmRead(cfg.rcmap[chan]);
if (data < 750 || data > 2250)
data = cfg.midrc;
return data;
}
static void mavlink_send_rc_raw(void)
{
mavlink_msg_rc_channels_send(MAVLINK_COMM_0,
millis(),
0,
pwmRead(0),
pwmRead(1),
pwmRead(2),
pwmRead(3),
pwmRead(4),
pwmRead(5),
pwmRead(6),
pwmRead(7),
0, 0, 0, 0, 0, 0, 0, 0, 0, 8, 0);
}
uint16_t pwmReadRawRC(uint8_t chan)
{
return pwmRead(mcfg.rcmap[chan]);
}
void processFlightCommands(void)
{
uint8_t channel;
if ( rcActive == true )
{
// Read receiver commands
for (channel = 0; channel < 8; channel++)
rxCommand[channel] = (float)pwmRead(systemConfig.rcMap[channel]);
rxCommand[ROLL] -= systemConfig.midCommand; // Roll Range -1000:1000
rxCommand[PITCH] -= systemConfig.midCommand; // Pitch Range -1000:1000
rxCommand[YAW] -= systemConfig.midCommand; // Yaw Range -1000:1000
rxCommand[THROTTLE] -= systemConfig.midCommand - MIDCOMMAND; // Throttle Range 2000:4000
rxCommand[AUX1] -= systemConfig.midCommand - MIDCOMMAND; // Aux1 Range 2000:4000
rxCommand[AUX2] -= systemConfig.midCommand - MIDCOMMAND; // Aux2 Range 2000:4000
rxCommand[AUX3] -= systemConfig.midCommand - MIDCOMMAND; // Aux3 Range 2000:4000
rxCommand[AUX4] -= systemConfig.midCommand - MIDCOMMAND; // Aux4 Range 2000:4000
}
// Set past command in detent values
for (channel = 0; channel < 3; channel++)
previousCommandInDetent[channel] = commandInDetent[channel];
// Apply deadbands and set detent discretes'
for (channel = 0; channel < 3; channel++)
{
if ((rxCommand[channel] <= DEADBAND) && (rxCommand[channel] >= -DEADBAND))
{
rxCommand[channel] = 0;
commandInDetent[channel] = true;
}
else
{
commandInDetent[channel] = false;
if (rxCommand[channel] > 0)
{
rxCommand[channel] = (rxCommand[channel] - DEADBAND) * DEADBAND_SLOPE;
}
else
{
rxCommand[channel] = (rxCommand[channel] + DEADBAND) * DEADBAND_SLOPE;
}
}
}
///////////////////////////////////
// Check for low throttle
if ( rxCommand[THROTTLE] < systemConfig.minCheck )
{
// Check for disarm command ( low throttle, left yaw ), will disarm immediately
if ( (rxCommand[YAW] < (systemConfig.minCheck - MIDCOMMAND)) && (armed == true) )
{
armed = false;
// Zero PID integrators when disarmed
zeroIntegralError();
zeroLastError();
}
// Check for gyro bias command ( low throttle, left yaw, aft pitch, right roll )
if ( (rxCommand[YAW ] < (systemConfig.minCheck - MIDCOMMAND)) &&
(rxCommand[ROLL ] > (systemConfig.maxCheck - MIDCOMMAND)) &&
(rxCommand[PITCH] < (systemConfig.minCheck - MIDCOMMAND)) )
{
computeGyroRTBias();
pulseMotors(3);
}
// Check for arm command ( low throttle, right yaw), must be present for 1 sec before arming
if ( (rxCommand[YAW] > (systemConfig.maxCheck - MIDCOMMAND) ) && (armed == false) )
{
armingTimer++;
if ( armingTimer > 50 )
{
zeroIntegralError();
armed = true;
armingTimer = 0;
}
}
else
{
armingTimer = 0;
}
}
// Check for armed true and throttle command > minThrottle
if ( (armed == true) && (rxCommand[THROTTLE] > systemConfig.minThrottle) )
holdIntegrators = false;
else
holdIntegrators = true;
// Check AUX1 for rate or attitude mode
if ( rxCommand[AUX1] > MIDCOMMAND )
{
flightMode = ATTITUDE;
}
else
{
flightMode = RATE;
}
}
bool check_mode(uint32_t now)
{
static uint32_t prev_time = 0;
static uint32_t time_sticks_have_been_in_arming_position = 0;
// see it has been at least 20 ms
uint32_t dt = now-prev_time;
if (dt < 20000)
{
return false;
}
// if it has, then do stuff
prev_time = now;
// check for failsafe mode
if(check_failsafe())
{
return true;
}
else
{
// check for arming switch
if (get_param_int(PARAM_ARM_STICKS))
{
if (_armed_state == DISARMED)
{
// if left stick is down and to the right
if (pwmRead(get_param_int(PARAM_RC_F_CHANNEL)) < get_param_int(PARAM_RC_F_BOTTOM) + get_param_int(PARAM_ARM_THRESHOLD)
&& pwmRead(get_param_int(PARAM_RC_Z_CHANNEL)) > (get_param_int(PARAM_RC_Z_CENTER) + get_param_int(PARAM_RC_Z_RANGE)/2)
- get_param_int(PARAM_ARM_THRESHOLD))
{
time_sticks_have_been_in_arming_position += dt;
}
else
{
time_sticks_have_been_in_arming_position = 0;
}
if (time_sticks_have_been_in_arming_position > 500000)
{
arm();
time_sticks_have_been_in_arming_position = 0;
}
}
else // _armed_state is ARMED
{
// if left stick is down and to the left
if (pwmRead(get_param_int(PARAM_RC_F_CHANNEL)) < get_param_int(PARAM_RC_F_BOTTOM) +
get_param_int(PARAM_ARM_THRESHOLD)
&& pwmRead(get_param_int(PARAM_RC_Z_CHANNEL)) < (get_param_int(PARAM_RC_Z_CENTER) - get_param_int(PARAM_RC_Z_RANGE)/2)
+ get_param_int(PARAM_ARM_THRESHOLD))
{
time_sticks_have_been_in_arming_position += dt;
}
else
{
time_sticks_have_been_in_arming_position = 0;
}
if (time_sticks_have_been_in_arming_position > 500000)
{
disarm();
time_sticks_have_been_in_arming_position = 0;
}
}
}
else
{
if (rc_switch(get_param_int(PARAM_ARM_CHANNEL)))
{
arm();
}
else
{
disarm();
}
}
}
return true;
}
