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; 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; 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]; /////////////////////////////////// }