/** ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules * @{ * @addtogroup ManualControlModule Manual Control Module * @brief Provide manual control or allow it alter flight mode. * @{ * * Reads in the ManualControlCommand FlightMode setting from receiver then either * pass the settings straght to ActuatorDesired object (manual mode) or to * AttitudeDesired object (stabilized mode) * * @file manualcontrol.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief ManualControl module. Handles safety R/C link and flight mode. * * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ /* * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, but * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. * * You should have received a copy of the GNU General Public License along * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "openpilot.h" #include "manualcontrol.h" #include "manualcontrolsettings.h" #include "stabilizationsettings.h" #include "manualcontrolcommand.h" #include "actuatordesired.h" #include "stabilizationdesired.h" #include "flighttelemetrystats.h" #include "flightstatus.h" #include "accessorydesired.h" // Private constants #if defined(PIOS_MANUAL_STACK_SIZE) #define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE #else #define STACK_SIZE_BYTES 824 #endif #define TASK_PRIORITY (tskIDLE_PRIORITY+4) #define UPDATE_PERIOD_MS 20 #define THROTTLE_FAILSAFE -0.1 #define FLIGHT_MODE_LIMIT 1.0/3.0 #define ARMED_TIME_MS 1000 #define ARMED_THRESHOLD 0.50 //safe band to allow a bit of calibration error or trim offset (in microseconds) #define CONNECTION_OFFSET 150 // Private types typedef enum { ARM_STATE_DISARMED, ARM_STATE_ARMING_MANUAL, ARM_STATE_ARMED, ARM_STATE_DISARMING_MANUAL, ARM_STATE_DISARMING_TIMEOUT } ArmState_t; // Private variables static xTaskHandle taskHandle; static ArmState_t armState; static portTickType lastSysTime; // Private functions static void updateActuatorDesired(ManualControlCommandData * cmd); static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings); static void processFlightMode(ManualControlSettingsData * settings, float flightMode); static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings); static void manualControlTask(void *parameters); static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral); static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time); static bool okToArm(void); static bool validInputRange(int16_t min, int16_t max, uint16_t value); #define assumptions1 ( \ ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ ) #define assumptions3 ( \ ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ ) #define assumptions5 ( \ ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ ) #define ARMING_CHANNEL_ROLL 0 #define ARMING_CHANNEL_PITCH 1 #define ARMING_CHANNEL_YAW 2 #define assumptions7 ( \ ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \ ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \ ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \ ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \ ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) && \ ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) \ ) #define assumptions8 ( \ ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \ ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \ ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \ ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \ ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \ ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) \ ) #define assumptions_flightmode ( \ ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int) FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \ ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \ ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \ ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \ ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int) FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \ ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) \ ) #define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode) /** * Module initialization */ int32_t ManualControlStart() { /* Check the assumptions about uavobject enum's are correct */ if(!assumptions) return -1; // Start main task xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle); PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL); return 0; } /** * Module initialization */ int32_t ManualControlInitialize() { /* Check the assumptions about uavobject enum's are correct */ if(!assumptions) return -1; // Start main task //ManualControlStart(); return 0; } module_initcall(ManualControlInitialize, 0, ManualControlStart, 0, MODULE_EXEC_NOORDER_FLAG); /** * Module task */ static void manualControlTask(void *parameters) { ManualControlSettingsData settings; ManualControlCommandData cmd; FlightStatusData flightStatus; float flightMode = 0; uint8_t disconnected_count = 0; uint8_t connected_count = 0; // For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically // this includes not even registering it if not used AccessoryDesiredCreateInstance(); AccessoryDesiredCreateInstance(); // Make sure unarmed on power up ManualControlCommandGet(&cmd); FlightStatusGet(&flightStatus); flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED; armState = ARM_STATE_DISARMED; // Main task loop lastSysTime = xTaskGetTickCount(); while (1) { float scaledChannel[MANUALCONTROLCOMMAND_CHANNEL_NUMELEM]; // Wait until next update vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS); PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL); // Read settings ManualControlSettingsGet(&settings); if (ManualControlCommandReadOnly(&cmd)) { FlightTelemetryStatsData flightTelemStats; FlightTelemetryStatsGet(&flightTelemStats); if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { /* trying to fly via GCS and lost connection. fall back to transmitter */ UAVObjMetadata metadata; UAVObjGetMetadata(&cmd, &metadata); metadata.access = ACCESS_READWRITE; UAVObjSetMetadata(&cmd, &metadata); } } if (!ManualControlCommandReadOnly(&cmd)) { // Read channel values in us // TODO: settings.InputMode is currently ignored because PIOS will not allow runtime // selection of PWM and PPM. The configuration is currently done at compile time in // the pios_config.h file. for (int n = 0; n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { #if defined(PIOS_INCLUDE_PWM) cmd.Channel[n] = PIOS_PWM_Get(n); #elif defined(PIOS_INCLUDE_PPM) cmd.Channel[n] = PIOS_PPM_Get(n); #elif defined(PIOS_INCLUDE_SPEKTRUM) cmd.Channel[n] = PIOS_SPEKTRUM_Get(n); #elif defined(PIOS_INCLUDE_SBUS) cmd.Channel[n] = PIOS_SBUS_Get(n); #endif scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); } // Check settings, if error raise alarm if (settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE || settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE || settings.Yaw >= MANUALCONTROLSETTINGS_YAW_NONE || settings.Throttle >= MANUALCONTROLSETTINGS_THROTTLE_NONE || settings.FlightMode >= MANUALCONTROLSETTINGS_FLIGHTMODE_NONE) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; ManualControlCommandSet(&cmd); continue; } // decide if we have valid manual input or not bool valid_input_detected = validInputRange(settings.ChannelMin[settings.Throttle], settings.ChannelMax[settings.Throttle], cmd.Channel[settings.Throttle]) && validInputRange(settings.ChannelMin[settings.Roll], settings.ChannelMax[settings.Roll], cmd.Channel[settings.Roll]) && validInputRange(settings.ChannelMin[settings.Yaw], settings.ChannelMax[settings.Yaw], cmd.Channel[settings.Yaw]) && validInputRange(settings.ChannelMin[settings.Pitch], settings.ChannelMax[settings.Pitch], cmd.Channel[settings.Pitch]); // Implement hysteresis loop on connection status if (valid_input_detected && (++connected_count > 10)) { cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE; connected_count = 0; disconnected_count = 0; } else if (!valid_input_detected && (++disconnected_count > 10)) { cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; connected_count = 0; disconnected_count = 0; } if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { cmd.Throttle = -1; // Shut down engine with no control cmd.Roll = 0; cmd.Yaw = 0; cmd.Pitch = 0; //cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning // Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this, // or leave throttle at IDLE speed or above when going into AUTO-failsafe. AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); ManualControlCommandSet(&cmd); } else { AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); // Scale channels to -1 -> +1 range cmd.Roll = scaledChannel[settings.Roll]; cmd.Pitch = scaledChannel[settings.Pitch]; cmd.Yaw = scaledChannel[settings.Yaw]; cmd.Throttle = scaledChannel[settings.Throttle]; flightMode = scaledChannel[settings.FlightMode]; AccessoryDesiredData accessory; // Set Accessory 0 if(settings.Accessory0 != MANUALCONTROLSETTINGS_ACCESSORY0_NONE) { accessory.AccessoryVal = scaledChannel[settings.Accessory0]; if(AccessoryDesiredInstSet(0, &accessory) != 0) AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); } // Set Accessory 1 if(settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) { accessory.AccessoryVal = scaledChannel[settings.Accessory1]; if(AccessoryDesiredInstSet(1, &accessory) != 0) AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); } // Set Accsesory 2 if(settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) { accessory.AccessoryVal = scaledChannel[settings.Accessory2]; if(AccessoryDesiredInstSet(2, &accessory) != 0) AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); } processFlightMode(&settings, flightMode); processArm(&cmd, &settings); // Update cmd object ManualControlCommandSet(&cmd); } } else { ManualControlCommandGet(&cmd); /* Under GCS control */ } FlightStatusGet(&flightStatus); // Depending on the mode update the Stabilization or Actuator objects switch(PARSE_FLIGHT_MODE(flightStatus.FlightMode)) { case FLIGHTMODE_UNDEFINED: // This reflects a bug in the code architecture! AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); break; case FLIGHTMODE_MANUAL: updateActuatorDesired(&cmd); break; case FLIGHTMODE_STABILIZED: updateStabilizationDesired(&cmd, &settings); break; case FLIGHTMODE_GUIDANCE: // TODO: Implement break; } } } static void updateActuatorDesired(ManualControlCommandData * cmd) { ActuatorDesiredData actuator; ActuatorDesiredGet(&actuator); actuator.Roll = cmd->Roll; actuator.Pitch = cmd->Pitch; actuator.Yaw = cmd->Yaw; actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; ActuatorDesiredSet(&actuator); } static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings) { StabilizationDesiredData stabilization; StabilizationDesiredGet(&stabilization); StabilizationSettingsData stabSettings; StabilizationSettingsGet(&stabSettings); uint8_t * stab_settings; FlightStatusData flightStatus; FlightStatusGet(&flightStatus); switch(flightStatus.FlightMode) { case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: stab_settings = settings->Stabilization1Settings; break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: stab_settings = settings->Stabilization2Settings; break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: stab_settings = settings->Stabilization3Settings; break; default: // Major error, this should not occur because only enter this block when one of these is true AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); return; } // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0]; stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1]; stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2]; stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode ; stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? fmod(cmd->Yaw * 180.0, 360) : 0; // this is an invalid mode stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; StabilizationDesiredSet(&stabilization); } /** * Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range. */ static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral) { float valueScaled; // Scale if ((max > min && value >= neutral) || (min > max && value <= neutral)) { if (max != neutral) valueScaled = (float)(value - neutral) / (float)(max - neutral); else valueScaled = 0; } else { if (min != neutral) valueScaled = (float)(value - neutral) / (float)(neutral - min); else valueScaled = 0; } // Bound if (valueScaled > 1.0) valueScaled = 1.0; else if (valueScaled < -1.0) valueScaled = -1.0; return valueScaled; } static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) { if(end_time > start_time) return (end_time - start_time) * portTICK_RATE_MS; return ((((portTICK_RATE_MS) -1) - start_time) + end_time) * portTICK_RATE_MS; } /** * @brief Determine if the aircraft is safe to arm * @returns True if safe to arm, false otherwise */ static bool okToArm(void) { // read alarms SystemAlarmsData alarms; SystemAlarmsGet(&alarms); // Check each alarm for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) { if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) continue; return false; } } return true; } /** * @brief Update the flightStatus object only if value changed. Reduces callbacks * @param[in] val The new value */ static void setArmedIfChanged(uint8_t val) { FlightStatusData flightStatus; FlightStatusGet(&flightStatus); if(flightStatus.Armed != val) { flightStatus.Armed = val; FlightStatusSet(&flightStatus); } } /** * @brief Process the inputs and determine whether to arm or not * @param[out] cmd The structure to set the armed in * @param[in] settings Settings indicating the necessary position */ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings) { bool lowThrottle = cmd->Throttle <= 0; if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) { // In this configuration we always disarm setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); } else { // Not really needed since this function not called when disconnected if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) return; // The throttle is not low, in case we where arming or disarming, abort if (!lowThrottle) { switch(armState) { case ARM_STATE_DISARMING_MANUAL: case ARM_STATE_DISARMING_TIMEOUT: armState = ARM_STATE_ARMED; break; case ARM_STATE_ARMING_MANUAL: armState = ARM_STATE_DISARMED; break; default: // Nothing needs to be done in the other states break; } return; } // The rest of these cases throttle is low if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) { // In this configuration, we go into armed state as soon as the throttle is low, never disarm setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); return; } // When the configuration is not "Always armed" and no "Always disarmed", // the state will not be changed when the throttle is not low static portTickType armedDisarmStart; float armingInputLevel = 0; // Calc channel see assumptions7 int8_t sign = ((settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) ? -1 : 1; switch ( (settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) { case ARMING_CHANNEL_ROLL: armingInputLevel = sign * cmd->Roll; break; case ARMING_CHANNEL_PITCH: armingInputLevel = sign * cmd->Pitch; break; case ARMING_CHANNEL_YAW: armingInputLevel = sign * cmd->Yaw; break; } bool manualArm = false; bool manualDisarm = false; if (armingInputLevel <= -ARMED_THRESHOLD) manualArm = true; else if (armingInputLevel >= +ARMED_THRESHOLD) manualDisarm = true; switch(armState) { case ARM_STATE_DISARMED: setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); // only allow arming if it's OK too if (manualArm && okToArm()) { armedDisarmStart = lastSysTime; armState = ARM_STATE_ARMING_MANUAL; } break; case ARM_STATE_ARMING_MANUAL: setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING); if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) armState = ARM_STATE_ARMED; else if (!manualArm) armState = ARM_STATE_DISARMED; break; case ARM_STATE_ARMED: // When we get here, the throttle is low, // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled armedDisarmStart = lastSysTime; armState = ARM_STATE_DISARMING_TIMEOUT; setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); break; case ARM_STATE_DISARMING_TIMEOUT: // We get here when armed while throttle low, even when the arming timeout is not enabled if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout)) armState = ARM_STATE_DISARMED; // Switch to disarming due to manual control when needed if (manualDisarm) { armedDisarmStart = lastSysTime; armState = ARM_STATE_DISARMING_MANUAL; } break; case ARM_STATE_DISARMING_MANUAL: if (manualDisarm &&(timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) armState = ARM_STATE_DISARMED; else if (!manualDisarm) armState = ARM_STATE_ARMED; break; } // End Switch } } /** * @brief Determine which of three positions the flight mode switch is in and set flight mode accordingly * @param[out] cmd Pointer to the command structure to set the flight mode in * @param[in] settings The settings which indicate which position is which mode * @param[in] flightMode the value of the switch position */ static void processFlightMode(ManualControlSettingsData * settings, float flightMode) { FlightStatusData flightStatus; FlightStatusGet(&flightStatus); uint8_t newMode; // Note here the code is ass if (flightMode < -FLIGHT_MODE_LIMIT) newMode = settings->FlightModePosition[0]; else if (flightMode > FLIGHT_MODE_LIMIT) newMode = settings->FlightModePosition[2]; else newMode = settings->FlightModePosition[1]; if(flightStatus.FlightMode != newMode) { flightStatus.FlightMode = newMode; FlightStatusSet(&flightStatus); } } /** * @brief Determine if the manual input value is within acceptable limits * @returns return TRUE if so, otherwise return FALSE */ bool validInputRange(int16_t min, int16_t max, uint16_t value) { if (min > max) { int16_t tmp = min; min = max; max = tmp; } return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET); } /** * @} * @} */