/** * Module task */ static void manualControlTask(void *parameters) { ManualControlSettingsData settings; ManualControlCommandData cmd; portTickType lastSysTime; float flightMode = 0; uint8_t disconnected_count = 0; uint8_t connected_count = 0; enum { CONNECTED, DISCONNECTED } connection_state = DISCONNECTED; // Make sure unarmed on power up ManualControlCommandGet(&cmd); cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE; ManualControlCommandSet(&cmd); 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)) { // 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; } // 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); #endif scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n], 0); } // 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]; if (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) cmd.Accessory1 = scaledChannel[settings.Accessory1]; else cmd.Accessory1 = 0; if (settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) cmd.Accessory2 = scaledChannel[settings.Accessory2]; else cmd.Accessory2 = 0; if (settings.Accessory3 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE) cmd.Accessory3 = scaledChannel[settings.Accessory3]; else cmd.Accessory3 = 0; // Note here the code is ass if (flightMode < -FLIGHT_MODE_LIMIT) cmd.FlightMode = settings.FlightModePosition[0]; else if (flightMode > FLIGHT_MODE_LIMIT) cmd.FlightMode = settings.FlightModePosition[2]; else cmd.FlightMode = settings.FlightModePosition[1]; // Update the ManualControlCommand object ManualControlCommandSet(&cmd); // This seems silly to set then get, but the reason is if the GCS is // the control input, the set command will be blocked by the read only // setting and the get command will pull the right values from telemetry } else ManualControlCommandGet(&cmd); /* Under GCS control */ // Implement hysteresis loop on connection status // Must check both Max and Min in case they reversed if (!ManualControlCommandReadOnly(&cmd) && cmd.Channel[settings.Throttle] < settings.ChannelMax[settings.Throttle] - CONNECTION_OFFSET && cmd.Channel[settings.Throttle] < settings.ChannelMin[settings.Throttle] - CONNECTION_OFFSET) { if (disconnected_count++ > 10) { connection_state = DISCONNECTED; connected_count = 0; disconnected_count = 0; } else disconnected_count++; } else { if (connected_count++ > 10) { connection_state = CONNECTED; connected_count = 0; disconnected_count = 0; } else connected_count++; } if (connection_state == DISCONNECTED) { 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 AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); ManualControlCommandSet(&cmd); } else { cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE; AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); ManualControlCommandSet(&cmd); } // // Arming and Disarming mechanism // // Look for state changes and write in newArmState uint8_t newCmdArmed = cmd.Armed; // By default, keep the arming state the same if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) { // In this configuration we always disarm newCmdArmed = MANUALCONTROLCOMMAND_ARMED_FALSE; } else { // In all other cases, we will not change the arm state when disconnected if (connection_state == CONNECTED) { if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) { // In this configuration, we go into armed state as soon as the throttle is low, never disarm if (cmd.Throttle < 0) { newCmdArmed = MANUALCONTROLCOMMAND_ARMED_TRUE; } } else { // When the configuration is not "Always armed" and no "Always disarmed", // the state will not be changed when the throttle is not low if (cmd.Throttle < 0) { static portTickType armedDisarmStart; float armingInputLevel = 0; // Calc channel see assumptions7 switch ( (settings.Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) { case ARMING_CHANNEL_ROLL: armingInputLevel = cmd.Roll; break; case ARMING_CHANNEL_PITCH: armingInputLevel = cmd.Pitch; break; case ARMING_CHANNEL_YAW: armingInputLevel = cmd.Yaw; break; } bool manualArm = false; bool manualDisarm = false; if (connection_state == CONNECTED) { // Should use RC input only if RX is connected if (armingInputLevel <= -0.90) manualArm = true; else if (armingInputLevel >= +0.90) manualDisarm = true; } // Swap arm-disarming see assumptions8 if ((settings.Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) { bool temp = manualArm; manualArm = manualDisarm; manualDisarm = temp; } switch(armState) { case ARM_STATE_DISARMED: newCmdArmed = MANUALCONTROLCOMMAND_ARMED_FALSE; if (manualArm) { if (okToArm()) // only allow arming if it's OK too { armedDisarmStart = lastSysTime; armState = ARM_STATE_ARMING_MANUAL; } } break; case ARM_STATE_ARMING_MANUAL: if (manualArm) { if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS) armState = ARM_STATE_ARMED; } else 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; newCmdArmed = MANUALCONTROLCOMMAND_ARMED_TRUE; 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) if (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) { if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS) armState = ARM_STATE_DISARMED; } else armState = ARM_STATE_ARMED; break; } // End Switch } else { // The throttle is not low, in case we where arming or disarming, abort 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; } } } } } // Update cmd object when needed if (newCmdArmed != cmd.Armed) { cmd.Armed = newCmdArmed; ManualControlCommandSet(&cmd); } // // End of arming/disarming // // Depending on the mode update the Stabilization or Actuator objects switch(PARSE_FLIGHT_MODE(cmd.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; } } }
/** * @brief Handler to interprete Command inputs in regards to arming/disarming * @input: ManualControlCommand, AccessoryDesired * @output: FlightStatus.Arming */ void armHandler(bool newinit) { static ArmState_t armState; if (newinit) { AccessoryDesiredInitialize(); setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); armState = ARM_STATE_DISARMED; } portTickType sysTime = xTaskGetTickCount(); FlightModeSettingsData settings; FlightModeSettingsGet(&settings); ManualControlCommandData cmd; ManualControlCommandGet(&cmd); AccessoryDesiredData acc; bool lowThrottle = cmd.Throttle < 0; bool armSwitch = false; switch (settings.Arming) { case FLIGHTMODESETTINGS_ARMING_ACCESSORY0: AccessoryDesiredInstGet(0, &acc); armSwitch = true; break; case FLIGHTMODESETTINGS_ARMING_ACCESSORY1: AccessoryDesiredInstGet(1, &acc); armSwitch = true; break; case FLIGHTMODESETTINGS_ARMING_ACCESSORY2: AccessoryDesiredInstGet(2, &acc); armSwitch = true; break; default: break; } // immediate disarm on switch if (armSwitch && acc.AccessoryVal <= -ARMED_THRESHOLD) { lowThrottle = true; } if (forcedDisArm()) { // PathPlanner forces explicit disarming due to error condition (crash, impact, fire, ...) setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); return; } if (settings.Arming == FLIGHTMODESETTINGS_ARMING_ALWAYSDISARMED) { // In this configuration we always disarm setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); 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 == FLIGHTMODESETTINGS_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 switch (settings.Arming) { case FLIGHTMODESETTINGS_ARMING_ROLLLEFT: armingInputLevel = 1.0f * cmd.Roll; break; case FLIGHTMODESETTINGS_ARMING_ROLLRIGHT: armingInputLevel = -1.0f * cmd.Roll; break; case FLIGHTMODESETTINGS_ARMING_PITCHFORWARD: armingInputLevel = 1.0f * cmd.Pitch; break; case FLIGHTMODESETTINGS_ARMING_PITCHAFT: armingInputLevel = -1.0f * cmd.Pitch; break; case FLIGHTMODESETTINGS_ARMING_YAWLEFT: armingInputLevel = 1.0f * cmd.Yaw; break; case FLIGHTMODESETTINGS_ARMING_YAWRIGHT: armingInputLevel = -1.0f * cmd.Yaw; break; case FLIGHTMODESETTINGS_ARMING_ACCESSORY0: case FLIGHTMODESETTINGS_ARMING_ACCESSORY1: case FLIGHTMODESETTINGS_ARMING_ACCESSORY2: armingInputLevel = -1.0f * acc.AccessoryVal; 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 = sysTime; armState = ARM_STATE_ARMING_MANUAL; } break; case ARM_STATE_ARMING_MANUAL: setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING); if (manualArm && (timeDifferenceMs(armedDisarmStart, sysTime) > settings.ArmingSequenceTime)) { 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 = sysTime; armState = ARM_STATE_DISARMING_TIMEOUT; setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); break; case ARM_STATE_DISARMING_TIMEOUT: { // we should never reach the disarming timeout if the pathfollower is engaged - reset timeout FlightStatusControlChainData cc; FlightStatusControlChainGet(&cc); if (cc.PathFollower == FLIGHTSTATUS_CONTROLCHAIN_TRUE) { armedDisarmStart = sysTime; } } // We get here when armed while throttle low, even when the arming timeout is not enabled if ((settings.ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, sysTime) > settings.ArmedTimeout)) { armState = ARM_STATE_DISARMED; } // Switch to disarming due to manual control when needed if (manualDisarm) { armedDisarmStart = sysTime; armState = ARM_STATE_DISARMING_MANUAL; } break; case ARM_STATE_DISARMING_MANUAL: // arming switch disarms immediately, if (manualDisarm && (timeDifferenceMs(armedDisarmStart, sysTime) > settings.DisarmingSequenceTime)) { armState = ARM_STATE_DISARMED; } else if (!manualDisarm) { armState = ARM_STATE_ARMED; } break; } // End Switch }