Ejemplo n.º 1
0
void zeroPIDstates(void)
{
    uint8_t index;

    for (index = 0; index < NUMBER_OF_PIDS; index++)
         setPIDstates(index, 0.0f);
}
Ejemplo n.º 2
0
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;
	}
}
Ejemplo n.º 3
0
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];


	///////////////////////////////////
}
Ejemplo n.º 4
0
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];

	///////////////////////////////////
}