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;
}
}
void processFlightCommands(void)
{
uint8_t channel;
uint8_t channelsToRead = 8;
float hdgDelta, simpleX, simpleY;
if ( rcActive == true )
{
// Read receiver commands
if (eepromConfig.receiverType == PPM)
channelsToRead = eepromConfig.ppmChannels;
for (channel = 0; channel < channelsToRead; channel++)
{
if (eepromConfig.receiverType == SPEKTRUM)
rxCommand[channel] = spektrumRead(eepromConfig.rcMap[channel]);
else if (eepromConfig.receiverType == SBUS)
rxCommand[channel] = sBusRead(eepromConfig.rcMap[channel]);
else
rxCommand[channel] = rxRead(eepromConfig.rcMap[channel]);
}
rxCommand[ROLL] -= eepromConfig.midCommand; // Roll Range -1000:1000
rxCommand[PITCH] -= eepromConfig.midCommand; // Pitch Range -1000:1000
rxCommand[YAW] -= eepromConfig.midCommand; // Yaw Range -1000:1000
for (channel = 3; channel < channelsToRead; channel++)
rxCommand[channel] -= eepromConfig.midCommand - MIDCOMMAND; // 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] < eepromConfig.minCheck )
{
// Check for disarm command ( low throttle, left yaw )
if ( (rxCommand[YAW] < (eepromConfig.minCheck - MIDCOMMAND)) && (armed == true) )
{
disarmingTimer++;
if (disarmingTimer > eepromConfig.disarmCount)
{
zeroPIDstates();
armed = false;
disarmingTimer = 0;
}
}
else
{
disarmingTimer = 0;
}
// Check for gyro bias command ( low throttle, left yaw, aft pitch, right roll )
if ( (rxCommand[YAW ] < (eepromConfig.minCheck - MIDCOMMAND)) &&
(rxCommand[ROLL ] > (eepromConfig.maxCheck - MIDCOMMAND)) &&
(rxCommand[PITCH] < (eepromConfig.minCheck - MIDCOMMAND)) )
{
computeMPU6000RTData();
pulseMotors(3);
}
// Check for arm command ( low throttle, right yaw)
if ((rxCommand[YAW] > (eepromConfig.maxCheck - MIDCOMMAND) ) && (armed == false) && (execUp == true))
{
armingTimer++;
if (armingTimer > eepromConfig.armCount)
{
zeroPIDstates();
armed = true;
armingTimer = 0;
}
}
else
{
armingTimer = 0;
}
}
///////////////////////////////////
// Check for armed true and throttle command > minThrottle
if ((armed == true) && (rxCommand[THROTTLE] > eepromConfig.minThrottle))
pidReset = false;
else
pidReset = true;
///////////////////////////////////
// Check yaw in detent and flight mode to determine hdg hold engaged state
if ((commandInDetent[YAW] == true) && (flightMode == ATTITUDE) && (headingHoldEngaged == false))
{
headingHoldEngaged = true;
setPIDstates(HEADING_PID, 0.0f);
setPIDstates(YAW_RATE_PID, 0.0f);
headingReference = heading.mag;
}
if (((commandInDetent[YAW] == false) || (flightMode != ATTITUDE)) && (headingHoldEngaged == true))
{
headingHoldEngaged = false;
}
///////////////////////////////////
// Vertical Mode Command Processing
verticalReferenceCommand = rxCommand[THROTTLE] - eepromConfig.midCommand;
// Set past altitude reference in detent value
previousVertRefCmdInDetent = vertRefCmdInDetent;
// Apply deadband and set detent discrete'
if ((verticalReferenceCommand <= ALT_DEADBAND) && (verticalReferenceCommand >= -ALT_DEADBAND))
{
verticalReferenceCommand = 0;
vertRefCmdInDetent = true;
}
else
{
vertRefCmdInDetent = false;
if (verticalReferenceCommand > 0)
{
verticalReferenceCommand = (verticalReferenceCommand - ALT_DEADBAND) * ALT_DEADBAND_SLOPE;
}
else
{
verticalReferenceCommand = (verticalReferenceCommand + ALT_DEADBAND) * ALT_DEADBAND_SLOPE;
}
}
///////////////////////////////////////////////
// Need to have AUX channels update modes
// based on change, to allow for both external
// remote commanding from serial port and with
// transmitter switches
//
// Conditions ---------------------------------
// A switch can actuate multiple modes
// Mode enables are defined by channel ranges
// A mode is only enabled/disabled when a
// channel range changes, this allows remote
// commands via serial to be sent
///////////////////////////////////////////////
// Search through each AUX channel
int ch;
for (ch=AUX1; ch<LASTCHANNEL; ch++)
{
// Only make update if channel value changed
if (fabs(previousRxCommand[ch] - rxCommand[ch]) > CHANGE_RANGE)
{
// Search through each mode slot
int slot;
for (slot=1; slot < MODE_SLOTS; slot++)
{
// If mode slot uses current rx channel, update if mode is on/off
if (eepromConfig.mode[slot].channel == ch)
{
// Only change the mode state if the rx channels are in range
int chValue = constrain(rxCommand[ch]/2, 1000, 2000);
if ((chValue >= eepromConfig.mode[slot].minChannelValue) && (chValue <= eepromConfig.mode[slot].maxChannelValue))
{
switch(eepromConfig.mode[slot].modeType)
{
case MODE_NONE:
flightMode = ATTITUDE;
verticalModeState = ALT_DISENGAGED_THROTTLE_INACTIVE;
autoNavMode = MODE_NONE;
break;
case MODE_ATTITUDE:
autoNavMode = MODE_NONE;
if (eepromConfig.mode[slot].state)
{
flightMode = ATTITUDE;
setPIDstates(ROLL_ATT_PID, 0.0f);
setPIDstates(PITCH_ATT_PID, 0.0f);
setPIDstates(HEADING_PID, 0.0f);
}
else
{
// if OFF and no other mode set, default to rate mode
flightMode = RATE;
setPIDstates(ROLL_RATE_PID, 0.0f);
setPIDstates(PITCH_RATE_PID, 0.0f);
setPIDstates(YAW_RATE_PID, 0.0f);
}
break;
case MODE_RATE:
autoNavMode = MODE_NONE;
if (eepromConfig.mode[slot].state)
{
flightMode = RATE;
setPIDstates(ROLL_RATE_PID, 0.0f);
setPIDstates(PITCH_RATE_PID, 0.0f);
setPIDstates(YAW_RATE_PID, 0.0f);
}
else
{
// if OFF and no other mode set, default to attitude mode
flightMode = ATTITUDE;
setPIDstates(ROLL_ATT_PID, 0.0f);
setPIDstates(PITCH_ATT_PID, 0.0f);
setPIDstates(HEADING_PID, 0.0f);
}
break;
case MODE_SIMPLE:
autoNavMode = MODE_NONE;
if (eepromConfig.mode[slot].state)
{
flightMode = MODE_SIMPLE;
hdgDelta = sensors.attitude500Hz[YAW] - homeData.magHeading;
hdgDelta = standardRadianFormat(hdgDelta);
simpleX = cosf(hdgDelta) * rxCommand[PITCH] + sinf(hdgDelta) * rxCommand[ROLL ];
simpleY = cosf(hdgDelta) * rxCommand[ROLL ] - sinf(hdgDelta) * rxCommand[PITCH];
rxCommand[ROLL ] = simpleY;
rxCommand[PITCH] = simpleX;
}
else
{
// if OFF and no other mode set, default to attitude mode
flightMode = ATTITUDE;
setPIDstates(ROLL_ATT_PID, 0.0f);
setPIDstates(PITCH_ATT_PID, 0.0f);
setPIDstates(HEADING_PID, 0.0f);
}
break;
case MODE_AUTONAV:
if (eepromConfig.mode[slot].state)
{
flightMode = ATTITUDE;
//verticalModeState = ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT;
autoNavMode = MODE_AUTONAV;
setAutoNavState(AUTONAV_ENABLED);
}
else
{
flightMode = ATTITUDE;
//verticalModeState = ALT_DISENGAGED_THROTTLE_INACTIVE;
autoNavMode = MODE_NONE;
setAutoNavState(AUTONAV_DISABLED);
}
break;
case MODE_POSITIONHOLD:
if (eepromConfig.mode[slot].state)
{
flightMode = ATTITUDE;
//verticalModeState = ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT;
autoNavMode = MODE_POSITIONHOLD;
}
else
{
flightMode = ATTITUDE;
//verticalModeState = ALT_DISENGAGED_THROTTLE_INACTIVE;
autoNavMode = MODE_NONE;
}
break;
case MODE_RETURNTOHOME:
if (eepromConfig.mode[slot].state)
{
flightMode = ATTITUDE;
//verticalModeState = ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT;
autoNavMode = MODE_RETURNTOHOME;
}
else
{
flightMode = ATTITUDE;
//verticalModeState = ALT_DISENGAGED_THROTTLE_INACTIVE;
autoNavMode = MODE_NONE;
}
break;
case MODE_ALTHOLD:
if (eepromConfig.mode[slot].state)
{
if (verticalModeState == ALT_DISENGAGED_THROTTLE_ACTIVE)
{
verticalModeState = ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT;
setPIDstates(HDOT_PID, 0.0f);
setPIDstates(H_PID, 0.0f);
altitudeHoldReference = hEstimate;
throttleReference = rxCommand[THROTTLE];
}
else if (verticalModeState == ALT_DISENGAGED_THROTTLE_INACTIVE)
verticalModeState = ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT;
}
else
if (verticalModeState == VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY)
{
verticalModeState = ALT_DISENGAGED_THROTTLE_INACTIVE;
altitudeHoldReference = hEstimate;
}
else
verticalModeState = ALT_DISENGAGED_THROTTLE_INACTIVE;
break;
case MODE_PANIC:
if (eepromConfig.mode[slot].state)
{
flightMode = ATTITUDE;
verticalModeState = ALT_DISENGAGED_THROTTLE_ACTIVE;
autoNavMode = MODE_PANIC;
}
break;
}
}
}
}
}
previousRxCommand[ch] = rxCommand[ch];
}
///////////////////////////////////
// AutoNavigation State Machine
switch (autoNavMode)
{
case MODE_NONE:
autoNavPitchAxisCorrection = 0.0;
autoNavRollAxisCorrection = 0.0;
autoNavYawAxisCorrection = 0.0;
break;
case MODE_AUTONAV:
processAutoNavigation();
break;
case MODE_POSITIONHOLD:
processPositionHold();
break;
case MODE_RETURNTOHOME:
processReturnToHome();
break;
}
///////////////////////////////////
// Vertical Mode State Machine
switch (verticalModeState)
{
case ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT:
if ((vertRefCmdInDetent == true) || eepromConfig.verticalVelocityHoldOnly)
verticalModeState = ALT_HOLD_AT_REFERENCE_ALTITUDE;
break;
case ALT_DISENGAGED_THROTTLE_ACTIVE:
break;
case ALT_HOLD_AT_REFERENCE_ALTITUDE:
if ((vertRefCmdInDetent == false) || eepromConfig.verticalVelocityHoldOnly)
verticalModeState = VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY;
break;
case VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY:
if ((vertRefCmdInDetent == true) && !eepromConfig.verticalVelocityHoldOnly)
{
verticalModeState = ALT_HOLD_AT_REFERENCE_ALTITUDE;
altitudeHoldReference = hEstimate;
}
break;
case ALT_DISENGAGED_THROTTLE_INACTIVE: // This mode verifies throttle is at center when disengaging alt hold
if (((rxCommand[THROTTLE] < throttleCmd + THROTTLE_WINDOW) && (rxCommand[THROTTLE] > throttleCmd - THROTTLE_WINDOW)) || eepromConfig.verticalVelocityHoldOnly)
verticalModeState = ALT_DISENGAGED_THROTTLE_ACTIVE;
}
}
void processFlightCommands(void)
{
uint8_t channel, subChannel;
float hdgDelta, simpleX, simpleY;
if (rcActive == true)
{
// Read receiver commands
for (channel = 0; channel < 8; channel++)
{
subChannel = channel;
if (channel >= VERTICAL)
subChannel++;
if (eepromConfig.receiverType == SPEKTRUM)
rxCommand[subChannel] = (float)spektrumRead(eepromConfig.rcMap[channel]);
else
rxCommand[subChannel] = (float)ppmRxRead(eepromConfig.rcMap[channel]);
}
rxCommand[ROLL] -= eepromConfig.midCommand; // Roll Range -1000:1000
rxCommand[PITCH] -= eepromConfig.midCommand; // Pitch Range -1000:1000
rxCommand[YAW] -= eepromConfig.midCommand; // Yaw Range -1000:1000
rxCommand[VERTICAL] = rxCommand[THROTTLE] - eepromConfig.midCommand; // Vertical Range -1000:1000
rxCommand[THROTTLE] -= eepromConfig.midCommand - MIDCOMMAND; // Throttle Range 2000:4000
rxCommand[AUX1] -= eepromConfig.midCommand - MIDCOMMAND; // Aux1 Range 2000:4000
rxCommand[AUX2] -= eepromConfig.midCommand - MIDCOMMAND; // Aux2 Range 2000:4000
rxCommand[AUX3] -= eepromConfig.midCommand - MIDCOMMAND; // Aux3 Range 2000:4000
rxCommand[AUX4] -= eepromConfig.midCommand - MIDCOMMAND; // Aux4 Range 2000:4000
}
// Set past command in detent values
for (channel = 0; channel < 4; channel++)
previousCommandInDetent[channel] = commandInDetent[channel];
// Apply deadbands and set detent discretes'
for (channel = 0; channel < 4; 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] < eepromConfig.minCheck )
{
// Check for disarm command ( low throttle, left yaw )
if (((rxCommand[YAW] < (eepromConfig.minCheck - MIDCOMMAND)) && (armed == true)) && (verticalModeState == ALT_DISENGAGED_THROTTLE_ACTIVE))
{
disarmingTimer++;
if (disarmingTimer > eepromConfig.disarmCount)
{
armed = false;
disarmingTimer = 0;
}
}
else
{
disarmingTimer = 0;
}
// Check for gyro bias command ( low throttle, left yaw, aft pitch, right roll )
if ( (rxCommand[YAW ] < (eepromConfig.minCheck - MIDCOMMAND)) &&
(rxCommand[ROLL ] > (eepromConfig.maxCheck - MIDCOMMAND)) &&
(rxCommand[PITCH] < (eepromConfig.minCheck - MIDCOMMAND)) )
{
gyroRTBias[ROLL ] = 0.0f;
gyroRTBias[PITCH] = 0.0f;
gyroRTBias[YAW ] = 0.0f;
gyroRunTimeCalCount = 2520;
pulseMotors(3);
}
// Check for arm command ( low throttle, right yaw)
if ((rxCommand[YAW] > (eepromConfig.maxCheck - MIDCOMMAND) ) && (armed == false) && (systemOperational == true) && (gyroRunTimeCalCount == 0))
{
armingTimer++;
if (armingTimer > eepromConfig.armCount)
{
armed = true;
armingTimer = 0;
}
}
else
{
armingTimer = 0;
}
}
///////////////////////////////////
// Check for armed true and throttle command > minThrottle
if ((armed == true) && (rxCommand[THROTTLE] > eepromConfig.minThrottle))
tasks500Hz_U.resetPIDs = false;
else
tasks500Hz_U.resetPIDs = true;
///////////////////////////////////
// Simple Mode Command Processing
if (rxCommand[AUX3] > MIDCOMMAND)
{
hdgDelta = tasks500Hz_Y.attitudes[YAW] - homeData.magHeading;
hdgDelta = standardRadianFormat(hdgDelta);
simpleX = cosf(hdgDelta) * rxCommand[PITCH] + sinf(hdgDelta) * rxCommand[ROLL ];
simpleY = cosf(hdgDelta) * rxCommand[ROLL ] - sinf(hdgDelta) * rxCommand[PITCH];
rxCommand[ROLL ] = simpleY;
rxCommand[PITCH] = simpleX;
}
///////////////////////////////////
// Check AUX1 for rate, attitude, or GPS mode (3 Position Switch) NOT COMPLETE YET....
if (rxCommand[AUX1] > MIDCOMMAND)
{
flightMode = ATTITUDE;
tasks500Hz_U.rateModes[ROLL ] = OUTER_LOOP_INPUT;
tasks500Hz_U.rateModes[PITCH] = OUTER_LOOP_INPUT;
tasks500Hz_U.attModes[ROLL ] = COMMAND_INPUT;
tasks500Hz_U.attModes[PITCH] = COMMAND_INPUT;
tasks500Hz_U.velModes[XAXIS] = STATE_INPUT;
tasks500Hz_U.velModes[YAXIS] = STATE_INPUT;
tasks500Hz_U.posModes[XAXIS] = STATE_INPUT;
tasks500Hz_U.posModes[YAXIS] = STATE_INPUT;
tasks500Hz_U.attCmds[ROLL ] = rxCommand[ROLL ] * eepromConfig.attitudeScaling;
tasks500Hz_U.attCmds[PITCH] = rxCommand[PITCH] * eepromConfig.attitudeScaling;
}
else if (rxCommand[AUX1] <= MIDCOMMAND)
{
flightMode = RATE;
tasks500Hz_U.rateModes[ROLL ] = COMMAND_INPUT;
tasks500Hz_U.rateModes[PITCH] = COMMAND_INPUT;
tasks500Hz_U.attModes[ROLL ] = STATE_INPUT;
tasks500Hz_U.attModes[PITCH] = STATE_INPUT;
tasks500Hz_U.velModes[XAXIS] = STATE_INPUT;
tasks500Hz_U.velModes[YAXIS] = STATE_INPUT;
tasks500Hz_U.posModes[XAXIS] = STATE_INPUT;
tasks500Hz_U.posModes[YAXIS] = STATE_INPUT;
tasks500Hz_U.rateCmds[ROLL ] = rxCommand[ROLL ] * eepromConfig.rollAndPitchRateScaling;
tasks500Hz_U.rateCmds[PITCH] = rxCommand[PITCH] * eepromConfig.rollAndPitchRateScaling;
}
///////////////////////////////////
// Check yaw in detent and flight mode to determine hdg hold engaged state
if ((commandInDetent[YAW] == true) &&
(flightMode == ATTITUDE) &&
(headingHoldEngaged == false) &&
(abs(tasks500Hz_U.gyro[YAW]) < (5.0f * D2R)))
{
headingHoldEngaged = true;
tasks500Hz_U.rateModes[YAW] = OUTER_LOOP_INPUT;
tasks500Hz_U.attModes[YAW] = COMMAND_INPUT;
tasks500Hz_U.attCmds[YAW] = tasks500Hz_Y.attitudes[YAW];
}
if ((commandInDetent[YAW] == false) || (flightMode != ATTITUDE))
{
headingHoldEngaged = false;
tasks500Hz_U.rateModes[YAW] = COMMAND_INPUT;
tasks500Hz_U.attModes[YAW] = STATE_INPUT;
tasks500Hz_U.rateCmds[YAW] = rxCommand[YAW] * eepromConfig.yawRateScaling;
}
///////////////////////////////////
// Vertical Mode State Machine
switch (verticalModeState)
{
case ALT_DISENGAGED_THROTTLE_ACTIVE:
tasks500Hz_U.velModes[ZAXIS] = STATE_INPUT;
tasks500Hz_U.posModes[ZAXIS] = STATE_INPUT;
tasks500Hz_U.velCmds[ZAXIS] = rxCommand[THROTTLE];
if ((rxCommand[AUX2] > MIDCOMMAND) && (previousAUX2State <= MIDCOMMAND)) // AUX2 Rising edge detection
{
verticalModeState = ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT;
tasks500Hz_U.posCmds[ZAXIS] = tasks500Hz_U.positions[ZAXIS];
}
break;
///////////////////////////////
case ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT:
tasks500Hz_U.velModes[ZAXIS] = OUTER_LOOP_INPUT;
tasks500Hz_U.posModes[ZAXIS] = COMMAND_INPUT;
if ((commandInDetent[VERTICAL] == true) || eepromConfig.verticalVelocityHoldOnly)
verticalModeState = ALT_HOLD_AT_REFERENCE_ALTITUDE;
if ((rxCommand[AUX2] <= MIDCOMMAND) && (previousAUX2State > MIDCOMMAND)) // AUX2 Falling edge detection
verticalModeState = ALT_DISENGAGED_THROTTLE_INACTIVE;
if ((rxCommand[AUX4] > MIDCOMMAND) && (previousAUX4State <= MIDCOMMAND)) // AUX4 Rising edge detection
verticalModeState = ALT_DISENGAGED_THROTTLE_ACTIVE;
break;
///////////////////////////////
case ALT_HOLD_AT_REFERENCE_ALTITUDE:
tasks500Hz_U.velModes[ZAXIS] = OUTER_LOOP_INPUT;
tasks500Hz_U.posModes[ZAXIS] = COMMAND_INPUT;
if ((commandInDetent[VERTICAL] == false) || eepromConfig.verticalVelocityHoldOnly)
verticalModeState = VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY;
if ((rxCommand[AUX2] <= MIDCOMMAND) && (previousAUX2State > MIDCOMMAND)) // AUX2 Falling edge detection
verticalModeState = ALT_DISENGAGED_THROTTLE_INACTIVE;
if ((rxCommand[AUX4] > MIDCOMMAND) && (previousAUX4State <= MIDCOMMAND)) // AUX4 Rising edge detection
verticalModeState = ALT_DISENGAGED_THROTTLE_ACTIVE;
break;
///////////////////////////////
case VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY:
tasks500Hz_U.velModes[ZAXIS] = COMMAND_INPUT;
tasks500Hz_U.posModes[ZAXIS] = STATE_INPUT;
tasks500Hz_U.velCmds[ZAXIS] = rxCommand[VERTICAL] * eepromConfig.hDotScaling;
if ((commandInDetent[VERTICAL] == true) && !eepromConfig.verticalVelocityHoldOnly)
{
verticalModeState = ALT_HOLD_AT_REFERENCE_ALTITUDE;
tasks500Hz_U.posCmds[ZAXIS] = tasks500Hz_U.positions[ZAXIS];
}
if ((rxCommand[AUX2] <= MIDCOMMAND) && (previousAUX2State > MIDCOMMAND)) // AUX2 Falling edge detection
{
verticalModeState = ALT_DISENGAGED_THROTTLE_INACTIVE;
tasks500Hz_U.posCmds[ZAXIS] = tasks500Hz_U.positions[ZAXIS];
}
if ((rxCommand[AUX4] > MIDCOMMAND) && (previousAUX4State <= MIDCOMMAND)) // AUX4 Rising edge detection
verticalModeState = ALT_DISENGAGED_THROTTLE_ACTIVE;
break;
///////////////////////////////
case ALT_DISENGAGED_THROTTLE_INACTIVE:
if (((rxCommand[THROTTLE] < tasks500Hz_Y.axisCmds[VERTICAL] + THROTTLE_WINDOW) && (rxCommand[THROTTLE] > tasks500Hz_Y.axisCmds[VERTICAL] - THROTTLE_WINDOW)) ||
eepromConfig.verticalVelocityHoldOnly)
verticalModeState = ALT_DISENGAGED_THROTTLE_ACTIVE;
if ((rxCommand[AUX2] > MIDCOMMAND) && (previousAUX2State <= MIDCOMMAND)) // AUX2 Rising edge detection
verticalModeState = ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT;
if ((rxCommand[AUX4] > MIDCOMMAND) && (previousAUX4State <= MIDCOMMAND)) // AUX4 Rising edge detection
verticalModeState = ALT_DISENGAGED_THROTTLE_ACTIVE;
break;
}
previousAUX2State = rxCommand[AUX2];
previousAUX4State = rxCommand[AUX4];
///////////////////////////////////
}
void processFlightCommands(void)
{
uint8_t channel;
if ( rcActive == true )
{
// Read receiver commands
for (channel = 0; channel < 8; channel++)
rxCommand[channel] = (float)rxRead(eepromConfig.rcMap[channel]);
rxCommand[ROLL] -= eepromConfig.midCommand; // Roll Range -1000:1000
rxCommand[PITCH] -= eepromConfig.midCommand; // Pitch Range -1000:1000
rxCommand[YAW] -= eepromConfig.midCommand; // Yaw Range -1000:1000
rxCommand[THROTTLE] -= eepromConfig.midCommand - MIDCOMMAND; // Throttle Range 2000:4000
rxCommand[AUX1] -= eepromConfig.midCommand - MIDCOMMAND; // Aux1 Range 2000:4000
rxCommand[AUX2] -= eepromConfig.midCommand - MIDCOMMAND; // Aux2 Range 2000:4000
rxCommand[AUX3] -= eepromConfig.midCommand - MIDCOMMAND; // Aux3 Range 2000:4000
rxCommand[AUX4] -= eepromConfig.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] < eepromConfig.minCheck )
{
// Check for disarm command ( low throttle, left yaw ), will disarm immediately
if ( (rxCommand[YAW] < (eepromConfig.minCheck - MIDCOMMAND)) && (armed == true) )
{
armed = false;
zeroPIDintegralError();
zeroPIDstates();
}
// Check for gyro bias command ( low throttle, left yaw, aft pitch, right roll )
if ( (rxCommand[YAW ] < (eepromConfig.minCheck - MIDCOMMAND)) &&
(rxCommand[ROLL ] > (eepromConfig.maxCheck - MIDCOMMAND)) &&
(rxCommand[PITCH] < (eepromConfig.minCheck - MIDCOMMAND)) )
{
computeMPU6000RTData();
pulseMotors(3);
}
// Check for arm command ( low throttle, right yaw), must be present for 1 sec before arming
if ((rxCommand[YAW] > (eepromConfig.maxCheck - MIDCOMMAND) ) && (armed == false) && (execUp == true))
{
armingTimer++;
if ( armingTimer > 50 )
{
zeroPIDintegralError();
zeroPIDstates();
armed = true;
armingTimer = 0;
}
}
else
{
armingTimer = 0;
}
}
///////////////////////////////////
// Check for armed true and throttle command > minThrottle
if ((armed == true) && (rxCommand[THROTTLE] > eepromConfig.minThrottle))
holdIntegrators = false;
else
holdIntegrators = true;
///////////////////////////////////
// Check AUX1 for rate, attitude, or GPS mode (3 Position Switch) NOT COMPLETE YET....
if ((rxCommand[AUX1] > MIDCOMMAND) && (flightMode == RATE))
{
flightMode = ATTITUDE;
setPIDintegralError(ROLL_ATT_PID, 0.0f);
setPIDintegralError(PITCH_ATT_PID, 0.0f);
setPIDintegralError(HEADING_PID, 0.0f);
setPIDstates(ROLL_ATT_PID, 0.0f);
setPIDstates(PITCH_ATT_PID, 0.0f);
setPIDstates(HEADING_PID, 0.0f);
}
else if ((rxCommand[AUX1] <= MIDCOMMAND) && (flightMode == ATTITUDE))
{
flightMode = RATE;
setPIDintegralError(ROLL_RATE_PID, 0.0f);
setPIDintegralError(PITCH_RATE_PID, 0.0f);
setPIDintegralError(YAW_RATE_PID, 0.0f);
setPIDstates(ROLL_RATE_PID, 0.0f);
setPIDstates(PITCH_RATE_PID, 0.0f);
setPIDstates(YAW_RATE_PID, 0.0f);
}
///////////////////////////////////
if ((commandInDetent[YAW] == true) && (flightMode == ATTITUDE))
headingHoldEngaged = true;
else
headingHoldEngaged = false;
///////////////////////////////////
// Check AUX2 for altitude hold mode (2 Position Switch)
if ((rxCommand[AUX2] > MIDCOMMAND) && (previousAUX2State <= MIDCOMMAND)) // Rising edge detection
{
altitudeHoldState = ENGAGED;
altitudeHoldThrottleValue = rxCommand[THROTTLE];
}
else if ((rxCommand[AUX2] <= MIDCOMMAND) && (previousAUX2State > MIDCOMMAND)) // Falling edge detection
{
altitudeHoldState = DISENGAGED;
}
previousAUX2State = rxCommand[AUX2];
///////////////////////////////////
}
void processFlightCommands(void)
{
uint8_t channel;
if (rcActive == true)
{
// Read receiver commands
for (channel = 0; channel < 8; channel++)
{
if (eepromConfig.receiverType == SPEKTRUM)
rxCommand[channel] = (float)spektrumRead(eepromConfig.rcMap[channel]);
else
rxCommand[channel] = (float)ppmRxRead(eepromConfig.rcMap[channel]);
}
rxCommand[ROLL] -= eepromConfig.midCommand; // Roll Range -1000:1000
rxCommand[PITCH] -= eepromConfig.midCommand; // Pitch Range -1000:1000
rxCommand[YAW] -= eepromConfig.midCommand; // Yaw Range -1000:1000
rxCommand[THROTTLE] -= eepromConfig.midCommand - MIDCOMMAND; // Throttle Range 2000:4000
rxCommand[AUX1] -= eepromConfig.midCommand - MIDCOMMAND; // Aux1 Range 2000:4000
rxCommand[AUX2] -= eepromConfig.midCommand - MIDCOMMAND; // Aux2 Range 2000:4000
rxCommand[AUX3] -= eepromConfig.midCommand - MIDCOMMAND; // Aux3 Range 2000:4000
rxCommand[AUX4] -= eepromConfig.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] < eepromConfig.minCheck )
{
// Check for disarm command ( low throttle, left yaw )
if (((rxCommand[YAW] < (eepromConfig.minCheck - MIDCOMMAND)) && (armed == true)) && (verticalModeState == ALT_DISENGAGED_THROTTLE_ACTIVE))
{
disarmingTimer++;
if (disarmingTimer > eepromConfig.disarmCount)
{
zeroPIDintegralError();
zeroPIDstates();
armed = false;
disarmingTimer = 0;
}
}
else
{
disarmingTimer = 0;
}
// Check for gyro bias command ( low throttle, left yaw, aft pitch, right roll )
if ( (rxCommand[YAW ] < (eepromConfig.minCheck - MIDCOMMAND)) &&
(rxCommand[ROLL ] > (eepromConfig.maxCheck - MIDCOMMAND)) &&
(rxCommand[PITCH] < (eepromConfig.minCheck - MIDCOMMAND)) )
{
computeMPU6000RTData();
pulseMotors(3);
}
// Check for arm command ( low throttle, right yaw)
if ((rxCommand[YAW] > (eepromConfig.maxCheck - MIDCOMMAND) ) && (armed == false) && (execUp == true))
{
armingTimer++;
if (armingTimer > eepromConfig.armCount)
{
zeroPIDintegralError();
zeroPIDstates();
armed = true;
armingTimer = 0;
}
}
else
{
armingTimer = 0;
}
}
///////////////////////////////////
// Check for armed true and throttle command > minThrottle
if ((armed == true) && (rxCommand[THROTTLE] > eepromConfig.minThrottle))
holdIntegrators = false;
else
holdIntegrators = true;
///////////////////////////////////
// Check AUX1 for rate, attitude, or GPS mode (3 Position Switch) NOT COMPLETE YET....
if ((rxCommand[AUX1] > MIDCOMMAND) && (flightMode == RATE))
{
flightMode = ATTITUDE;
setPIDintegralError(ROLL_ATT_PID, 0.0f);
setPIDintegralError(PITCH_ATT_PID, 0.0f);
setPIDintegralError(HEADING_PID, 0.0f);
setPIDstates(ROLL_ATT_PID, 0.0f);
setPIDstates(PITCH_ATT_PID, 0.0f);
setPIDstates(HEADING_PID, 0.0f);
}
else if ((rxCommand[AUX1] <= MIDCOMMAND) && (flightMode == ATTITUDE))
{
flightMode = RATE;
setPIDintegralError(ROLL_RATE_PID, 0.0f);
setPIDintegralError(PITCH_RATE_PID, 0.0f);
setPIDintegralError(YAW_RATE_PID, 0.0f);
setPIDstates(ROLL_RATE_PID, 0.0f);
setPIDstates(PITCH_RATE_PID, 0.0f);
setPIDstates(YAW_RATE_PID, 0.0f);
}
///////////////////////////////////
// Check yaw in detent and flight mode to determine hdg hold engaged state
if ((commandInDetent[YAW] == true) && (flightMode == ATTITUDE) && (headingHoldEngaged == false))
{
headingHoldEngaged = true;
setPIDintegralError(HEADING_PID, 0.0f);
setPIDstates(YAW_RATE_PID, 0.0f);
headingReference = heading.mag;
}
if (((commandInDetent[YAW] == false) || (flightMode != ATTITUDE)) && (headingHoldEngaged == true))
{
headingHoldEngaged = false;
}
///////////////////////////////////
// Vertical Mode Command Processing
verticalReferenceCommand = rxCommand[THROTTLE] - eepromConfig.midCommand;
// Set past altitude reference in detent value
previousVertRefCmdInDetent = vertRefCmdInDetent;
// Apply deadband and set detent discrete'
if ((verticalReferenceCommand <= ALT_DEADBAND) && (verticalReferenceCommand >= -ALT_DEADBAND))
{
verticalReferenceCommand = 0;
vertRefCmdInDetent = true;
}
else
{
vertRefCmdInDetent = false;
if (verticalReferenceCommand > 0)
{
verticalReferenceCommand = (verticalReferenceCommand - ALT_DEADBAND) * ALT_DEADBAND_SLOPE;
}
else
{
verticalReferenceCommand = (verticalReferenceCommand + ALT_DEADBAND) * ALT_DEADBAND_SLOPE;
}
}
///////////////////////////////////
// Vertical Mode State Machine
switch (verticalModeState)
{
case ALT_DISENGAGED_THROTTLE_ACTIVE:
if ((rxCommand[AUX2] > MIDCOMMAND) && (previousAUX2State <= MIDCOMMAND)) // AUX2 Rising edge detection
{
verticalModeState = ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT;
setPIDintegralError(HDOT_PID, 0.0f);
setPIDintegralError(H_PID, 0.0f);
setPIDstates(HDOT_PID, 0.0f);
setPIDstates(H_PID, 0.0f);
altitudeHoldReference = hEstimate;
throttleReference = rxCommand[THROTTLE];
}
break;
///////////////////////////////
case ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT:
if ((vertRefCmdInDetent == true) || eepromConfig.verticalVelocityHoldOnly)
verticalModeState = ALT_HOLD_AT_REFERENCE_ALTITUDE;
if ((rxCommand[AUX2] <= MIDCOMMAND) && (previousAUX2State > MIDCOMMAND)) // AUX2 Falling edge detection
verticalModeState = ALT_DISENGAGED_THROTTLE_INACTIVE;
if ((rxCommand[AUX4] > MIDCOMMAND) && (previousAUX4State <= MIDCOMMAND)) // AUX4 Rising edge detection
verticalModeState = ALT_DISENGAGED_THROTTLE_ACTIVE;
break;
///////////////////////////////
case ALT_HOLD_AT_REFERENCE_ALTITUDE:
if ((vertRefCmdInDetent == false) || eepromConfig.verticalVelocityHoldOnly)
verticalModeState = VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY;
if ((rxCommand[AUX2] <= MIDCOMMAND) && (previousAUX2State > MIDCOMMAND)) // AUX2 Falling edge detection
verticalModeState = ALT_DISENGAGED_THROTTLE_INACTIVE;
if ((rxCommand[AUX4] > MIDCOMMAND) && (previousAUX4State <= MIDCOMMAND)) // AUX4 Rising edge detection
verticalModeState = ALT_DISENGAGED_THROTTLE_ACTIVE;
break;
///////////////////////////////
case VERTICAL_VELOCITY_HOLD_AT_REFERENCE_VELOCITY:
if ((vertRefCmdInDetent == true) && !eepromConfig.verticalVelocityHoldOnly)
{
verticalModeState = ALT_HOLD_AT_REFERENCE_ALTITUDE;
altitudeHoldReference = hEstimate;
}
if ((rxCommand[AUX2] <= MIDCOMMAND) && (previousAUX2State > MIDCOMMAND)) // AUX2 Falling edge detection
{
verticalModeState = ALT_DISENGAGED_THROTTLE_INACTIVE;
altitudeHoldReference = hEstimate;
}
if ((rxCommand[AUX4] > MIDCOMMAND) && (previousAUX4State <= MIDCOMMAND)) // AUX4 Rising edge detection
verticalModeState = ALT_DISENGAGED_THROTTLE_ACTIVE;
break;
///////////////////////////////
case ALT_DISENGAGED_THROTTLE_INACTIVE:
if (((rxCommand[THROTTLE] < throttleCmd + THROTTLE_WINDOW) && (rxCommand[THROTTLE] > throttleCmd - THROTTLE_WINDOW)) ||
eepromConfig.verticalVelocityHoldOnly)
verticalModeState = ALT_DISENGAGED_THROTTLE_ACTIVE;
if ((rxCommand[AUX2] > MIDCOMMAND) && (previousAUX2State <= MIDCOMMAND)) // AUX2 Rising edge detection
verticalModeState = ALT_HOLD_FIXED_AT_ENGAGEMENT_ALT;
if ((rxCommand[AUX4] > MIDCOMMAND) && (previousAUX4State <= MIDCOMMAND)) // AUX4 Rising edge detection
verticalModeState = ALT_DISENGAGED_THROTTLE_ACTIVE;
break;
}
previousAUX2State = rxCommand[AUX2];
previousAUX4State = rxCommand[AUX4];
///////////////////////////////////
}
void updateCommands(void)
{
uint8_t i;
static uint8_t commandDelay;
if(!featureGet(FEATURE_SPEKTRUM) || spektrumFrameComplete())
computeRC();
// Ground Routines
if(rcData[THROTTLE] < cfg.minCheck) {
zeroPIDs(); // Stops integrators from exploding on the ground
if(cfg.auxActivate[OPT_ARM] > 0) {
if(auxOptions[OPT_ARM] && mode.OK_TO_ARM) { // AUX Arming
mode.ARMED = 1;
headfreeReference = stateData.heading;
} else if(mode.ARMED){ // AUX Disarming
mode.ARMED = 0;
}
} else if(rcData[YAW] > cfg.maxCheck && !mode.ARMED) { // Stick Arming
if(commandDelay++ == 20) {
mode.ARMED = 1;
headfreeReference = stateData.heading;
}
} else if(rcData[YAW] < cfg.minCheck && mode.ARMED) { // Stick Disarming
if(commandDelay++ == 20) {
mode.ARMED = 0;
}
} else if(rcData[YAW] < cfg.minCheck && rcData[PITCH] > cfg.minCheck && !mode.ARMED) {
if(commandDelay++ == 20) {
computeGyroRTBias();
//pulseMotors(3);
// GPS Reset
}
} else {
commandDelay = 0;
}
} else if(rcData[THROTTLE] > cfg.maxCheck && !mode.ARMED) {
if(rcData[YAW] > cfg.maxCheck && rcData[PITCH] < cfg.minCheck) {
if(commandDelay++ == 20) {
magCalibration();
}
} else if(rcData[YAW] < cfg.minCheck && rcData[PITCH] < cfg.minCheck) {
if(commandDelay++ == 20) {
accelCalibration();
pulseMotors(3);
}
} else if (rcData[PITCH] > cfg.maxCheck) {
cfg.angleTrim[PITCH] += 0.01;
writeParams();
} else if (rcData[PITCH] < cfg.minCheck) {
cfg.angleTrim[PITCH] -= 0.01;
writeParams();
} else if (rcData[ROLL] > cfg.maxCheck) {
cfg.angleTrim[ROLL] += 0.01;
writeParams();
} else if (rcData[ROLL] < cfg.minCheck) {
cfg.angleTrim[ROLL] -= 0.01;
writeParams();
} else {
commandDelay = 0;
}
}
// Failsafe
if(featureGet(FEATURE_FAILSAFE)) {
if(failsafeCnt > cfg.failsafeOnDelay && mode.ARMED) {
// Stabilise and set Throttle to failsafe level
for(i = 0; i < 3; ++i) {
rcData[i] = cfg.midCommand;
}
rcData[THROTTLE] = cfg.failsafeThrottle;
mode.FAILSAFE = 1;
if(failsafeCnt > cfg.failsafeOffDelay + cfg.failsafeOnDelay) {
// Disarm
mode.ARMED = 0;
// you will have to switch off first to rearm
mode.OK_TO_ARM = 0;
}
if(failsafeCnt > cfg.failsafeOnDelay && !mode.ARMED) {
mode.ARMED = 0;
// you will have to switch off first to rearm
mode.OK_TO_ARM = 0;
}
++failsafeCnt;
} else {
mode.FAILSAFE = 0;
}
}
// Aux Options
uint16_t auxOptionMask = 0;
for(i = 0; i < AUX_CHANNELS; ++i) {
auxOptionMask |= (rcData[AUX1 + i] < cfg.minCheck) << (3 * i) |
(rcData[AUX1 + i] > cfg.minCheck && rcData[i] < cfg.minCheck) << (3 * i + 1) |
(rcData[AUX1 + i] > cfg.maxCheck) << (3 * i + 2);
}
for(i = 0; i < AUX_OPTIONS; ++i) {
auxOptions[i] = (auxOptionMask & cfg.auxActivate[i]) > 0;
}
if(auxOptions[OPT_ARM] == 0) {
mode.OK_TO_ARM = 1;
}
// Passthrough
if(auxOptions[OPT_PASSTHRU]) {
mode.PASSTHRU_MODE = 1;
} else {
mode.PASSTHRU_MODE = 0;
}
// Level - TODO Add failsafe and ACC check
if(auxOptions[OPT_LEVEL] || mode.FAILSAFE) { //
if(!mode.LEVEL_MODE) {
zeroPID(&pids[ROLL_LEVEL_PID]);
zeroPID(&pids[PITCH_LEVEL_PID]);
mode.LEVEL_MODE = 1;
}
} else {
mode.LEVEL_MODE = 0;
}
// Heading
if(auxOptions[OPT_HEADING]) {
if(!mode.HEADING_MODE) {
mode.HEADING_MODE = 1;
headingHold = stateData.heading;
}
} else {
mode.HEADING_MODE = 0;
}
// Headfree
if(auxOptions[OPT_HEADFREE]) {
if(!mode.HEADFREE_MODE) {
mode.HEADFREE_MODE = 1;
headfreeReference = stateData.heading;
}
} else {
mode.HEADFREE_MODE = 0;
}
if(auxOptions[OPT_HEADFREE_REF]) {
headfreeReference = stateData.heading;
}
// GPS GOES HERE
uint8_t axis;
uint16_t tmp, tmp2;
// Apply deadbands, rates and expo
for (axis = 0; axis < 3; axis++) {
lastCommandInDetent[axis] = commandInDetent[axis];
tmp = min(abs(rcData[axis] - cfg.midCommand), 500);
if (tmp > cfg.deadBand[axis]) {
tmp -= cfg.deadBand[axis];
commandInDetent[axis] = false;
} else {
tmp = 0;
commandInDetent[axis] = true;
}
if(axis != 2) { // Roll and Pitch
tmp2 = tmp / 100;
command[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
} else { // Yaw
command[axis] = tmp;
}
if (rcData[axis] < cfg.midCommand)
command[axis] = -command[axis];
}
tmp = constrain(rcData[THROTTLE], cfg.minCheck, 2000);
tmp = (uint32_t) (tmp - cfg.minCheck) * 1000 / (2000 - cfg.minCheck); // [MINCHECK;2000] -> [0;1000]
tmp2 = tmp / 100;
command[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
// This will force a reset
if(fabs(command[THROTTLE] - altitudeThrottleHold) > THROTTLE_HOLD_DEADBAND)
mode.ALTITUDE_MODE = 0;
// Altitude TODO Add barometer check
if(auxOptions[OPT_ALTITUDE]) {
if(!mode.ALTITUDE_MODE) {
mode.ALTITUDE_MODE = 1;
altitudeThrottleHold = command[THROTTLE];
altitudeHold = stateData.altitude;
zeroPID(&pids[ALTITUDE_PID]);
}
} else {
mode.ALTITUDE_MODE = 0;
}
}