From b5a27d05e72106ffde439558e6c00af9958c9b49 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 9 Feb 2014 19:33:29 +0100 Subject: [PATCH 01/18] OP-1197 configurable failsafes OP-1219 thrust control OP-1217 separate manualcontrolsettings apart --- flight/libraries/sanitycheck.c | 47 +++--- flight/modules/Actuator/actuator.c | 28 +++- flight/modules/AltitudeHold/altitudehold.c | 28 ++-- flight/modules/Autotune/autotune.c | 8 +- .../fixedwingpathfollower.c | 30 ++-- .../modules/ManualControl/inc/manualcontrol.h | 56 +++---- flight/modules/ManualControl/manualcontrol.c | 147 +++++++++++------- flight/modules/PathPlanner/pathplanner.c | 6 +- flight/modules/Stabilization/stabilization.c | 47 +++--- .../VtolPathFollower/vtolpathfollower.c | 32 ++-- .../boards/coptercontrol/firmware/Makefile | 1 + .../boards/revolution/firmware/UAVObjects.inc | 1 + .../boards/revoproto/firmware/UAVObjects.inc | 1 + .../boards/simposix/firmware/UAVObjects.inc | 1 + .../src/plugins/config/configinputwidget.cpp | 69 ++++---- .../src/plugins/config/configinputwidget.h | 4 + .../src/plugins/config/stabilization.ui | 26 ++-- .../src/plugins/hitl/aerosimrcsimulator.cpp | 2 +- .../src/plugins/hitl/fgsimulator.cpp | 10 +- .../src/plugins/hitl/il2simulator.cpp | 2 +- .../src/plugins/hitl/xplanesimulator.cpp | 2 +- .../opmap/opmap_edit_waypoint_dialog.cpp | 2 +- .../vehicleconfigurationhelper.cpp | 44 +++--- .../src/plugins/uavobjects/uavobjects.pro | 2 + .../uavobjectdefinition/actuatordesired.xml | 2 +- .../altitudeholddesired.xml | 2 +- .../altitudeholdsettings.xml | 6 +- .../fixedwingpathfollowersettings.xml | 6 +- .../flightmodesettings.xml | 66 ++++++++ .../manualcontrolcommand.xml | 1 + .../manualcontrolsettings.xml | 57 +------ .../stabilizationdesired.xml | 3 +- .../stabilizationsettings.xml | 4 +- shared/uavobjectdefinition/systemsettings.xml | 5 + .../vtolpathfollowersettings.xml | 2 +- 35 files changed, 421 insertions(+), 329 deletions(-) create mode 100644 shared/uavobjectdefinition/flightmodesettings.xml diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index 97e355305..04015dfb7 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -34,6 +34,7 @@ // UAVOs #include +#include #include #include #include @@ -85,47 +86,47 @@ int32_t configuration_check() // For each available flight mode position sanity check the available // modes uint8_t num_modes; - uint8_t modes[MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM]; + uint8_t modes[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM]; ManualControlSettingsFlightModeNumberGet(&num_modes); - ManualControlSettingsFlightModePositionGet(modes); + FlightModeSettingsFlightModePositionGet(modes); for (uint32_t i = 0; i < num_modes; i++) { switch (modes[i]) { - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL: + case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL: if (multirotor) { severity = SYSTEMALARMS_ALARM_ERROR; } break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1: + case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1: severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(1, multirotor) : severity; break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2: + case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2: severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(2, multirotor) : severity; break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3: + case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3: severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(3, multirotor) : severity; break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE: + case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE: if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE)) { severity = SYSTEMALARMS_ALARM_ERROR; } break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD: - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO: + case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD: + case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO: if (coptercontrol) { severity = SYSTEMALARMS_ALARM_ERROR; } // TODO: put check equivalent to TASK_MONITOR_IsRunning // here as soon as available for delayed callbacks break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL: + case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL: if (coptercontrol) { severity = SYSTEMALARMS_ALARM_ERROR; } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold severity = SYSTEMALARMS_ALARM_ERROR; } break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD: + case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD: if (coptercontrol) { severity = SYSTEMALARMS_ALARM_ERROR; } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { @@ -133,7 +134,7 @@ int32_t configuration_check() severity = SYSTEMALARMS_ALARM_ERROR; } break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND: + case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND: if (coptercontrol) { severity = SYSTEMALARMS_ALARM_ERROR; } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { @@ -141,7 +142,7 @@ int32_t configuration_check() severity = SYSTEMALARMS_ALARM_ERROR; } break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI: + case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI: if (coptercontrol) { severity = SYSTEMALARMS_ALARM_ERROR; } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { @@ -149,7 +150,7 @@ int32_t configuration_check() severity = SYSTEMALARMS_ALARM_ERROR; } break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER: + case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER: if (coptercontrol) { severity = SYSTEMALARMS_ALARM_ERROR; } else { @@ -163,7 +164,7 @@ int32_t configuration_check() } } break; - case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE: + case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE: if (coptercontrol) { severity = SYSTEMALARMS_ALARM_ERROR; } else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { @@ -201,23 +202,23 @@ int32_t configuration_check() static int32_t check_stabilization_settings(int index, bool multirotor) { // Make sure the modes have identical sizes - if (MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NUMELEM || - MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NUMELEM) { + if (FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM != FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_NUMELEM || + FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM != FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_NUMELEM) { return SYSTEMALARMS_ALARM_ERROR; } - uint8_t modes[MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM]; + uint8_t modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM]; // Get the different axis modes for this switch position switch (index) { case 1: - ManualControlSettingsStabilization1SettingsArrayGet(modes); + FlightModeSettingsStabilization1SettingsArrayGet(modes); break; case 2: - ManualControlSettingsStabilization2SettingsArrayGet(modes); + FlightModeSettingsStabilization2SettingsArrayGet(modes); break; case 3: - ManualControlSettingsStabilization3SettingsArrayGet(modes); + FlightModeSettingsStabilization3SettingsArrayGet(modes); break; default: return SYSTEMALARMS_ALARM_ERROR; @@ -226,14 +227,14 @@ static int32_t check_stabilization_settings(int index, bool multirotor) // For multirotors verify that nothing is set to "none" if (multirotor) { for (uint32_t i = 0; i < NELEMENTS(modes); i++) { - if (modes[i] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE) { + if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE) { return SYSTEMALARMS_ALARM_ERROR; } } } // Warning: This assumes that certain conditions in the XML file are met. That - // MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE has the same numeric value for each channel + // FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE has the same numeric value for each channel // and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_NONE return SYSTEMALARMS_ALARM_OK; diff --git a/flight/modules/Actuator/actuator.c b/flight/modules/Actuator/actuator.c index 782cc8ed1..f2f91f0b2 100644 --- a/flight/modules/Actuator/actuator.c +++ b/flight/modules/Actuator/actuator.c @@ -166,6 +166,9 @@ static void actuatorTask(__attribute__((unused)) void *parameters) ActuatorDesiredData desired; MixerStatusData mixerStatus; FlightStatusData flightStatus; + SystemSettingsThrustControlOptions thrustType; + float throttleDesired; + float collectiveDesired; /* Read initial values of ActuatorSettings */ ActuatorSettingsData actuatorSettings; @@ -220,6 +223,22 @@ static void actuatorTask(__attribute__((unused)) void *parameters) FlightStatusGet(&flightStatus); ActuatorDesiredGet(&desired); ActuatorCommandGet(&command); + SystemSettingsThrustControlGet(&thrustType); + + // read in throttle and collective -demultiplex thrust + switch (thrustType) { + case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE: + throttleDesired = desired.Thrust; + ManualControlCommandCollectiveGet(&collectiveDesired); + break; + case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE: + ManualControlCommandThrottleGet(&throttleDesired); + collectiveDesired = desired.Thrust; + break; + default: + ManualControlCommandThrottleGet(&throttleDesired); + ManualControlCommandCollectiveGet(&collectiveDesired); + } #ifdef DIAG_MIXERSTATUS MixerStatusGet(&mixerStatus); @@ -239,17 +258,17 @@ static void actuatorTask(__attribute__((unused)) void *parameters) AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR); bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED; - bool positiveThrottle = desired.Throttle >= 0.00f; + bool positiveThrottle = throttleDesired >= 0.00f; bool spinWhileArmed = actuatorSettings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE; - float curve1 = MixerCurve(desired.Throttle, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM); + float curve1 = MixerCurve(throttleDesired, mixerSettings.ThrottleCurve1, MIXERSETTINGS_THROTTLECURVE1_NUMELEM); // The source for the secondary curve is selectable float curve2 = 0; AccessoryDesiredData accessory; switch (mixerSettings.Curve2Source) { case MIXERSETTINGS_CURVE2SOURCE_THROTTLE: - curve2 = MixerCurve(desired.Throttle, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); + curve2 = MixerCurve(throttleDesired, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); break; case MIXERSETTINGS_CURVE2SOURCE_ROLL: curve2 = MixerCurve(desired.Roll, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); @@ -262,8 +281,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters) curve2 = MixerCurve(desired.Yaw, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); break; case MIXERSETTINGS_CURVE2SOURCE_COLLECTIVE: - ManualControlCommandCollectiveGet(&curve2); - curve2 = MixerCurve(curve2, mixerSettings.ThrottleCurve2, + curve2 = MixerCurve(collectiveDesired, mixerSettings.ThrottleCurve2, MIXERSETTINGS_THROTTLECURVE2_NUMELEM); break; case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0: diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index bf996d0d1..56b6e1f81 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -114,7 +114,7 @@ MODULE_INITCALL(AltitudeHoldInitialize, AltitudeHoldStart); */ static void altitudeHoldTask(void) { - static float startThrottle = 0.5f; + static float startThrust = 0.5f; // make sure we run only when we are supposed to run FlightStatusData flightStatus; @@ -127,7 +127,7 @@ static void altitudeHoldTask(void) default: pid_zero(&pid0); pid_zero(&pid1); - StabilizationDesiredThrottleGet(&startThrottle); + StabilizationDesiredThrustGet(&startThrust); DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); return; @@ -160,30 +160,30 @@ static void altitudeHoldTask(void) AltitudeHoldStatusSet(&altitudeHoldStatus); - float throttle; + float thrust; switch (altitudeHoldDesired.ControlMode) { - case ALTITUDEHOLDDESIRED_CONTROLMODE_THROTTLE: - throttle = altitudeHoldDesired.SetPoint; + case ALTITUDEHOLDDESIRED_CONTROLMODE_THRUST: + thrust = altitudeHoldDesired.SetPoint; break; default: // velocity control loop - throttle = startThrottle - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); + thrust = startThrust - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); - if (throttle >= 1.0f) { - throttle = 1.0f; + if (thrust >= 1.0f) { + thrust = 1.0f; } - if (throttle <= 0.0f) { - throttle = 0.0f; + if (thrust <= 0.0f) { + thrust = 0.0f; } break; } StabilizationDesiredData stab; StabilizationDesiredGet(&stab); - stab.Roll = altitudeHoldDesired.Roll; - stab.Pitch = altitudeHoldDesired.Pitch; - stab.Yaw = altitudeHoldDesired.Yaw; - stab.Throttle = throttle; + stab.Roll = altitudeHoldDesired.Roll; + stab.Pitch = altitudeHoldDesired.Pitch; + stab.Yaw = altitudeHoldDesired.Yaw; + stab.Thrust = thrust; stab.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stab.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stab.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; diff --git a/flight/modules/Autotune/autotune.c b/flight/modules/Autotune/autotune.c index 246180409..a4c5af745 100644 --- a/flight/modules/Autotune/autotune.c +++ b/flight/modules/Autotune/autotune.c @@ -182,8 +182,8 @@ static void AutotuneTask(__attribute__((unused)) void *parameters) } stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; - stabDesired.Yaw = manualControl.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; - stabDesired.Throttle = manualControl.Throttle; + stabDesired.Yaw = manualControl.Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; + stabDesired.Thrust = manualControl.Thrust; switch (state) { case AT_INIT: @@ -191,7 +191,7 @@ static void AutotuneTask(__attribute__((unused)) void *parameters) lastUpdateTime = xTaskGetTickCount(); // Only start when armed and flying - if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && stabDesired.Throttle > 0) { + if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED && stabDesired.Thrust > 0) { state = AT_START; } break; @@ -236,7 +236,7 @@ static void AutotuneTask(__attribute__((unused)) void *parameters) case AT_FINISHED: // Wait until disarmed and landed before updating the settings - if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && stabDesired.Throttle <= 0) { + if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED && stabDesired.Thrust <= 0) { state = AT_SET; } diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c index 5858bf5e0..604d09698 100644 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -357,10 +357,10 @@ static void updateFixedAttitude(float *attitude) StabilizationDesiredData stabDesired; StabilizationDesiredGet(&stabDesired); - stabDesired.Roll = attitude[0]; - stabDesired.Pitch = attitude[1]; - stabDesired.Yaw = attitude[2]; - stabDesired.Throttle = attitude[3]; + stabDesired.Roll = attitude[0]; + stabDesired.Pitch = attitude[1]; + stabDesired.Yaw = attitude[2]; + stabDesired.Thrust = attitude[3]; stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; @@ -420,7 +420,7 @@ static uint8_t updateFixedDesiredAttitude() /** - * Compute speed error (required for throttle and pitch) + * Compute speed error (required for thrust and pitch) */ // Current ground speed @@ -474,9 +474,9 @@ static uint8_t updateFixedDesiredAttitude() } /** - * Compute desired throttle command + * Compute desired thrust command */ - // compute saturated integral error throttle response. Make integral leaky for better performance. Approximately 30s time constant. + // compute saturated integral error thrust response. Make integral leaky for better performance. Approximately 30s time constant. if (fixedwingpathfollowerSettings.PowerPI.Ki > 0) { powerIntegral = bound(powerIntegral + -descentspeedError * dT, -fixedwingpathfollowerSettings.PowerPI.ILimit / fixedwingpathfollowerSettings.PowerPI.Ki, @@ -491,7 +491,7 @@ static uint8_t updateFixedDesiredAttitude() fixedwingpathfollowerSettings.AirspeedToPowerCrossFeed.Max ); - // Compute final throttle response + // Compute final thrust response powerCommand = -descentspeedError * fixedwingpathfollowerSettings.PowerPI.Kp + powerIntegral * fixedwingpathfollowerSettings.PowerPI.Ki + speedErrorToPowerCommandComponent; @@ -501,14 +501,14 @@ static uint8_t updateFixedDesiredAttitude() fixedwingpathfollowerStatus.ErrorInt.Power = powerIntegral; fixedwingpathfollowerStatus.Command.Power = powerCommand; - // set throttle - stabDesired.Throttle = bound(fixedwingpathfollowerSettings.ThrottleLimit.Neutral + powerCommand, - fixedwingpathfollowerSettings.ThrottleLimit.Min, - fixedwingpathfollowerSettings.ThrottleLimit.Max); + // set thrust + stabDesired.Thrust = bound(fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand, + fixedwingpathfollowerSettings.ThrustLimit.Min, + fixedwingpathfollowerSettings.ThrustLimit.Max); // Error condition: plane cannot hold altitude at current speed. fixedwingpathfollowerStatus.Errors.Lowpower = 0; - if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit.Max && // throttle at maximum + if (powerCommand >= fixedwingpathfollowerSettings.ThrustLimit.Max && // thrust at maximum velocityState.Down > 0 && // we ARE going down descentspeedDesired < 0 && // we WANT to go up airspeedError > 0 && // we are too slow already @@ -516,9 +516,9 @@ static uint8_t updateFixedDesiredAttitude() fixedwingpathfollowerStatus.Errors.Lowpower = 1; result = 0; } - // Error condition: plane keeps climbing despite minimum throttle (opposite of above) + // Error condition: plane keeps climbing despite minimum thrust (opposite of above) fixedwingpathfollowerStatus.Errors.Highpower = 0; - if (powerCommand >= fixedwingpathfollowerSettings.ThrottleLimit.Min && // throttle at minimum + if (powerCommand >= fixedwingpathfollowerSettings.ThrustLimit.Min && // thrust at minimum velocityState.Down < 0 && // we ARE going up descentspeedDesired > 0 && // we WANT to go down airspeedError < 0 && // we are too fast already diff --git a/flight/modules/ManualControl/inc/manualcontrol.h b/flight/modules/ManualControl/inc/manualcontrol.h index 09b819a01..ef35f4d26 100644 --- a/flight/modules/ManualControl/inc/manualcontrol.h +++ b/flight/modules/ManualControl/inc/manualcontrol.h @@ -61,46 +61,46 @@ int32_t ManualControlInitialize(); */ #define assumptions1 \ ( \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ + ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ + ((int)FLIGHTMODESETTINGS_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_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ + ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION2SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ + ((int)FLIGHTMODESETTINGS_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_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ + ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_AXISLOCK == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_WEAKLEVELING == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) && \ + ((int)FLIGHTMODESETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ ) #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_ALTITUDEHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) && \ - ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO) && \ - ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int)FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \ - ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) && \ - ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER == (int)FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && \ - ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE == (int)FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) && \ - ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND == (int)FLIGHTSTATUS_FLIGHTMODE_LAND) && \ - ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE == (int)FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) && \ - ((int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI == (int)FLIGHTSTATUS_FLIGHTMODE_POI) \ + ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL == (int)FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \ + ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \ + ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \ + ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int)FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \ + ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) && \ + ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO == (int)FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO) && \ + ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int)FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \ + ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int)FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) && \ + ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER == (int)FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) && \ + ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE == (int)FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) && \ + ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND == (int)FLIGHTSTATUS_FLIGHTMODE_LAND) && \ + ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE == (int)FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) && \ + ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI == (int)FLIGHTSTATUS_FLIGHTMODE_POI) \ ) #define assumptions_channelcount \ diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index ea1c41ff6..940e5cf5c 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -44,6 +44,7 @@ #include "manualcontrol.h" #include "manualcontrolsettings.h" #include "manualcontrolcommand.h" +#include "flightmodesettings.h" #include "positionstate.h" #include "pathdesired.h" #include "stabilizationbank.h" @@ -92,12 +93,12 @@ static float inputFiltered[MANUALCONTROLSETTINGS_RESPONSETIME_NUMELEM]; // Private functions static void updateActuatorDesired(ManualControlCommandData *cmd); -static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualControlSettingsData *settings); +static void updateStabilizationDesired(ManualControlCommandData *cmd, FlightModeSettingsData *settings); static void updateLandDesired(ManualControlCommandData *cmd, bool changed); static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed); static void updatePathDesired(ManualControlCommandData *cmd, bool changed, bool home); -static void processFlightMode(ManualControlSettingsData *settings, float flightMode, ManualControlCommandData *cmd); -static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData *settings, int8_t armSwitch); +static void processFlightMode(ManualControlSettingsData *settings, FlightModeSettingsData *modeSettings, float flightMode, ManualControlCommandData *cmd); +static void processArm(ManualControlCommandData *cmd, FlightModeSettingsData *settings, int8_t armSwitch); static void setArmedIfChanged(uint8_t val); static void configurationUpdatedCb(UAVObjEvent *ev); @@ -157,6 +158,7 @@ int32_t ManualControlInitialize() StabilizationDesiredInitialize(); ReceiverActivityInitialize(); ManualControlSettingsInitialize(); + FlightModeSettingsInitialize(); return 0; } @@ -168,6 +170,7 @@ MODULE_INITCALL(ManualControlInitialize, ManualControlStart); static void manualControlTask(__attribute__((unused)) void *parameters) { ManualControlSettingsData settings; + FlightModeSettingsData modeSettings; ManualControlCommandData cmd; FlightStatusData flightStatus; float flightMode = 0; @@ -203,6 +206,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters) lastSysTime = xTaskGetTickCount(); float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM] = { 0 }; + SystemSettingsThrustControlOptions thrustType; while (1) { // Wait until next update @@ -213,6 +217,8 @@ static void manualControlTask(__attribute__((unused)) void *parameters) // Read settings ManualControlSettingsGet(&settings); + FlightModeSettingsGet(&modeSettings); + SystemSettingsThrustControlGet(&thrustType); /* Update channel activity monitor */ if (flightStatus.Armed == ARM_STATE_DISARMED) { @@ -283,7 +289,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters) || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_NODRIVER || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_NODRIVER || // Check the FlightModeNumber is valid - settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM || + settings.FlightModeNumber < 1 || settings.FlightModeNumber > FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM || // Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care ((settings.FlightModeNumber > 1) && (settings.ChannelGroups.FlightMode >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE @@ -323,16 +329,26 @@ static void manualControlTask(__attribute__((unused)) void *parameters) int8_t armSwitch = 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.Collective = 0; - if (settings.FailsafeBehavior != MANUALCONTROLSETTINGS_FAILSAFEBEHAVIOR_NONE) { + cmd.Throttle = settings.FailsafeChannel.Throttle; + cmd.Roll = settings.FailsafeChannel.Roll; + cmd.Pitch = settings.FailsafeChannel.Pitch; + cmd.Yaw = settings.FailsafeChannel.Yaw; + cmd.Collective = settings.FailsafeChannel.Collective; + switch (thrustType) { + case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE: + cmd.Thrust = cmd.Throttle; + break; + case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE: + cmd.Thrust = cmd.Collective; + break; + default: + break; + } + if (settings.FailsafeFlightModeSwitchPosition >= 0 && settings.FailsafeFlightModeSwitchPosition < settings.FlightModeNumber) { FlightStatusGet(&flightStatus); - cmd.FlightModeSwitchPosition = (uint8_t)settings.FailsafeBehavior - 1; - flightStatus.FlightMode = settings.FlightModePosition[settings.FailsafeBehavior - 1]; + cmd.FlightModeSwitchPosition = (uint8_t)settings.FailsafeFlightModeSwitchPosition; + flightStatus.FlightMode = modeSettings.FlightModePosition[settings.FailsafeFlightModeSwitchPosition]; FlightStatusSet(&flightStatus); } AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); @@ -340,21 +356,21 @@ static void manualControlTask(__attribute__((unused)) void *parameters) AccessoryDesiredData accessory; // Set Accessory 0 if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - accessory.AccessoryVal = 0; + accessory.AccessoryVal = settings.FailsafeChannel.Accessory0; if (AccessoryDesiredInstSet(0, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); } } // Set Accessory 1 if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - accessory.AccessoryVal = 0; + accessory.AccessoryVal = settings.FailsafeChannel.Accessory1; if (AccessoryDesiredInstSet(1, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); } } // Set Accessory 2 if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - accessory.AccessoryVal = 0; + accessory.AccessoryVal = settings.FailsafeChannel.Accessory2; if (AccessoryDesiredInstSet(2, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); } @@ -393,6 +409,17 @@ static void manualControlTask(__attribute__((unused)) void *parameters) cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE]; } + switch (thrustType) { + case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE: + cmd.Thrust = cmd.Throttle; + break; + case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE: + cmd.Thrust = cmd.Collective; + break; + default: + break; + } + AccessoryDesiredData accessory; // Set Accessory 0 if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { @@ -400,7 +427,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters) #ifdef USE_INPUT_LPF applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT); #endif - if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ACCESSORY0) { + if (modeSettings.Arming == FLIGHTMODESETTINGS_ARMING_ACCESSORY0) { if (accessory.AccessoryVal > ARMED_THRESHOLD) { armSwitch = 1; } else if (accessory.AccessoryVal < -ARMED_THRESHOLD) { @@ -417,7 +444,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters) #ifdef USE_INPUT_LPF applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT); #endif - if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ACCESSORY1) { + if (modeSettings.Arming == FLIGHTMODESETTINGS_ARMING_ACCESSORY1) { if (accessory.AccessoryVal > ARMED_THRESHOLD) { armSwitch = 1; } else if (accessory.AccessoryVal < -ARMED_THRESHOLD) { @@ -434,7 +461,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters) #ifdef USE_INPUT_LPF applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT); #endif - if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ACCESSORY2) { + if (modeSettings.Arming == FLIGHTMODESETTINGS_ARMING_ACCESSORY2) { if (accessory.AccessoryVal > ARMED_THRESHOLD) { armSwitch = 1; } else if (accessory.AccessoryVal < -ARMED_THRESHOLD) { @@ -447,11 +474,11 @@ static void manualControlTask(__attribute__((unused)) void *parameters) } } - processFlightMode(&settings, flightMode, &cmd); + processFlightMode(&settings, &modeSettings, flightMode, &cmd); } // Process arming outside conditional so system will disarm when disconnected - processArm(&cmd, &settings, armSwitch); + processArm(&cmd, &modeSettings, armSwitch); // Update cmd object ManualControlCommandSet(&cmd); @@ -481,7 +508,7 @@ static void manualControlTask(__attribute__((unused)) void *parameters) updateActuatorDesired(&cmd); break; case FLIGHTMODE_STABILIZED: - updateStabilizationDesired(&cmd, &settings); + updateStabilizationDesired(&cmd, &modeSettings); break; case FLIGHTMODE_TUNING: // Tuning takes settings directly from manualcontrolcommand. No need to @@ -656,14 +683,14 @@ 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; + actuator.Roll = cmd->Roll; + actuator.Pitch = cmd->Pitch; + actuator.Yaw = cmd->Yaw; + actuator.Thrust = cmd->Thrust; ActuatorDesiredSet(&actuator); } -static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualControlSettingsData *settings) +static void updateStabilizationDesired(ManualControlCommandData *cmd, FlightModeSettingsData *settings) { StabilizationDesiredData stabilization; @@ -738,7 +765,7 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont 0; // this is an invalid mode } - stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; + stabilization.Thrust = cmd->Thrust; StabilizationDesiredSet(&stabilization); } @@ -761,8 +788,8 @@ static void updatePathDesired(__attribute__((unused)) ManualControlCommandData * // Simple Return To Base mode - keep altitude the same, fly to home position PositionStateData positionState; PositionStateGet(&positionState); - ManualControlSettingsData settings; - ManualControlSettingsGet(&settings); + FlightModeSettingsData settings; + FlightModeSettingsGet(&settings); PathDesiredData pathDesired; PathDesiredGet(&pathDesired); @@ -850,9 +877,9 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2; const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2; - // this is the max speed in m/s at the extents of throttle - float throttleRate; - uint8_t throttleExp; + // this is the max speed in m/s at the extents of thrust + float thrustRate; + uint8_t thrustExp; static uint8_t flightMode; static bool newaltitude = true; @@ -862,8 +889,8 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) AltitudeHoldDesiredData altitudeHoldDesiredData; AltitudeHoldDesiredGet(&altitudeHoldDesiredData); - AltitudeHoldSettingsThrottleExpGet(&throttleExp); - AltitudeHoldSettingsThrottleRateGet(&throttleRate); + AltitudeHoldSettingsThrustExpGet(&thrustExp); + AltitudeHoldSettingsThrustRateGet(&thrustRate); StabilizationBankData stabSettings; StabilizationBankGet(&stabSettings); @@ -880,20 +907,20 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) } uint8_t cutOff; - AltitudeHoldSettingsCutThrottleWhenZeroGet(&cutOff); - if (cutOff && cmd->Throttle < 0) { - // Cut throttle if desired - altitudeHoldDesiredData.SetPoint = cmd->Throttle; - altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_THROTTLE; + AltitudeHoldSettingsCutThrustWhenZeroGet(&cutOff); + if (cutOff && cmd->Thrust < 0) { + // Cut thrust if desired + altitudeHoldDesiredData.SetPoint = cmd->Thrust; + altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_THRUST; newaltitude = true; - } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle > DEADBAND_HIGH) { + } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Thrust > DEADBAND_HIGH) { // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255 - altitudeHoldDesiredData.SetPoint = -((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate); + altitudeHoldDesiredData.SetPoint = -((thrustExp * powf((cmd->Thrust - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - thrustExp) * (cmd->Thrust - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * thrustRate); altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY; newaltitude = true; - } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle < DEADBAND_LOW) { - altitudeHoldDesiredData.SetPoint = -(-(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate); + } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Thrust < DEADBAND_LOW) { + altitudeHoldDesiredData.SetPoint = -(-(thrustExp * powf((DEADBAND_LOW - (cmd->Thrust < 0 ? 0 : cmd->Thrust)) / DEADBAND_LOW, 3) + (255 - thrustExp) * (DEADBAND_LOW - cmd->Thrust) / DEADBAND_LOW) / 255 * thrustRate); altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY; newaltitude = true; } else if (newaltitude == true) { @@ -1041,7 +1068,7 @@ static void setArmedIfChanged(uint8_t val) * @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, int8_t armSwitch) +static void processArm(ManualControlCommandData *cmd, FlightModeSettingsData *settings, int8_t armSwitch) { bool lowThrottle = cmd->Throttle < 0; @@ -1049,9 +1076,9 @@ static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData * do NOT check throttle if disarming via switch, must be instant */ switch (settings->Arming) { - case MANUALCONTROLSETTINGS_ARMING_ACCESSORY0: - case MANUALCONTROLSETTINGS_ARMING_ACCESSORY1: - case MANUALCONTROLSETTINGS_ARMING_ACCESSORY2: + case FLIGHTMODESETTINGS_ARMING_ACCESSORY0: + case FLIGHTMODESETTINGS_ARMING_ACCESSORY1: + case FLIGHTMODESETTINGS_ARMING_ACCESSORY2: if (armSwitch < 0) { lowThrottle = true; } @@ -1066,7 +1093,7 @@ static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData return; } - if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) { + if (settings->Arming == FLIGHTMODESETTINGS_ARMING_ALWAYSDISARMED) { // In this configuration we always disarm setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); } else { @@ -1093,7 +1120,7 @@ static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData } // The rest of these cases throttle is low - if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) { + 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; @@ -1106,27 +1133,27 @@ static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData // Calc channel see assumptions7 switch (settings->Arming) { - case MANUALCONTROLSETTINGS_ARMING_ROLLLEFT: + case FLIGHTMODESETTINGS_ARMING_ROLLLEFT: armingInputLevel = 1.0f * cmd->Roll; break; - case MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT: + case FLIGHTMODESETTINGS_ARMING_ROLLRIGHT: armingInputLevel = -1.0f * cmd->Roll; break; - case MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD: + case FLIGHTMODESETTINGS_ARMING_PITCHFORWARD: armingInputLevel = 1.0f * cmd->Pitch; break; - case MANUALCONTROLSETTINGS_ARMING_PITCHAFT: + case FLIGHTMODESETTINGS_ARMING_PITCHAFT: armingInputLevel = -1.0f * cmd->Pitch; break; - case MANUALCONTROLSETTINGS_ARMING_YAWLEFT: + case FLIGHTMODESETTINGS_ARMING_YAWLEFT: armingInputLevel = 1.0f * cmd->Yaw; break; - case MANUALCONTROLSETTINGS_ARMING_YAWRIGHT: + case FLIGHTMODESETTINGS_ARMING_YAWRIGHT: armingInputLevel = -1.0f * cmd->Yaw; break; - case MANUALCONTROLSETTINGS_ARMING_ACCESSORY0: - case MANUALCONTROLSETTINGS_ARMING_ACCESSORY1: - case MANUALCONTROLSETTINGS_ARMING_ACCESSORY2: + case FLIGHTMODESETTINGS_ARMING_ACCESSORY0: + case FLIGHTMODESETTINGS_ARMING_ACCESSORY1: + case FLIGHTMODESETTINGS_ARMING_ACCESSORY2: armingInputLevel = -1.0f * (float)armSwitch; break; } @@ -1199,7 +1226,7 @@ static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData * @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, ManualControlCommandData *cmd) +static void processFlightMode(ManualControlSettingsData *settings, FlightModeSettingsData *modeSettings, float flightMode, ManualControlCommandData *cmd) { FlightStatusData flightStatus; @@ -1213,7 +1240,7 @@ static void processFlightMode(ManualControlSettingsData *settings, float flightM cmd->FlightModeSwitchPosition = pos; - uint8_t newMode = settings->FlightModePosition[pos]; + uint8_t newMode = modeSettings->FlightModePosition[pos]; if (flightStatus.FlightMode != newMode) { flightStatus.FlightMode = newMode; diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index 6c548b6b7..eba9e2092 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -41,7 +41,7 @@ #include "velocitystate.h" #include "waypoint.h" #include "waypointactive.h" -#include "manualcontrolsettings.h" +#include "flightmodesettings.h" #include #include "paths.h" @@ -170,8 +170,8 @@ static void pathPlannerTask() // copy pasta: same calculation as in manualcontrol, set return to home coordinates PositionStateData positionState; PositionStateGet(&positionState); - ManualControlSettingsData settings; - ManualControlSettingsGet(&settings); + FlightModeSettingsData settings; + FlightModeSettingsGet(&settings); pathDesired.Start.North = 0; pathDesired.Start.East = 0; diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index a956bb62e..354bf9eef 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -49,6 +49,7 @@ #include "gyrostate.h" #include "flightstatus.h" #include "manualcontrolsettings.h" +#include "flightmodesettings.h" #include "manualcontrol.h" // Just to get a macro #include "taskinfo.h" @@ -110,8 +111,8 @@ struct pid pids[PID_MAX]; int cur_flight_mode = -1; static uint8_t rattitude_anti_windup; -static float cruise_control_min_throttle; -static float cruise_control_max_throttle; +static float cruise_control_min_thrust; +static float cruise_control_max_thrust; static uint8_t cruise_control_max_angle; static float cruise_control_max_power_factor; static float cruise_control_power_trim; @@ -168,7 +169,7 @@ int32_t StabilizationStart() int32_t StabilizationInitialize() { // stop the compile if the number of switch positions changes, but has not been changed here - PIOS_STATIC_ASSERT(NUM_FMS_POSITIONS == sizeof(((ManualControlSettingsData *)0)->FlightModePosition) / sizeof((((ManualControlSettingsData *)0)->FlightModePosition)[0])); + PIOS_STATIC_ASSERT(NUM_FMS_POSITIONS == sizeof(((FlightModeSettingsData *)0)->FlightModePosition) / sizeof((((FlightModeSettingsData *)0)->FlightModePosition)[0])); // Initialize variables StabilizationSettingsInitialize(); @@ -205,6 +206,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) ActuatorDesiredData actuatorDesired; StabilizationDesiredData stabDesired; + float throttleDesired; RateDesiredData rateDesired; AttitudeStateData attitudeState; GyroStateData gyroStateData; @@ -236,6 +238,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); FlightStatusGet(&flightStatus); StabilizationDesiredGet(&stabDesired); + ManualControlCommandThrottleGet(&throttleDesired); AttitudeStateGet(&attitudeState); GyroStateGet(&gyroStateData); StabilizationBankGet(&stabBank); @@ -592,10 +595,10 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // Save dT actuatorDesired.UpdateTime = dT * 1000; - actuatorDesired.Throttle = stabDesired.Throttle; + actuatorDesired.Thrust = stabDesired.Thrust; - // Suppress desired output while disarmed or throttle low, for configured axis - if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || stabDesired.Throttle < 0) { + // Suppress desired output while disarmed or thrust low, for configured axis + if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || throttleDesired < 0) { if (lowThrottleZeroAxis[ROLL]) { actuatorDesired.Roll = 0.0f; } @@ -609,9 +612,9 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) } } - // modify throttle according to 1/cos(bank angle) + // modify thrust according to 1/cos(bank angle) // to maintain same altitdue with changing bank angle - // but without manually adjusting throttle + // but without manually adjusting thrust // do it here and all the various flight modes (e.g. Altitude Hold) can use it if (flight_mode_switch_position < NUM_FMS_POSITIONS && cruise_control_flight_mode_switch_pos_enable[flight_mode_switch_position] != (uint8_t)0 @@ -645,7 +648,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) // if inverted and they want negative boost if (angle > 90.0f && cruise_control_inverted_power_switch == (int8_t)-1) { factor = -factor; - // as long as throttle is getting reversed + // as long as thrust is getting reversed // we may as well do pitch and yaw for a complete "invert switch" actuatorDesired.Pitch = -actuatorDesired.Pitch; actuatorDesired.Yaw = -actuatorDesired.Yaw; @@ -653,15 +656,15 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) } } - // also don't adjust throttle if <= 0, leaves neg alone and zero throttle stops motors - if (actuatorDesired.Throttle > cruise_control_min_throttle) { + // also don't adjust thrust if <= 0, leaves neg alone and zero thrust stops motors + if (actuatorDesired.Thrust > cruise_control_min_thrust) { // quad example factor of 2 at hover power of 40%: (0.4 - 0.0) * 2.0 + 0.0 = 0.8 // CP heli example factor of 2 at hover stick of 60%: (0.6 - 0.5) * 2.0 + 0.5 = 0.7 - actuatorDesired.Throttle = (actuatorDesired.Throttle - cruise_control_neutral_thrust) * factor + cruise_control_neutral_thrust; - if (actuatorDesired.Throttle > cruise_control_max_throttle) { - actuatorDesired.Throttle = cruise_control_max_throttle; - } else if (actuatorDesired.Throttle < cruise_control_min_throttle) { - actuatorDesired.Throttle = cruise_control_min_throttle; + actuatorDesired.Thrust = (actuatorDesired.Thrust - cruise_control_neutral_thrust) * factor + cruise_control_neutral_thrust; + if (actuatorDesired.Thrust > cruise_control_max_thrust) { + actuatorDesired.Thrust = cruise_control_max_thrust; + } else if (actuatorDesired.Thrust < cruise_control_min_thrust) { + actuatorDesired.Thrust = cruise_control_min_thrust; } } } @@ -676,7 +679,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) } if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || - (lowThrottleZeroIntegral && stabDesired.Throttle < 0)) { + (lowThrottleZeroIntegral && throttleDesired < 0)) { // Force all axes to reinitialize when engaged for (uint8_t i = 0; i < MAX_AXES; i++) { previous_mode[i] = 255; @@ -906,10 +909,10 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) weak_leveling_kp = settings.WeakLevelingKp; weak_leveling_max = settings.MaxWeakLevelingRate; - // Whether to zero the PID integrals while throttle is low + // Whether to zero the PID integrals while thrust is low lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE; - // Whether to suppress (zero) the StabilizationDesired output for each axis while disarmed or throttle is low + // Whether to suppress (zero) the StabilizationDesired output for each axis while disarmed or thrust is low lowThrottleZeroAxis[ROLL] = settings.LowThrottleZeroAxis.Roll == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; lowThrottleZeroAxis[PITCH] = settings.LowThrottleZeroAxis.Pitch == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; lowThrottleZeroAxis[YAW] = settings.LowThrottleZeroAxis.Yaw == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; @@ -933,10 +936,10 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) cur_flight_mode = -1; // Rattitude flight mode anti-windup factor - rattitude_anti_windup = settings.RattitudeAntiWindup; + rattitude_anti_windup = settings.RattitudeAntiWindup; - cruise_control_min_throttle = (float)settings.CruiseControlMinThrottle / 100.0f; - cruise_control_max_throttle = (float)settings.CruiseControlMaxThrottle / 100.0f; + cruise_control_min_thrust = (float)settings.CruiseControlMinThrust / 100.0f; + cruise_control_max_thrust = (float)settings.CruiseControlMaxThrust / 100.0f; cruise_control_max_angle = settings.CruiseControlMaxAngle; cruise_control_max_power_factor = settings.CruiseControlMaxPowerFactor; cruise_control_power_trim = settings.CruiseControlPowerTrim / 100.0f; diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index 225a93967..101dbec2d 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -158,7 +158,7 @@ static float northPosIntegral = 0; static float eastPosIntegral = 0; static float downPosIntegral = 0; -static float throttleOffset = 0; +static float thrustOffset = 0; /** * Module thread, should not return. */ @@ -263,10 +263,10 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters) eastPosIntegral = 0; downPosIntegral = 0; - // Track throttle before engaging this mode. Cheap system ident + // Track thrust before engaging this mode. Cheap system ident StabilizationDesiredData stabDesired; StabilizationDesiredGet(&stabDesired); - throttleOffset = stabDesired.Throttle; + thrustOffset = stabDesired.Thrust; break; } @@ -550,10 +550,10 @@ static void updateFixedAttitude(float *attitude) StabilizationDesiredData stabDesired; StabilizationDesiredGet(&stabDesired); - stabDesired.Roll = attitude[0]; - stabDesired.Pitch = attitude[1]; - stabDesired.Yaw = attitude[2]; - stabDesired.Throttle = attitude[3]; + stabDesired.Roll = attitude[0]; + stabDesired.Pitch = attitude[1]; + stabDesired.Yaw = attitude[2]; + stabDesired.Thrust = attitude[3]; stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; @@ -653,13 +653,13 @@ static void updateVtolDesiredAttitude(bool yaw_attitude) downError = velocityDesired.Down - downVel; // Must flip this sign downError = -downError; - downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.Ki, - -vtolpathfollowerSettings.VerticalVelPID.ILimit, - vtolpathfollowerSettings.VerticalVelPID.ILimit); - downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID.Kp + downVelIntegral - - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID.Kd); + downVelIntegral = bound(downVelIntegral + downError * dT * vtolpathfollowerSettings.VerticalVelPID.Ki, + -vtolpathfollowerSettings.VerticalVelPID.ILimit, + vtolpathfollowerSettings.VerticalVelPID.ILimit); + downCommand = (downError * vtolpathfollowerSettings.VerticalVelPID.Kp + downVelIntegral + - nedAccel.Down * vtolpathfollowerSettings.VerticalVelPID.Kd); - stabDesired.Throttle = bound(downCommand + throttleOffset, 0, 1); + stabDesired.Thrust = bound(downCommand + thrustOffset, 0, 1); // Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the // craft should move similarly for 5 deg roll versus 5 deg pitch @@ -670,11 +670,11 @@ static void updateVtolDesiredAttitude(bool yaw_attitude) eastCommand * cosf(DEG2RAD(attitudeState.Yaw)), -vtolpathfollowerSettings.MaxRollPitch, vtolpathfollowerSettings.MaxRollPitch); - if (vtolpathfollowerSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) { - // For now override throttle with manual control. Disable at your risk, quad goes to China. + if (vtolpathfollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_FALSE) { + // For now override thrust with manual control. Disable at your risk, quad goes to China. ManualControlCommandData manualControl; ManualControlCommandGet(&manualControl); - stabDesired.Throttle = manualControl.Throttle; + stabDesired.Thrust = manualControl.Thrust; } stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; diff --git a/flight/targets/boards/coptercontrol/firmware/Makefile b/flight/targets/boards/coptercontrol/firmware/Makefile index f8534da9a..a7d78b440 100644 --- a/flight/targets/boards/coptercontrol/firmware/Makefile +++ b/flight/targets/boards/coptercontrol/firmware/Makefile @@ -98,6 +98,7 @@ ifndef TESTAPP SRC += $(OPUAVSYNTHDIR)/i2cstats.c SRC += $(OPUAVSYNTHDIR)/watchdogstatus.c SRC += $(OPUAVSYNTHDIR)/manualcontrolsettings.c + SRC += $(OPUAVSYNTHDIR)/flightmodesettings.c SRC += $(OPUAVSYNTHDIR)/mixersettings.c SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c SRC += $(OPUAVSYNTHDIR)/attitudesettings.c diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index a0e668138..a232bb333 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -60,6 +60,7 @@ UAVOBJSRCFILENAMES += homelocation UAVOBJSRCFILENAMES += i2cstats UAVOBJSRCFILENAMES += manualcontrolcommand UAVOBJSRCFILENAMES += manualcontrolsettings +UAVOBJSRCFILENAMES += flightmodesettings UAVOBJSRCFILENAMES += mixersettings UAVOBJSRCFILENAMES += mixerstatus UAVOBJSRCFILENAMES += nedaccel diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index f93429e80..9921dc965 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -61,6 +61,7 @@ UAVOBJSRCFILENAMES += homelocation UAVOBJSRCFILENAMES += i2cstats UAVOBJSRCFILENAMES += manualcontrolcommand UAVOBJSRCFILENAMES += manualcontrolsettings +UAVOBJSRCFILENAMES += flightmodesettings UAVOBJSRCFILENAMES += mixersettings UAVOBJSRCFILENAMES += mixerstatus UAVOBJSRCFILENAMES += nedaccel diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index 44a9828c6..f47089b8d 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -65,6 +65,7 @@ UAVOBJSRCFILENAMES += homelocation UAVOBJSRCFILENAMES += i2cstats UAVOBJSRCFILENAMES += manualcontrolcommand UAVOBJSRCFILENAMES += manualcontrolsettings +UAVOBJSRCFILENAMES += flightmodesettings UAVOBJSRCFILENAMES += mixersettings UAVOBJSRCFILENAMES += mixerstatus UAVOBJSRCFILENAMES += nedaccel diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp index 002b81fff..526355308 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -57,10 +57,11 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : loop(NULL), skipflag(false) { - manualCommandObj = ManualControlCommand::GetInstance(getObjectManager()); - manualSettingsObj = ManualControlSettings::GetInstance(getObjectManager()); - flightStatusObj = FlightStatus::GetInstance(getObjectManager()); - receiverActivityObj = ReceiverActivity::GetInstance(getObjectManager()); + manualCommandObj = ManualControlCommand::GetInstance(getObjectManager()); + manualSettingsObj = ManualControlSettings::GetInstance(getObjectManager()); + flightModeSettingsObj = FlightModeSettings::GetInstance(getObjectManager()); + flightStatusObj = FlightStatus::GetInstance(getObjectManager()); + receiverActivityObj = ReceiverActivity::GetInstance(getObjectManager()); ui = new Ui_InputWidget(); ui->setupUi(this); @@ -132,26 +133,26 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : connect(ui->wzBack, SIGNAL(clicked()), this, SLOT(wzBack())); ui->stackedWidget->setCurrentIndex(0); - addWidgetBinding("ManualControlSettings", "FlightModePosition", ui->fmsModePos1, 0, 1, true); - addWidgetBinding("ManualControlSettings", "FlightModePosition", ui->fmsModePos2, 1, 1, true); - addWidgetBinding("ManualControlSettings", "FlightModePosition", ui->fmsModePos3, 2, 1, true); - addWidgetBinding("ManualControlSettings", "FlightModePosition", ui->fmsModePos4, 3, 1, true); - addWidgetBinding("ManualControlSettings", "FlightModePosition", ui->fmsModePos5, 4, 1, true); - addWidgetBinding("ManualControlSettings", "FlightModePosition", ui->fmsModePos6, 5, 1, true); + addWidgetBinding("FlightModeSettings", "FlightModePosition", ui->fmsModePos1, 0, 1, true); + addWidgetBinding("FlightModeSettings", "FlightModePosition", ui->fmsModePos2, 1, 1, true); + addWidgetBinding("FlightModeSettings", "FlightModePosition", ui->fmsModePos3, 2, 1, true); + addWidgetBinding("FlightModeSettings", "FlightModePosition", ui->fmsModePos4, 3, 1, true); + addWidgetBinding("FlightModeSettings", "FlightModePosition", ui->fmsModePos5, 4, 1, true); + addWidgetBinding("FlightModeSettings", "FlightModePosition", ui->fmsModePos6, 5, 1, true); addWidgetBinding("ManualControlSettings", "FlightModeNumber", ui->fmsPosNum); - addWidgetBinding("ManualControlSettings", "Stabilization1Settings", ui->fmsSsPos1Roll, "Roll", 1, true); - addWidgetBinding("ManualControlSettings", "Stabilization2Settings", ui->fmsSsPos2Roll, "Roll", 1, true); - addWidgetBinding("ManualControlSettings", "Stabilization3Settings", ui->fmsSsPos3Roll, "Roll", 1, true); - addWidgetBinding("ManualControlSettings", "Stabilization1Settings", ui->fmsSsPos1Pitch, "Pitch", 1, true); - addWidgetBinding("ManualControlSettings", "Stabilization2Settings", ui->fmsSsPos2Pitch, "Pitch", 1, true); - addWidgetBinding("ManualControlSettings", "Stabilization3Settings", ui->fmsSsPos3Pitch, "Pitch", 1, true); - addWidgetBinding("ManualControlSettings", "Stabilization1Settings", ui->fmsSsPos1Yaw, "Yaw", 1, true); - addWidgetBinding("ManualControlSettings", "Stabilization2Settings", ui->fmsSsPos2Yaw, "Yaw", 1, true); - addWidgetBinding("ManualControlSettings", "Stabilization3Settings", ui->fmsSsPos3Yaw, "Yaw", 1, true); + addWidgetBinding("FlightModeSettings", "Stabilization1Settings", ui->fmsSsPos1Roll, "Roll", 1, true); + addWidgetBinding("FlightModeSettings", "Stabilization2Settings", ui->fmsSsPos2Roll, "Roll", 1, true); + addWidgetBinding("FlightModeSettings", "Stabilization3Settings", ui->fmsSsPos3Roll, "Roll", 1, true); + addWidgetBinding("FlightModeSettings", "Stabilization1Settings", ui->fmsSsPos1Pitch, "Pitch", 1, true); + addWidgetBinding("FlightModeSettings", "Stabilization2Settings", ui->fmsSsPos2Pitch, "Pitch", 1, true); + addWidgetBinding("FlightModeSettings", "Stabilization3Settings", ui->fmsSsPos3Pitch, "Pitch", 1, true); + addWidgetBinding("FlightModeSettings", "Stabilization1Settings", ui->fmsSsPos1Yaw, "Yaw", 1, true); + addWidgetBinding("FlightModeSettings", "Stabilization2Settings", ui->fmsSsPos2Yaw, "Yaw", 1, true); + addWidgetBinding("FlightModeSettings", "Stabilization3Settings", ui->fmsSsPos3Yaw, "Yaw", 1, true); - addWidgetBinding("ManualControlSettings", "Arming", ui->armControl); - addWidgetBinding("ManualControlSettings", "ArmedTimeout", ui->armTimeout, 0, 1000); + addWidgetBinding("FlightModeSettings", "Arming", ui->armControl); + addWidgetBinding("FlightModeSettings", "ArmedTimeout", ui->armTimeout, 0, 1000); connect(ManualControlCommand::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(moveFMSlider())); connect(ManualControlSettings::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updatePositionSlider())); @@ -365,10 +366,12 @@ void ConfigInputWidget::goToWizard() // that the UAVO is changed, but then backs out to the start and // chooses a different TX type (which could otherwise result in // unexpected TX channels being enabled) - manualSettingsData = manualSettingsObj->getData(); - previousManualSettingsData = manualSettingsData; - manualSettingsData.Arming = ManualControlSettings::ARMING_ALWAYSDISARMED; - manualSettingsObj->setData(manualSettingsData); + manualSettingsData = manualSettingsObj->getData(); + previousManualSettingsData = manualSettingsData; + flightModeSettingsData = flightModeSettingsObj->getData(); + previousFlightModeSettingsData = flightModeSettingsData; + flightModeSettingsData.Arming = FlightModeSettings::ARMING_ALWAYSDISARMED; + flightModeSettingsObj->setData(flightModeSettingsData); // start the wizard wizardSetUpStep(wizardWelcome); @@ -398,6 +401,7 @@ void ConfigInputWidget::wzCancel() // Load settings back from beginning of wizard manualSettingsObj->setData(previousManualSettingsData); + flightModeSettingsObj->setData(previousFlightModeSettingsData); } void ConfigInputWidget::wzNext() @@ -1218,13 +1222,13 @@ void ConfigInputWidget::moveSticks() Q_ASSERT(0); break; } - if (flightStatusData.FlightMode == manualSettingsData.FlightModePosition[0]) { + if (flightStatusData.FlightMode == flightModeSettingsData.FlightModePosition[0]) { m_txFlightMode->setElementId("flightModeLeft"); m_txFlightMode->setTransform(m_txFlightModeLOrig, false); - } else if (flightStatusData.FlightMode == manualSettingsData.FlightModePosition[1]) { + } else if (flightStatusData.FlightMode == flightModeSettingsData.FlightModePosition[1]) { m_txFlightMode->setElementId("flightModeCenter"); m_txFlightMode->setTransform(m_txFlightModeCOrig, false); - } else if (flightStatusData.FlightMode == manualSettingsData.FlightModePosition[2]) { + } else if (flightStatusData.FlightMode == flightModeSettingsData.FlightModePosition[2]) { m_txFlightMode->setElementId("flightModeRight"); m_txFlightMode->setTransform(m_txFlightModeROrig, false); } @@ -1418,11 +1422,12 @@ void ConfigInputWidget::simpleCalibration(bool enable) msgBox.setDefaultButton(QMessageBox::Ok); msgBox.exec(); - manualCommandData = manualCommandObj->getData(); + manualCommandData = manualCommandObj->getData(); - manualSettingsData = manualSettingsObj->getData(); - manualSettingsData.Arming = ManualControlSettings::ARMING_ALWAYSDISARMED; - manualSettingsObj->setData(manualSettingsData); + manualSettingsData = manualSettingsObj->getData(); + flightModeSettingsData = flightModeSettingsObj->getData(); + flightModeSettingsData.Arming = FlightModeSettings::ARMING_ALWAYSDISARMED; + flightModeSettingsObj->setData(flightModeSettingsData); for (unsigned int i = 0; i < ManualControlCommand::CHANNEL_NUMELEM; i++) { reverse[i] = manualSettingsData.ChannelMax[i] < manualSettingsData.ChannelMin[i]; diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.h b/ground/openpilotgcs/src/plugins/config/configinputwidget.h index 6cd2ef8cd..4d046ca85 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.h @@ -39,6 +39,7 @@ #include #include "manualcontrolcommand.h" #include "manualcontrolsettings.h" +#include "flightmodesettings.h" #include "receiveractivity.h" #include #include @@ -109,6 +110,9 @@ private: ManualControlSettings *manualSettingsObj; ManualControlSettings::DataFields manualSettingsData; ManualControlSettings::DataFields previousManualSettingsData; + FlightModeSettings *flightModeSettingsObj; + FlightModeSettings::DataFields flightModeSettingsData; + FlightModeSettings::DataFields previousFlightModeSettingsData; ReceiverActivity *receiverActivityObj; ReceiverActivity::DataFields receiverActivityData; diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index 7eabe0723..947505d8d 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -27160,7 +27160,7 @@ border-radius: 5; - <html><head/><body><p>-1, 0, or 1. Cruise Control multiplies the throttle stick by this value if the bank angle is past MaxAngle. The default is 0 which says to turn the motors off (actually set them to MinThrottle) when inverted. 1 says to use the unboosted throttle stick value. -1 (DON'T USE, INCOMPLETE, UNTESTED, for use by CP helis using idle up) says to reverse the throttle stick when inverted. + <html><head/><body><p>-1, 0, or 1. Cruise Control multiplies the throttle/collective stick by this value if the bank angle is past MaxAngle. The default is 0 which says to turn the motors off (actually set them to MinThrust) when inverted. 1 says to use the unboosted stick value. -1 (DON'T USE, INCOMPLETE, UNTESTED, for use by CP helis using idle up) says to reverse the collective stick when inverted. </p></body></html> @@ -27236,7 +27236,7 @@ border-radius: 5; - <html><head/><body><p>Really just a safety limit. 4.0 means it will not use more than 4 times the power the throttle stick is requesting.</p></body></html> + <html><head/><body><p>Really just a safety limit. 4.0 means it will not use more than 4 times the power the throttle/collective stick is requesting.</p></body></html> Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -27457,7 +27457,7 @@ color: rgb(255, 255, 255); border-radius: 5; - MinThrottle + MinThrust Qt::AlignCenter @@ -27467,7 +27467,7 @@ border-radius: 5; - <html><head/><body><p>Throttle stick below this disables Cruise Control. Also, by default Cruise Control forces the use of this value for throttle when the copter is inverted. For safety, never set this so low that the trimmed throttle stick cannot get below it.</p></body></html> + <html><head/><body><p>Throttle/Collective stick below this disables Cruise Control. Also, by default Cruise Control forces the use of this value for thrust when the copter is inverted. For safety, never set this so low that the trimmed throttle/collective stick cannot get below it.</p></body></html> Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -27484,7 +27484,7 @@ border-radius: 5; objname:StabilizationSettings - fieldname:CruiseControlMinThrottle + fieldname:CruiseControlMinThrust haslimits:no scale:1 buttongroup:16 @@ -27545,7 +27545,7 @@ border-radius: 5; objname:StabilizationSettings - fieldname:CruiseControlMaxThrottle + fieldname:CruiseControlMaxThrust haslimits:no scale:1 buttongroup:16 @@ -27579,7 +27579,7 @@ color: rgb(255, 255, 255); border-radius: 5; - MaxThrottle + MaxThrust Qt::AlignCenter @@ -30695,7 +30695,7 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:ThrottleRate + fieldname:ThrustRate haslimits:no scale:1 buttongroup:99 @@ -30768,7 +30768,7 @@ border-radius: 5; Qt::StrongFocus - <html><head/><body><p>Throttle exponential value.</p></body></html> + <html><head/><body><p>Thrust exponential value.</p></body></html> @@ -30791,7 +30791,7 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:ThrottleExp + fieldname:ThrustExp haslimits:no scale:1 buttongroup:99,10 @@ -30865,7 +30865,7 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:ThrottleRate + fieldname:ThrustRate haslimits:no scale:1 buttongroup:99,10 @@ -30890,7 +30890,7 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:ThrottleExp + fieldname:ThrustExp haslimits:no scale:1 buttongroup:99 @@ -31441,7 +31441,7 @@ color: rgb(255, 255, 255); border-radius: 5; - Throttle Stick Response + Throttle/Collective Stick Response Qt::AlignCenter diff --git a/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp index d829bd937..a3cfd4379 100644 --- a/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/aerosimrcsimulator.cpp @@ -97,7 +97,7 @@ void AeroSimRCSimulator::transmitUpdate() roll = actData.Roll; pitch = -actData.Pitch; yaw = actData.Yaw; - throttle = (actData.Throttle * 2.0) - 1.0; + throttle = (actData.Thrust * 2.0) - 1.0; } channels[0] = roll; channels[1] = pitch; diff --git a/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp index 3a1d9d2ae..1f4ae8c85 100644 --- a/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/fgsimulator.cpp @@ -174,7 +174,7 @@ void FGSimulator::transmitUpdate() ailerons = actData.Roll; elevator = -actData.Pitch; rudder = actData.Yaw; - throttle = actData.Throttle; + throttle = actData.Thrust; } int allowableDifference = 10; @@ -209,10 +209,10 @@ void FGSimulator::transmitUpdate() } if (settings.manualControlEnabled) { - actData.Roll = ailerons; - actData.Pitch = -elevator; - actData.Yaw = rudder; - actData.Throttle = throttle; + actData.Roll = ailerons; + actData.Pitch = -elevator; + actData.Yaw = rudder; + actData.Thrust = throttle; // actData.NumLongUpdates = (float)udpCounterFGrecv; // actData.UpdateTime = (float)udpCounterGCSsend; actDesired->setData(actData); diff --git a/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp b/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp index 4111dff0b..6341124e3 100644 --- a/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/il2simulator.cpp @@ -104,7 +104,7 @@ void IL2Simulator::transmitUpdate() float ailerons = actData.Roll; float elevator = actData.Pitch; float rudder = actData.Yaw; - float throttle = actData.Throttle * 2 - 1.0; + float throttle = actData.Thrust * 2 - 1.0; // Send update to Il2 QString cmd; diff --git a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp index e7b409815..fcc8b492f 100644 --- a/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp +++ b/ground/openpilotgcs/src/plugins/hitl/xplanesimulator.cpp @@ -102,7 +102,7 @@ void XplaneSimulator::transmitUpdate() float ailerons = actData.Roll; float elevator = actData.Pitch; float rudder = actData.Yaw; - float throttle = actData.Throttle > 0 ? actData.Throttle : 0; + float throttle = actData.Thrust > 0 ? actData.Thrust : 0; float none = -999; // quint32 none = *((quint32*)&tmp); // get float as 4 bytes diff --git a/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp b/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp index 709532222..89024da92 100644 --- a/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp +++ b/ground/openpilotgcs/src/plugins/opmap/opmap_edit_waypoint_dialog.cpp @@ -126,7 +126,7 @@ void opmap_edit_waypoint_dialog::setupModeWidgets() ui->modeParam1->setText("pitch"); ui->modeParam2->setText("roll"); ui->modeParam3->setText("yaw"); - ui->modeParam4->setText("throttle"); + ui->modeParam4->setText("thrust"); ui->modeParam1->setVisible(true); ui->modeParam2->setVisible(true); ui->modeParam3->setVisible(true); diff --git a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp index 6d8e0fd7d..cfd326a68 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp @@ -33,6 +33,7 @@ #include "mixersettings.h" #include "systemsettings.h" #include "manualcontrolsettings.h" +#include "flightmodesettings.h" #include "stabilizationsettings.h" #include "revocalibration.h" @@ -305,29 +306,34 @@ void VehicleConfigurationHelper::applyActuatorConfiguration() void VehicleConfigurationHelper::applyFlighModeConfiguration() { + FlightModeSettings *modeSettings = FlightModeSettings::GetInstance(m_uavoManager); ManualControlSettings *controlSettings = ManualControlSettings::GetInstance(m_uavoManager); + Q_ASSERT(modeSettings); Q_ASSERT(controlSettings); - ManualControlSettings::DataFields data = controlSettings->getData(); - data.Stabilization1Settings[0] = ManualControlSettings::STABILIZATION1SETTINGS_ATTITUDE; - data.Stabilization1Settings[1] = ManualControlSettings::STABILIZATION1SETTINGS_ATTITUDE; - data.Stabilization1Settings[2] = ManualControlSettings::STABILIZATION1SETTINGS_AXISLOCK; - data.Stabilization2Settings[0] = ManualControlSettings::STABILIZATION2SETTINGS_ATTITUDE; - data.Stabilization2Settings[1] = ManualControlSettings::STABILIZATION2SETTINGS_ATTITUDE; - data.Stabilization2Settings[2] = ManualControlSettings::STABILIZATION2SETTINGS_RATE; - data.Stabilization3Settings[0] = ManualControlSettings::STABILIZATION3SETTINGS_RATE; - data.Stabilization3Settings[1] = ManualControlSettings::STABILIZATION3SETTINGS_RATE; - data.Stabilization3Settings[2] = ManualControlSettings::STABILIZATION3SETTINGS_RATE; - data.FlightModeNumber = 3; - data.FlightModePosition[0] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED1; - data.FlightModePosition[1] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED2; - data.FlightModePosition[2] = ManualControlSettings::FLIGHTMODEPOSITION_STABILIZED3; - data.FlightModePosition[3] = ManualControlSettings::FLIGHTMODEPOSITION_ALTITUDEHOLD; - data.FlightModePosition[4] = ManualControlSettings::FLIGHTMODEPOSITION_POSITIONHOLD; - data.FlightModePosition[5] = ManualControlSettings::FLIGHTMODEPOSITION_MANUAL; - controlSettings->setData(data); - addModifiedObject(controlSettings, tr("Writing flight mode settings")); + FlightModeSettings::DataFields data = modeSettings->getData(); + ManualControlSettings::DataFields data2 = controlSettings->getData(); + data.Stabilization1Settings[0] = FlightModeSettings::STABILIZATION1SETTINGS_ATTITUDE; + data.Stabilization1Settings[1] = FlightModeSettings::STABILIZATION1SETTINGS_ATTITUDE; + data.Stabilization1Settings[2] = FlightModeSettings::STABILIZATION1SETTINGS_AXISLOCK; + data.Stabilization2Settings[0] = FlightModeSettings::STABILIZATION2SETTINGS_ATTITUDE; + data.Stabilization2Settings[1] = FlightModeSettings::STABILIZATION2SETTINGS_ATTITUDE; + data.Stabilization2Settings[2] = FlightModeSettings::STABILIZATION2SETTINGS_RATE; + data.Stabilization3Settings[0] = FlightModeSettings::STABILIZATION3SETTINGS_RATE; + data.Stabilization3Settings[1] = FlightModeSettings::STABILIZATION3SETTINGS_RATE; + data.Stabilization3Settings[2] = FlightModeSettings::STABILIZATION3SETTINGS_RATE; + data2.FlightModeNumber = 3; + data.FlightModePosition[0] = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED1; + data.FlightModePosition[1] = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED2; + data.FlightModePosition[2] = FlightModeSettings::FLIGHTMODEPOSITION_STABILIZED3; + data.FlightModePosition[3] = FlightModeSettings::FLIGHTMODEPOSITION_ALTITUDEHOLD; + data.FlightModePosition[4] = FlightModeSettings::FLIGHTMODEPOSITION_POSITIONHOLD; + data.FlightModePosition[5] = FlightModeSettings::FLIGHTMODEPOSITION_MANUAL; + modeSettings->setData(data); + addModifiedObject(modeSettings, tr("Writing flight mode settings 1/2")); + controlSettings->setData(data2); + addModifiedObject(controlSettings, tr("Writing flight mode settings 2/2")); } void VehicleConfigurationHelper::applySensorBiasConfiguration() diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index e5208baa9..64aca3033 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -71,6 +71,7 @@ HEADERS += \ $$UAVOBJECT_SYNTHETICS/stabilizationbank.h \ $$UAVOBJECT_SYNTHETICS/manualcontrolsettings.h \ $$UAVOBJECT_SYNTHETICS/manualcontrolcommand.h \ + $$UAVOBJECT_SYNTHETICS/flightmodesettings.h \ $$UAVOBJECT_SYNTHETICS/stabilizationdesired.h \ $$UAVOBJECT_SYNTHETICS/actuatorsettings.h \ $$UAVOBJECT_SYNTHETICS/actuatordesired.h \ @@ -169,6 +170,7 @@ SOURCES += \ $$UAVOBJECT_SYNTHETICS/stabilizationbank.cpp \ $$UAVOBJECT_SYNTHETICS/manualcontrolsettings.cpp \ $$UAVOBJECT_SYNTHETICS/manualcontrolcommand.cpp \ + $$UAVOBJECT_SYNTHETICS/flightmodesettings.cpp \ $$UAVOBJECT_SYNTHETICS/stabilizationdesired.cpp \ $$UAVOBJECT_SYNTHETICS/actuatorsettings.cpp \ $$UAVOBJECT_SYNTHETICS/actuatordesired.cpp \ diff --git a/shared/uavobjectdefinition/actuatordesired.xml b/shared/uavobjectdefinition/actuatordesired.xml index 371d025b1..8ec83b9e2 100644 --- a/shared/uavobjectdefinition/actuatordesired.xml +++ b/shared/uavobjectdefinition/actuatordesired.xml @@ -4,7 +4,7 @@ - + diff --git a/shared/uavobjectdefinition/altitudeholddesired.xml b/shared/uavobjectdefinition/altitudeholddesired.xml index 1b349766f..39babfe63 100644 --- a/shared/uavobjectdefinition/altitudeholddesired.xml +++ b/shared/uavobjectdefinition/altitudeholddesired.xml @@ -2,7 +2,7 @@ Holds the desired altitude (from manual control) as well as the desired attitude to pass through - + diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index 174468344..ed6322131 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -3,9 +3,9 @@ Settings for the @ref AltitudeHold module - - - + + + diff --git a/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml b/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml index 1d1e42c29..9be020de8 100644 --- a/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml +++ b/shared/uavobjectdefinition/fixedwingpathfollowersettings.xml @@ -32,7 +32,7 @@ - @@ -40,8 +40,8 @@ - - + + diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml new file mode 100644 index 000000000..b9b6aa211 --- /dev/null +++ b/shared/uavobjectdefinition/flightmodesettings.xml @@ -0,0 +1,66 @@ + + + Settings to control arming and flight mode + + + + + + + + + + + + + + + + + + + + + + diff --git a/shared/uavobjectdefinition/manualcontrolcommand.xml b/shared/uavobjectdefinition/manualcontrolcommand.xml index 8c0c6f3a2..067b4821e 100644 --- a/shared/uavobjectdefinition/manualcontrolcommand.xml +++ b/shared/uavobjectdefinition/manualcontrolcommand.xml @@ -7,6 +7,7 @@ + diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index 6105f44c2..d4fbe858b 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -17,64 +17,13 @@ - - - - - - - - - - - - - + + + diff --git a/shared/uavobjectdefinition/stabilizationdesired.xml b/shared/uavobjectdefinition/stabilizationdesired.xml index a93e1f34c..70bdfe9c0 100644 --- a/shared/uavobjectdefinition/stabilizationdesired.xml +++ b/shared/uavobjectdefinition/stabilizationdesired.xml @@ -4,9 +4,10 @@ - + + diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index a9334bf1b..1a9a3d601 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -30,8 +30,8 @@ - - + + diff --git a/shared/uavobjectdefinition/systemsettings.xml b/shared/uavobjectdefinition/systemsettings.xml index fc7c9d462..b3a54fffb 100644 --- a/shared/uavobjectdefinition/systemsettings.xml +++ b/shared/uavobjectdefinition/systemsettings.xml @@ -2,6 +2,11 @@ Select airframe type. Currently used by @ref ActuatorModule to choose mixing from @ref ActuatorDesired to @ref ActuatorCommand + + diff --git a/shared/uavobjectdefinition/vtolpathfollowersettings.xml b/shared/uavobjectdefinition/vtolpathfollowersettings.xml index 6fc4cce26..a8a5e49e7 100644 --- a/shared/uavobjectdefinition/vtolpathfollowersettings.xml +++ b/shared/uavobjectdefinition/vtolpathfollowersettings.xml @@ -9,7 +9,7 @@ - + From 37ef381f8dcf918316a494444427956477967ba4 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 16 Feb 2014 11:30:28 +0100 Subject: [PATCH 02/18] OP-1216 clamping of throttle to -1 in actuators (optional) --- flight/modules/Actuator/actuator.c | 6 +++++ shared/uavobjectdefinition/systemsettings.xml | 22 ++++++++++++++++--- 2 files changed, 25 insertions(+), 3 deletions(-) diff --git a/flight/modules/Actuator/actuator.c b/flight/modules/Actuator/actuator.c index f2f91f0b2..cb63111b7 100644 --- a/flight/modules/Actuator/actuator.c +++ b/flight/modules/Actuator/actuator.c @@ -167,6 +167,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters) MixerStatusData mixerStatus; FlightStatusData flightStatus; SystemSettingsThrustControlOptions thrustType; + SystemSettingsAllowReverseThrottleOptions noClamping; float throttleDesired; float collectiveDesired; @@ -224,6 +225,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters) ActuatorDesiredGet(&desired); ActuatorCommandGet(&command); SystemSettingsThrustControlGet(&thrustType); + SystemSettingsAllowReverseThrottleGet(&noClamping); // read in throttle and collective -demultiplex thrust switch (thrustType) { @@ -240,6 +242,10 @@ static void actuatorTask(__attribute__((unused)) void *parameters) ManualControlCommandCollectiveGet(&collectiveDesired); } + if (noClamping == SYSTEMSETTINGS_ALLOWREVERSETHROTTLE_FALSE) { + throttleDesired = (throttleDesired < 0) ? -1 : throttleDesired; + } + #ifdef DIAG_MIXERSTATUS MixerStatusGet(&mixerStatus); #endif diff --git a/shared/uavobjectdefinition/systemsettings.xml b/shared/uavobjectdefinition/systemsettings.xml index b3a54fffb..cafd62737 100644 --- a/shared/uavobjectdefinition/systemsettings.xml +++ b/shared/uavobjectdefinition/systemsettings.xml @@ -4,9 +4,25 @@ + "Throttle" (quadcopter, simple brushless planes, + car-type ground vehicles) + "Collective" (collective pitch as in most + helicopters, 3d quads, constant RPM variable pitch + airplanes, and ground vehicles with diesel-electric + locomotion) + "None" (craft has neither engines nor dynamic brakes. + Note that a glider with breaking flaps should likely + use "collective" and use the collective channel to + control the brakes for optimum autopilot performance) + --> + + From 227aab80b9d1352da514334558617d83e0c204da Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 16 Feb 2014 13:49:51 +0100 Subject: [PATCH 03/18] OP-1217 moved manualcontrol to receiver --- .../{manualcontrol.c => control.c} | 0 flight/modules/Receiver/receiver.c | 1303 +++++++++++++++++ 2 files changed, 1303 insertions(+) rename flight/modules/ManualControl/{manualcontrol.c => control.c} (100%) create mode 100644 flight/modules/Receiver/receiver.c diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/control.c similarity index 100% rename from flight/modules/ManualControl/manualcontrol.c rename to flight/modules/ManualControl/control.c diff --git a/flight/modules/Receiver/receiver.c b/flight/modules/Receiver/receiver.c new file mode 100644 index 000000000..940e5cf5c --- /dev/null +++ b/flight/modules/Receiver/receiver.c @@ -0,0 +1,1303 @@ +/** + ****************************************************************************** + * @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 +#include +#include "accessorydesired.h" +#include "actuatordesired.h" +#include "altitudeholddesired.h" +#include "flighttelemetrystats.h" +#include "flightstatus.h" +#include "sanitycheck.h" +#include "manualcontrol.h" +#include "manualcontrolsettings.h" +#include "manualcontrolcommand.h" +#include "flightmodesettings.h" +#include "positionstate.h" +#include "pathdesired.h" +#include "stabilizationbank.h" +#include "stabilizationdesired.h" +#include "receiveractivity.h" +#include "systemsettings.h" +#include +#include + +#if defined(PIOS_INCLUDE_USB_RCTX) +#include "pios_usb_rctx.h" +#endif /* PIOS_INCLUDE_USB_RCTX */ + +// Private constants +#if defined(PIOS_MANUAL_STACK_SIZE) +#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE +#else +#define STACK_SIZE_BYTES 1152 +#endif + +#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) // 3 = flight control +#define UPDATE_PERIOD_MS 20 +#define THROTTLE_FAILSAFE -0.1f +#define ARMED_THRESHOLD 0.50f +// safe band to allow a bit of calibration error or trim offset (in microseconds) +#define CONNECTION_OFFSET 250 + +// 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; + +#ifdef USE_INPUT_LPF +static portTickType lastSysTimeLPF; +static float inputFiltered[MANUALCONTROLSETTINGS_RESPONSETIME_NUMELEM]; +#endif + +// Private functions +static void updateActuatorDesired(ManualControlCommandData *cmd); +static void updateStabilizationDesired(ManualControlCommandData *cmd, FlightModeSettingsData *settings); +static void updateLandDesired(ManualControlCommandData *cmd, bool changed); +static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed); +static void updatePathDesired(ManualControlCommandData *cmd, bool changed, bool home); +static void processFlightMode(ManualControlSettingsData *settings, FlightModeSettingsData *modeSettings, float flightMode, ManualControlCommandData *cmd); +static void processArm(ManualControlCommandData *cmd, FlightModeSettingsData *settings, int8_t armSwitch); +static void setArmedIfChanged(uint8_t val); +static void configurationUpdatedCb(UAVObjEvent *ev); + +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); +static void applyDeadband(float *value, float deadband); + +#ifdef USE_INPUT_LPF +static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT); +#endif + +#define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12 +#define RCVR_ACTIVITY_MONITOR_MIN_RANGE 10 +struct rcvr_activity_fsm { + ManualControlSettingsChannelGroupsOptions group; + uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP]; + uint8_t sample_count; +}; +static struct rcvr_activity_fsm activity_fsm; + +static void resetRcvrActivity(struct rcvr_activity_fsm *fsm); +static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm); + +#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions_flightmode && assumptions_channelcount) + +/** + * Module starting + */ +int32_t ManualControlStart() +{ + // Start main task + xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_MANUALCONTROL, taskHandle); +#ifdef PIOS_INCLUDE_WDG + PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL); +#endif + + return 0; +} + +/** + * Module initialization + */ +int32_t ManualControlInitialize() +{ + /* Check the assumptions about uavobject enum's are correct */ + if (!assumptions) { + return -1; + } + + AccessoryDesiredInitialize(); + ManualControlCommandInitialize(); + FlightStatusInitialize(); + StabilizationDesiredInitialize(); + ReceiverActivityInitialize(); + ManualControlSettingsInitialize(); + FlightModeSettingsInitialize(); + + return 0; +} +MODULE_INITCALL(ManualControlInitialize, ManualControlStart); + +/** + * Module task + */ +static void manualControlTask(__attribute__((unused)) void *parameters) +{ + ManualControlSettingsData settings; + FlightModeSettingsData modeSettings; + 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(); + + // Run this initially to make sure the configuration is checked + configuration_check(); + + // Whenever the configuration changes, make sure it is safe to fly + SystemSettingsConnectCallback(configurationUpdatedCb); + ManualControlSettingsConnectCallback(configurationUpdatedCb); + + // Whenever the configuration changes, make sure it is safe to fly + + // Make sure unarmed on power up + ManualControlCommandGet(&cmd); + FlightStatusGet(&flightStatus); + flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED; + armState = ARM_STATE_DISARMED; + + /* Initialize the RcvrActivty FSM */ + portTickType lastActivityTime = xTaskGetTickCount(); + resetRcvrActivity(&activity_fsm); + + // Main task loop + lastSysTime = xTaskGetTickCount(); + + float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM] = { 0 }; + SystemSettingsThrustControlOptions thrustType; + + while (1) { + // Wait until next update + vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS); +#ifdef PIOS_INCLUDE_WDG + PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL); +#endif + + // Read settings + ManualControlSettingsGet(&settings); + FlightModeSettingsGet(&modeSettings); + SystemSettingsThrustControlGet(&thrustType); + + /* Update channel activity monitor */ + if (flightStatus.Armed == ARM_STATE_DISARMED) { + if (updateRcvrActivity(&activity_fsm)) { + /* Reset the aging timer because activity was detected */ + lastActivityTime = lastSysTime; + } + } + if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) { + resetRcvrActivity(&activity_fsm); + lastActivityTime = lastSysTime; + } + + if (ManualControlCommandReadOnly()) { + FlightTelemetryStatsData flightTelemStats; + FlightTelemetryStatsGet(&flightTelemStats); + if (flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { + /* trying to fly via GCS and lost connection. fall back to transmitter */ + UAVObjMetadata metadata; + ManualControlCommandGetMetadata(&metadata); + UAVObjSetAccess(&metadata, ACCESS_READWRITE); + ManualControlCommandSetMetadata(&metadata); + } + } + + if (!ManualControlCommandReadOnly()) { + bool valid_input_detected = true; + + // Read channel values in us + for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { + extern uint32_t pios_rcvr_group_map[]; + + if (cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Roll)[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + cmd.Channel[n] = PIOS_RCVR_INVALID; + } else { + cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[ + cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Pitch)[n]], + cast_struct_to_array(settings.ChannelNumber, settings.ChannelNumber.Pitch)[n]); + } + + // If a channel has timed out this is not valid data and we shouldn't update anything + // until we decide to go to failsafe + if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT) { + valid_input_detected = false; + } else { + scaledChannel[n] = scaleChannel(cmd.Channel[n], + cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Pitch)[n], + cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Pitch)[n], + cast_struct_to_array(settings.ChannelNeutral, settings.ChannelNeutral.Pitch)[n]); + } + } + + // Check settings, if error raise alarm + if (settings.ChannelGroups.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups.Throttle >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || + // Check all channel mappings are valid + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_INVALID + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_INVALID + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_INVALID + || + // Check the driver exists + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_NODRIVER + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_NODRIVER + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_NODRIVER + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_NODRIVER || + // Check the FlightModeNumber is valid + settings.FlightModeNumber < 1 || settings.FlightModeNumber > FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM || + // Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care + ((settings.FlightModeNumber > 1) + && (settings.ChannelGroups.FlightMode >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_INVALID + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_NODRIVER))) { + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); + cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; + ManualControlCommandSet(&cmd); + + // Need to do this here since we don't process armed status. Since this shouldn't happen in flight (changed config) + // immediately disarm + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); + + continue; + } + + // decide if we have valid manual input or not + valid_input_detected &= validInputRange(settings.ChannelMin.Throttle, + settings.ChannelMax.Throttle, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) + && validInputRange(settings.ChannelMin.Roll, + settings.ChannelMax.Roll, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) + && validInputRange(settings.ChannelMin.Yaw, + settings.ChannelMax.Yaw, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) + && validInputRange(settings.ChannelMin.Pitch, + settings.ChannelMax.Pitch, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_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; + } + + int8_t armSwitch = 0; + if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { + cmd.Throttle = settings.FailsafeChannel.Throttle; + cmd.Roll = settings.FailsafeChannel.Roll; + cmd.Pitch = settings.FailsafeChannel.Pitch; + cmd.Yaw = settings.FailsafeChannel.Yaw; + cmd.Collective = settings.FailsafeChannel.Collective; + switch (thrustType) { + case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE: + cmd.Thrust = cmd.Throttle; + break; + case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE: + cmd.Thrust = cmd.Collective; + break; + default: + break; + } + if (settings.FailsafeFlightModeSwitchPosition >= 0 && settings.FailsafeFlightModeSwitchPosition < settings.FlightModeNumber) { + FlightStatusGet(&flightStatus); + + cmd.FlightModeSwitchPosition = (uint8_t)settings.FailsafeFlightModeSwitchPosition; + flightStatus.FlightMode = modeSettings.FlightModePosition[settings.FailsafeFlightModeSwitchPosition]; + FlightStatusSet(&flightStatus); + } + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + + AccessoryDesiredData accessory; + // Set Accessory 0 + if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = settings.FailsafeChannel.Accessory0; + if (AccessoryDesiredInstSet(0, &accessory) != 0) { + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + } + // Set Accessory 1 + if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = settings.FailsafeChannel.Accessory1; + if (AccessoryDesiredInstSet(1, &accessory) != 0) { + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + } + // Set Accessory 2 + if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = settings.FailsafeChannel.Accessory2; + if (AccessoryDesiredInstSet(2, &accessory) != 0) { + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + } + } else if (valid_input_detected) { + AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); + + // Scale channels to -1 -> +1 range + cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]; + cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]; + cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]; + cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]; + flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE]; + + // Apply deadband for Roll/Pitch/Yaw stick inputs + if (settings.Deadband > 0.0f) { + applyDeadband(&cmd.Roll, settings.Deadband); + applyDeadband(&cmd.Pitch, settings.Deadband); + applyDeadband(&cmd.Yaw, settings.Deadband); + } +#ifdef USE_INPUT_LPF + // Apply Low Pass Filter to input channels, time delta between calls in ms + portTickType thisSysTime = xTaskGetTickCount(); + float dT = (thisSysTime > lastSysTimeLPF) ? + (float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) : + (float)UPDATE_PERIOD_MS; + lastSysTimeLPF = thisSysTime; + + applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings, dT); + applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings, dT); + applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings, dT); +#endif // USE_INPUT_LPF + if (cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_INVALID + && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_NODRIVER + && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_TIMEOUT) { + cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE]; + } + + switch (thrustType) { + case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE: + cmd.Thrust = cmd.Throttle; + break; + case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE: + cmd.Thrust = cmd.Collective; + break; + default: + break; + } + + AccessoryDesiredData accessory; + // Set Accessory 0 + if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0]; +#ifdef USE_INPUT_LPF + applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT); +#endif + if (modeSettings.Arming == FLIGHTMODESETTINGS_ARMING_ACCESSORY0) { + if (accessory.AccessoryVal > ARMED_THRESHOLD) { + armSwitch = 1; + } else if (accessory.AccessoryVal < -ARMED_THRESHOLD) { + armSwitch = -1; + } + } + if (AccessoryDesiredInstSet(0, &accessory) != 0) { + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + } + // Set Accessory 1 + if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1]; +#ifdef USE_INPUT_LPF + applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT); +#endif + if (modeSettings.Arming == FLIGHTMODESETTINGS_ARMING_ACCESSORY1) { + if (accessory.AccessoryVal > ARMED_THRESHOLD) { + armSwitch = 1; + } else if (accessory.AccessoryVal < -ARMED_THRESHOLD) { + armSwitch = -1; + } + } + if (AccessoryDesiredInstSet(1, &accessory) != 0) { + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + } + // Set Accessory 2 + if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2]; +#ifdef USE_INPUT_LPF + applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT); +#endif + if (modeSettings.Arming == FLIGHTMODESETTINGS_ARMING_ACCESSORY2) { + if (accessory.AccessoryVal > ARMED_THRESHOLD) { + armSwitch = 1; + } else if (accessory.AccessoryVal < -ARMED_THRESHOLD) { + armSwitch = -1; + } + } + + if (AccessoryDesiredInstSet(2, &accessory) != 0) { + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + } + + processFlightMode(&settings, &modeSettings, flightMode, &cmd); + } + + // Process arming outside conditional so system will disarm when disconnected + processArm(&cmd, &modeSettings, armSwitch); + + // Update cmd object + ManualControlCommandSet(&cmd); +#if defined(PIOS_INCLUDE_USB_RCTX) + if (pios_usb_rctx_id) { + PIOS_USB_RCTX_Update(pios_usb_rctx_id, + cmd.Channel, + cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Roll), + cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Roll), + NELEMENTS(cmd.Channel)); + } +#endif /* PIOS_INCLUDE_USB_RCTX */ + } else { + ManualControlCommandGet(&cmd); /* Under GCS control */ + } + + FlightStatusGet(&flightStatus); + + // Depending on the mode update the Stabilization or Actuator objects + static uint8_t lastFlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL; + 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, &modeSettings); + break; + case FLIGHTMODE_TUNING: + // Tuning takes settings directly from manualcontrolcommand. No need to + // call anything else. This just avoids errors. + break; + case FLIGHTMODE_GUIDANCE: + switch (flightStatus.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: + case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO: + altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode); + break; + case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: + case FLIGHTSTATUS_FLIGHTMODE_POI: + updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, false); + break; + case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: + updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, true); + break; + case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: + // No need to call anything. This just avoids errors. + break; + case FLIGHTSTATUS_FLIGHTMODE_LAND: + updateLandDesired(&cmd, lastFlightMode != flightStatus.FlightMode); + break; + default: + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); + } + break; + } + lastFlightMode = flightStatus.FlightMode; + } +} + +static void resetRcvrActivity(struct rcvr_activity_fsm *fsm) +{ + ReceiverActivityData data; + bool updated = false; + + /* Clear all channel activity flags */ + ReceiverActivityGet(&data); + if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE && data.ActiveChannel != 255) { + data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE; + data.ActiveChannel = 255; + updated = true; + } + if (updated) { + ReceiverActivitySet(&data); + } + + /* Reset the FSM state */ + fsm->group = 0; + fsm->sample_count = 0; +} + +static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels) +{ + for (uint8_t channel = 1; channel <= max_channels; channel++) { + // Subtract 1 because channels are 1 indexed + samples[channel - 1] = PIOS_RCVR_Read(rcvr_id, channel); + } +} + +static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm *fsm) +{ + bool activity_updated = false; + + /* Compare the current value to the previous sampled value */ + for (uint8_t channel = 1; channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; channel++) { + uint16_t delta; + uint16_t prev = fsm->prev[channel - 1]; // Subtract 1 because channels are 1 indexed + uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel); + if (curr > prev) { + delta = curr - prev; + } else { + delta = prev - curr; + } + + if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) { + /* Mark this channel as active */ + ReceiverActivityActiveGroupOptions group; + + /* Don't assume manualcontrolsettings and receiveractivity are in the same order. */ + switch (fsm->group) { + case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM: + group = RECEIVERACTIVITY_ACTIVEGROUP_PWM; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM: + group = RECEIVERACTIVITY_ACTIVEGROUP_PPM; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT: + group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT: + group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS: + group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS: + group = RECEIVERACTIVITY_ACTIVEGROUP_GCS; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK: + group = RECEIVERACTIVITY_ACTIVEGROUP_OPLINK; + break; + default: + PIOS_Assert(0); + break; + } + + ReceiverActivityActiveGroupSet((uint8_t *)&group); + ReceiverActivityActiveChannelSet(&channel); + activity_updated = true; + } + } + return activity_updated; +} + +static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm) +{ + bool activity_updated = false; + + if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + /* We're out of range, reset things */ + resetRcvrActivity(fsm); + } + + extern uint32_t pios_rcvr_group_map[]; + if (!pios_rcvr_group_map[fsm->group]) { + /* Unbound group, skip it */ + goto group_completed; + } + + if (fsm->sample_count == 0) { + /* Take a sample of each channel in this group */ + updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev)); + fsm->sample_count++; + return false; + } + + /* Compare with previous sample */ + activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm); + +group_completed: + /* Reset the sample counter */ + fsm->sample_count = 0; + + /* Find the next active group, but limit search so we can't loop forever here */ + for (uint8_t i = 0; i < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE; i++) { + /* Move to the next group */ + fsm->group++; + if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + /* Wrap back to the first group */ + fsm->group = 0; + } + if (pios_rcvr_group_map[fsm->group]) { + /* + * Found an active group, take a sample here to avoid an + * extra 20ms delay in the main thread so we can speed up + * this algorithm. + */ + updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev)); + fsm->sample_count++; + break; + } + } + + return activity_updated; +} + +static void updateActuatorDesired(ManualControlCommandData *cmd) +{ + ActuatorDesiredData actuator; + + ActuatorDesiredGet(&actuator); + actuator.Roll = cmd->Roll; + actuator.Pitch = cmd->Pitch; + actuator.Yaw = cmd->Yaw; + actuator.Thrust = cmd->Thrust; + ActuatorDesiredSet(&actuator); +} + +static void updateStabilizationDesired(ManualControlCommandData *cmd, FlightModeSettingsData *settings) +{ + StabilizationDesiredData stabilization; + + StabilizationDesiredGet(&stabilization); + + StabilizationBankData stabSettings; + StabilizationBankGet(&stabSettings); + + uint8_t *stab_settings; + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + switch (flightStatus.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: + stab_settings = cast_struct_to_array(settings->Stabilization1Settings, settings->Stabilization1Settings.Roll); + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: + stab_settings = cast_struct_to_array(settings->Stabilization2Settings, settings->Stabilization2Settings.Roll); + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: + stab_settings = cast_struct_to_array(settings->Stabilization3Settings, settings->Stabilization3Settings.Roll); + 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; + } + + stabilization.Roll = + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? 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.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : + 0; // this is an invalid mode + + // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order + stabilization.StabilizationMode.Roll = stab_settings[0]; + stabilization.StabilizationMode.Pitch = stab_settings[1]; + // Other axes (yaw) cannot be Rattitude, so use Rate + // Should really do this for Attitude mode as well? + if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) { + stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabilization.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw; + } else { + stabilization.StabilizationMode.Yaw = stab_settings[2]; + stabilization.Yaw = + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : + 0; // this is an invalid mode + } + + stabilization.Thrust = cmd->Thrust; + StabilizationDesiredSet(&stabilization); +} + +#if defined(REVOLUTION) +// TODO: Need compile flag to exclude this from copter control +/** + * @brief Update the position desired to current location when + * enabled and allow the waypoint to be moved by transmitter + */ +static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *cmd, bool changed, bool home) +{ + /* + static portTickType lastSysTime; + portTickType thisSysTime = xTaskGetTickCount(); + dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f); + lastSysTime = thisSysTime; + */ + + if (home && changed) { + // Simple Return To Base mode - keep altitude the same, fly to home position + PositionStateData positionState; + PositionStateGet(&positionState); + FlightModeSettingsData settings; + FlightModeSettingsGet(&settings); + + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + pathDesired.Start.North = 0; + pathDesired.Start.East = 0; + pathDesired.Start.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; + pathDesired.End.North = 0; + pathDesired.End.East = 0; + pathDesired.End.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; + pathDesired.StartingVelocity = 1; + pathDesired.EndingVelocity = 0; + pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + PathDesiredSet(&pathDesired); + } else if (changed) { + // After not being in this mode for a while init at current height + PositionStateData positionState; + PositionStateGet(&positionState); + + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + pathDesired.Start.North = positionState.North; + pathDesired.Start.East = positionState.East; + pathDesired.Start.Down = positionState.Down; + pathDesired.End.North = positionState.North; + pathDesired.End.East = positionState.East; + pathDesired.End.Down = positionState.Down; + pathDesired.StartingVelocity = 1; + pathDesired.EndingVelocity = 0; + pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + PathDesiredSet(&pathDesired); + /* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts. + } else { + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch; + pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll; + pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + PathDesiredSet(&pathDesired); + */ + } +} + +static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *cmd, bool changed) +{ + /* + static portTickType lastSysTime; + portTickType thisSysTime; + float dT; + + thisSysTime = xTaskGetTickCount(); + dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f); + lastSysTime = thisSysTime; + */ + + PositionStateData positionState; + + PositionStateGet(&positionState); + + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + if (changed) { + // After not being in this mode for a while init at current height + pathDesired.Start.North = positionState.North; + pathDesired.Start.East = positionState.East; + pathDesired.Start.Down = positionState.Down; + pathDesired.End.North = positionState.North; + pathDesired.End.East = positionState.East; + pathDesired.End.Down = positionState.Down; + pathDesired.StartingVelocity = 1; + pathDesired.EndingVelocity = 0; + pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + } + pathDesired.End.Down = positionState.Down + 5; + PathDesiredSet(&pathDesired); +} + +/** + * @brief Update the altitude desired to current altitude when + * enabled and enable altitude mode for stabilization + * @todo: Need compile flag to exclude this from copter control + */ +static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) +{ + const float DEADBAND = 0.20f; + const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2; + const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2; + + // this is the max speed in m/s at the extents of thrust + float thrustRate; + uint8_t thrustExp; + + static uint8_t flightMode; + static bool newaltitude = true; + + FlightStatusFlightModeGet(&flightMode); + + AltitudeHoldDesiredData altitudeHoldDesiredData; + AltitudeHoldDesiredGet(&altitudeHoldDesiredData); + + AltitudeHoldSettingsThrustExpGet(&thrustExp); + AltitudeHoldSettingsThrustRateGet(&thrustRate); + + StabilizationBankData stabSettings; + StabilizationBankGet(&stabSettings); + + PositionStateData posState; + PositionStateGet(&posState); + + altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; + altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax; + altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw; + + if (changed) { + newaltitude = true; + } + + uint8_t cutOff; + AltitudeHoldSettingsCutThrustWhenZeroGet(&cutOff); + if (cutOff && cmd->Thrust < 0) { + // Cut thrust if desired + altitudeHoldDesiredData.SetPoint = cmd->Thrust; + altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_THRUST; + newaltitude = true; + } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Thrust > DEADBAND_HIGH) { + // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 + // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255 + altitudeHoldDesiredData.SetPoint = -((thrustExp * powf((cmd->Thrust - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - thrustExp) * (cmd->Thrust - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * thrustRate); + altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY; + newaltitude = true; + } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Thrust < DEADBAND_LOW) { + altitudeHoldDesiredData.SetPoint = -(-(thrustExp * powf((DEADBAND_LOW - (cmd->Thrust < 0 ? 0 : cmd->Thrust)) / DEADBAND_LOW, 3) + (255 - thrustExp) * (DEADBAND_LOW - cmd->Thrust) / DEADBAND_LOW) / 255 * thrustRate); + altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY; + newaltitude = true; + } else if (newaltitude == true) { + altitudeHoldDesiredData.SetPoint = posState.Down; + altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE; + newaltitude = false; + } + + AltitudeHoldDesiredSet(&altitudeHoldDesiredData); +} +#else /* if defined(REVOLUTION) */ + +// TODO: These functions should never be accessible on CC. Any configuration that +// could allow them to be called should already throw an error to prevent this happening +// in flight +static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *cmd, + __attribute__((unused)) bool changed, + __attribute__((unused)) bool home) +{ + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); +} + +static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *cmd, + __attribute__((unused)) bool changed) +{ + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); +} + +static void altitudeHoldDesired(__attribute__((unused)) ManualControlCommandData *cmd, + __attribute__((unused)) bool changed) +{ + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); +} +#endif /* if defined(REVOLUTION) */ +/** + * 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.0f) { + valueScaled = 1.0f; + } else if (valueScaled < -1.0f) { + valueScaled = -1.0f; + } + + return valueScaled; +} + +static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) +{ + return (end_time - start_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) +{ + // update checks + configuration_check(); + + // read alarms + SystemAlarmsData alarms; + + SystemAlarmsGet(&alarms); + + // Check each alarm + for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) { + if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set + if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) { + continue; + } + + return false; + } + } + + uint8_t flightMode; + FlightStatusFlightModeGet(&flightMode); + switch (flightMode) { + case FLIGHTSTATUS_FLIGHTMODE_MANUAL: + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: + return true; + + default: + return false; + } +} +/** + * @brief Determine if the aircraft is forced to disarm by an explicit alarm + * @returns True if safe to arm, false otherwise + */ +static bool forcedDisArm(void) +{ + // read alarms + SystemAlarmsData alarms; + + SystemAlarmsGet(&alarms); + + if (alarms.Alarm.Guidance == SYSTEMALARMS_ALARM_CRITICAL) { + return true; + } + return false; +} + +/** + * @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, FlightModeSettingsData *settings, int8_t armSwitch) +{ + bool lowThrottle = cmd->Throttle < 0; + + /** + * do NOT check throttle if disarming via switch, must be instant + */ + switch (settings->Arming) { + case FLIGHTMODESETTINGS_ARMING_ACCESSORY0: + case FLIGHTMODESETTINGS_ARMING_ACCESSORY1: + case FLIGHTMODESETTINGS_ARMING_ACCESSORY2: + if (armSwitch < 0) { + lowThrottle = true; + } + break; + default: + break; + } + + 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); + } else { + // Not really needed since this function not called when disconnected + if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { + lowThrottle = true; + } + + // 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 * (float)armSwitch; + 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) > 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 = 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) > settings->DisarmingSequenceTime)) { + armState = ARM_STATE_DISARMED; + } else if (!manualDisarm) { + armState = ARM_STATE_ARMED; + } + break; + } // End Switch + } +} + +/** + * @brief Determine which of N 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, FlightModeSettingsData *modeSettings, float flightMode, ManualControlCommandData *cmd) +{ + FlightStatusData flightStatus; + + FlightStatusGet(&flightStatus); + + // Convert flightMode value into the switch position in the range [0..N-1] + uint8_t pos = ((int16_t)(flightMode * 256.0f) + 256) * settings->FlightModeNumber >> 9; + if (pos >= settings->FlightModeNumber) { + pos = settings->FlightModeNumber - 1; + } + + cmd->FlightModeSwitchPosition = pos; + + uint8_t newMode = modeSettings->FlightModePosition[pos]; + + 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; +} + +/** + * @brief Apply deadband to Roll/Pitch/Yaw channels + */ +static void applyDeadband(float *value, float deadband) +{ + if (fabsf(*value) < deadband) { + *value = 0.0f; + } else if (*value > 0.0f) { + *value -= deadband; + } else { + *value += deadband; + } +} + +#ifdef USE_INPUT_LPF +/** + * @brief Apply Low Pass Filter to Throttle/Roll/Pitch/Yaw or Accessory channel + */ +static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT) +{ + if (cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel]) { + float rt = (float)cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel]; + inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT); + *value = inputFiltered[channel]; + } +} +#endif // USE_INPUT_LPF +/** + * Called whenever a critical configuration component changes + */ +static void configurationUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + configuration_check(); +} + +/** + * @} + * @} + */ From d6ede78fd7de74e52acc61d4bd02fed7a36c7f66 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 16 Feb 2014 13:52:04 +0100 Subject: [PATCH 04/18] dummy commit - renamed file back --- flight/modules/ManualControl/{control.c => manualcontrol.c} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename flight/modules/ManualControl/{control.c => manualcontrol.c} (100%) diff --git a/flight/modules/ManualControl/control.c b/flight/modules/ManualControl/manualcontrol.c similarity index 100% rename from flight/modules/ManualControl/control.c rename to flight/modules/ManualControl/manualcontrol.c From 1898d0ad3568836c1a296520f59a71fbe4a8e2b0 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 16 Feb 2014 15:09:08 +0100 Subject: [PATCH 05/18] OP-1217 create new receiver module This module includes all the receiver IO and parsing/failsafe functionality of ManualControl, but does none of the interpretations in regards to flight mode / actuator control or arming that was included in the old ManualControl In a second step the old ManualControl module needs to be adapted to do only these ommitted things in a modular way - as a delayed callback --- .../fixedwingpathfollower.c | 1 - flight/modules/Receiver/receiver.c | 1160 ++++------------- .../boards/revolution/firmware/Makefile | 1 + .../manualcontrolsettings.xml | 2 +- shared/uavobjectdefinition/taskinfo.xml | 3 + 5 files changed, 270 insertions(+), 897 deletions(-) diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c index 604d09698..64edbe53d 100644 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -49,7 +49,6 @@ #include "attitudestate.h" #include "pathdesired.h" // object that will be updated by the module #include "positionstate.h" -#include "manualcontrol.h" #include "flightstatus.h" #include "pathstatus.h" #include "airspeedstate.h" diff --git a/flight/modules/Receiver/receiver.c b/flight/modules/Receiver/receiver.c index 940e5cf5c..8182e3fb1 100644 --- a/flight/modules/Receiver/receiver.c +++ b/flight/modules/Receiver/receiver.c @@ -2,17 +2,16 @@ ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules * @{ - * @addtogroup ManualControlModule Manual Control Module + * @addtogroup ReceiverModule 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) + * Reads in the ManualControlCommand from receiver then + * pass it to ManualControl * - * @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. + * @file receiver.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Receiver module. Handles safety R/C link and flight mode. * * @see The GNU Public License (GPL) Version 3 * @@ -35,23 +34,14 @@ #include #include -#include "accessorydesired.h" -#include "actuatordesired.h" -#include "altitudeholddesired.h" -#include "flighttelemetrystats.h" -#include "flightstatus.h" -#include "sanitycheck.h" -#include "manualcontrol.h" -#include "manualcontrolsettings.h" -#include "manualcontrolcommand.h" -#include "flightmodesettings.h" -#include "positionstate.h" -#include "pathdesired.h" -#include "stabilizationbank.h" -#include "stabilizationdesired.h" -#include "receiveractivity.h" -#include "systemsettings.h" -#include +#include +#include +#include +#include +#include +#include +#include +#include #include #if defined(PIOS_INCLUDE_USB_RCTX) @@ -59,8 +49,8 @@ #endif /* PIOS_INCLUDE_USB_RCTX */ // Private constants -#if defined(PIOS_MANUAL_STACK_SIZE) -#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE +#if defined(PIOS_RECEIVER_STACK_SIZE) +#define STACK_SIZE_BYTES PIOS_RECEIVER_STACK_SIZE #else #define STACK_SIZE_BYTES 1152 #endif @@ -73,17 +63,9 @@ #define CONNECTION_OFFSET 250 // 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; #ifdef USE_INPUT_LPF @@ -92,20 +74,9 @@ static float inputFiltered[MANUALCONTROLSETTINGS_RESPONSETIME_NUMELEM]; #endif // Private functions -static void updateActuatorDesired(ManualControlCommandData *cmd); -static void updateStabilizationDesired(ManualControlCommandData *cmd, FlightModeSettingsData *settings); -static void updateLandDesired(ManualControlCommandData *cmd, bool changed); -static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed); -static void updatePathDesired(ManualControlCommandData *cmd, bool changed, bool home); -static void processFlightMode(ManualControlSettingsData *settings, FlightModeSettingsData *modeSettings, float flightMode, ManualControlCommandData *cmd); -static void processArm(ManualControlCommandData *cmd, FlightModeSettingsData *settings, int8_t armSwitch); -static void setArmedIfChanged(uint8_t val); -static void configurationUpdatedCb(UAVObjEvent *ev); - -static void manualControlTask(void *parameters); +static void receiverTask(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); static void applyDeadband(float *value, float deadband); @@ -125,16 +96,22 @@ static struct rcvr_activity_fsm activity_fsm; static void resetRcvrActivity(struct rcvr_activity_fsm *fsm); static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm); -#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions_flightmode && assumptions_channelcount) +#define assumptions \ + ( \ + ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM) && \ + ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNUMBER_NUMELEM) && \ + ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMIN_NUMELEM) && \ + ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMAX_NUMELEM) && \ + ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNEUTRAL_NUMELEM)) /** * Module starting */ -int32_t ManualControlStart() +int32_t ReceiverStart() { // Start main task - xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_MANUALCONTROL, taskHandle); + xTaskCreate(receiverTask, (signed char *)"Receiver", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RECEIVER, taskHandle); #ifdef PIOS_INCLUDE_WDG PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL); #endif @@ -145,35 +122,27 @@ int32_t ManualControlStart() /** * Module initialization */ -int32_t ManualControlInitialize() +int32_t ReceiverInitialize() { /* Check the assumptions about uavobject enum's are correct */ - if (!assumptions) { - return -1; - } + PIOS_STATIC_ASSERT(assumptions); - AccessoryDesiredInitialize(); ManualControlCommandInitialize(); - FlightStatusInitialize(); - StabilizationDesiredInitialize(); ReceiverActivityInitialize(); ManualControlSettingsInitialize(); - FlightModeSettingsInitialize(); return 0; } -MODULE_INITCALL(ManualControlInitialize, ManualControlStart); +MODULE_INITCALL(ReceiverInitialize, ReceiverStart); /** * Module task */ -static void manualControlTask(__attribute__((unused)) void *parameters) +static void receiverTask(__attribute__((unused)) void *parameters) { ManualControlSettingsData settings; - FlightModeSettingsData modeSettings; ManualControlCommandData cmd; FlightStatusData flightStatus; - float flightMode = 0; uint8_t disconnected_count = 0; uint8_t connected_count = 0; @@ -183,20 +152,10 @@ static void manualControlTask(__attribute__((unused)) void *parameters) AccessoryDesiredCreateInstance(); AccessoryDesiredCreateInstance(); - // Run this initially to make sure the configuration is checked - configuration_check(); - - // Whenever the configuration changes, make sure it is safe to fly - SystemSettingsConnectCallback(configurationUpdatedCb); - ManualControlSettingsConnectCallback(configurationUpdatedCb); - // Whenever the configuration changes, make sure it is safe to fly - // Make sure unarmed on power up ManualControlCommandGet(&cmd); FlightStatusGet(&flightStatus); - flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED; - armState = ARM_STATE_DISARMED; /* Initialize the RcvrActivty FSM */ portTickType lastActivityTime = xTaskGetTickCount(); @@ -217,11 +176,10 @@ static void manualControlTask(__attribute__((unused)) void *parameters) // Read settings ManualControlSettingsGet(&settings); - FlightModeSettingsGet(&modeSettings); SystemSettingsThrustControlGet(&thrustType); /* Update channel activity monitor */ - if (flightStatus.Armed == ARM_STATE_DISARMED) { + if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) { if (updateRcvrActivity(&activity_fsm)) { /* Reset the aging timer because activity was detected */ lastActivityTime = lastSysTime; @@ -244,301 +202,252 @@ static void manualControlTask(__attribute__((unused)) void *parameters) } } - if (!ManualControlCommandReadOnly()) { - bool valid_input_detected = true; + bool valid_input_detected = true; - // Read channel values in us - for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { - extern uint32_t pios_rcvr_group_map[]; + // Read channel values in us + for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { + extern uint32_t pios_rcvr_group_map[]; - if (cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Roll)[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - cmd.Channel[n] = PIOS_RCVR_INVALID; - } else { - cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[ - cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Pitch)[n]], - cast_struct_to_array(settings.ChannelNumber, settings.ChannelNumber.Pitch)[n]); - } - - // If a channel has timed out this is not valid data and we shouldn't update anything - // until we decide to go to failsafe - if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT) { - valid_input_detected = false; - } else { - scaledChannel[n] = scaleChannel(cmd.Channel[n], - cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Pitch)[n], - cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Pitch)[n], - cast_struct_to_array(settings.ChannelNeutral, settings.ChannelNeutral.Pitch)[n]); - } + if (cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Roll)[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + cmd.Channel[n] = PIOS_RCVR_INVALID; + } else { + cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[ + cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Pitch)[n]], + cast_struct_to_array(settings.ChannelNumber, settings.ChannelNumber.Pitch)[n]); } - // Check settings, if error raise alarm - if (settings.ChannelGroups.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || settings.ChannelGroups.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || settings.ChannelGroups.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || settings.ChannelGroups.Throttle >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || - // Check all channel mappings are valid - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_INVALID - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_INVALID - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_INVALID - || - // Check the driver exists - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_NODRIVER - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_NODRIVER - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_NODRIVER - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_NODRIVER || - // Check the FlightModeNumber is valid - settings.FlightModeNumber < 1 || settings.FlightModeNumber > FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM || - // Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care - ((settings.FlightModeNumber > 1) - && (settings.ChannelGroups.FlightMode >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_INVALID - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_NODRIVER))) { - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); - cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; - ManualControlCommandSet(&cmd); - - // Need to do this here since we don't process armed status. Since this shouldn't happen in flight (changed config) - // immediately disarm - setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); - - continue; + // If a channel has timed out this is not valid data and we shouldn't update anything + // until we decide to go to failsafe + if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT) { + valid_input_detected = false; + } else { + scaledChannel[n] = scaleChannel(cmd.Channel[n], + cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Pitch)[n], + cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Pitch)[n], + cast_struct_to_array(settings.ChannelNeutral, settings.ChannelNeutral.Pitch)[n]); } - - // decide if we have valid manual input or not - valid_input_detected &= validInputRange(settings.ChannelMin.Throttle, - settings.ChannelMax.Throttle, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) - && validInputRange(settings.ChannelMin.Roll, - settings.ChannelMax.Roll, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) - && validInputRange(settings.ChannelMin.Yaw, - settings.ChannelMax.Yaw, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) - && validInputRange(settings.ChannelMin.Pitch, - settings.ChannelMax.Pitch, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_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; - } - - int8_t armSwitch = 0; - if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { - cmd.Throttle = settings.FailsafeChannel.Throttle; - cmd.Roll = settings.FailsafeChannel.Roll; - cmd.Pitch = settings.FailsafeChannel.Pitch; - cmd.Yaw = settings.FailsafeChannel.Yaw; - cmd.Collective = settings.FailsafeChannel.Collective; - switch (thrustType) { - case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE: - cmd.Thrust = cmd.Throttle; - break; - case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE: - cmd.Thrust = cmd.Collective; - break; - default: - break; - } - if (settings.FailsafeFlightModeSwitchPosition >= 0 && settings.FailsafeFlightModeSwitchPosition < settings.FlightModeNumber) { - FlightStatusGet(&flightStatus); - - cmd.FlightModeSwitchPosition = (uint8_t)settings.FailsafeFlightModeSwitchPosition; - flightStatus.FlightMode = modeSettings.FlightModePosition[settings.FailsafeFlightModeSwitchPosition]; - FlightStatusSet(&flightStatus); - } - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - - AccessoryDesiredData accessory; - // Set Accessory 0 - if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - accessory.AccessoryVal = settings.FailsafeChannel.Accessory0; - if (AccessoryDesiredInstSet(0, &accessory) != 0) { - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - } - } - // Set Accessory 1 - if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - accessory.AccessoryVal = settings.FailsafeChannel.Accessory1; - if (AccessoryDesiredInstSet(1, &accessory) != 0) { - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - } - } - // Set Accessory 2 - if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - accessory.AccessoryVal = settings.FailsafeChannel.Accessory2; - if (AccessoryDesiredInstSet(2, &accessory) != 0) { - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - } - } - } else if (valid_input_detected) { - AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); - - // Scale channels to -1 -> +1 range - cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]; - cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]; - cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]; - cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]; - flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE]; - - // Apply deadband for Roll/Pitch/Yaw stick inputs - if (settings.Deadband > 0.0f) { - applyDeadband(&cmd.Roll, settings.Deadband); - applyDeadband(&cmd.Pitch, settings.Deadband); - applyDeadband(&cmd.Yaw, settings.Deadband); - } -#ifdef USE_INPUT_LPF - // Apply Low Pass Filter to input channels, time delta between calls in ms - portTickType thisSysTime = xTaskGetTickCount(); - float dT = (thisSysTime > lastSysTimeLPF) ? - (float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) : - (float)UPDATE_PERIOD_MS; - lastSysTimeLPF = thisSysTime; - - applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings, dT); - applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings, dT); - applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings, dT); -#endif // USE_INPUT_LPF - if (cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_INVALID - && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_NODRIVER - && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_TIMEOUT) { - cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE]; - } - - switch (thrustType) { - case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE: - cmd.Thrust = cmd.Throttle; - break; - case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE: - cmd.Thrust = cmd.Collective; - break; - default: - break; - } - - AccessoryDesiredData accessory; - // Set Accessory 0 - if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0]; -#ifdef USE_INPUT_LPF - applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT); -#endif - if (modeSettings.Arming == FLIGHTMODESETTINGS_ARMING_ACCESSORY0) { - if (accessory.AccessoryVal > ARMED_THRESHOLD) { - armSwitch = 1; - } else if (accessory.AccessoryVal < -ARMED_THRESHOLD) { - armSwitch = -1; - } - } - if (AccessoryDesiredInstSet(0, &accessory) != 0) { - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - } - } - // Set Accessory 1 - if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1]; -#ifdef USE_INPUT_LPF - applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT); -#endif - if (modeSettings.Arming == FLIGHTMODESETTINGS_ARMING_ACCESSORY1) { - if (accessory.AccessoryVal > ARMED_THRESHOLD) { - armSwitch = 1; - } else if (accessory.AccessoryVal < -ARMED_THRESHOLD) { - armSwitch = -1; - } - } - if (AccessoryDesiredInstSet(1, &accessory) != 0) { - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - } - } - // Set Accessory 2 - if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2]; -#ifdef USE_INPUT_LPF - applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT); -#endif - if (modeSettings.Arming == FLIGHTMODESETTINGS_ARMING_ACCESSORY2) { - if (accessory.AccessoryVal > ARMED_THRESHOLD) { - armSwitch = 1; - } else if (accessory.AccessoryVal < -ARMED_THRESHOLD) { - armSwitch = -1; - } - } - - if (AccessoryDesiredInstSet(2, &accessory) != 0) { - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - } - } - - processFlightMode(&settings, &modeSettings, flightMode, &cmd); - } - - // Process arming outside conditional so system will disarm when disconnected - processArm(&cmd, &modeSettings, armSwitch); - - // Update cmd object - ManualControlCommandSet(&cmd); -#if defined(PIOS_INCLUDE_USB_RCTX) - if (pios_usb_rctx_id) { - PIOS_USB_RCTX_Update(pios_usb_rctx_id, - cmd.Channel, - cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Roll), - cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Roll), - NELEMENTS(cmd.Channel)); - } -#endif /* PIOS_INCLUDE_USB_RCTX */ - } else { - ManualControlCommandGet(&cmd); /* Under GCS control */ } - FlightStatusGet(&flightStatus); - - // Depending on the mode update the Stabilization or Actuator objects - static uint8_t lastFlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL; - switch (PARSE_FLIGHT_MODE(flightStatus.FlightMode)) { - case FLIGHTMODE_UNDEFINED: - // This reflects a bug in the code architecture! + // Check settings, if error raise alarm + if (settings.ChannelGroups.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups.Throttle >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || + // Check all channel mappings are valid + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_INVALID + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_INVALID + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_INVALID + || + // Check the driver exists + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_NODRIVER + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_NODRIVER + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_NODRIVER + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_NODRIVER + || + // Check collective if required + (thrustType == SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE && ( + settings.ChannelGroups.Collective >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] == (uint16_t)PIOS_RCVR_INVALID + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] == (uint16_t)PIOS_RCVR_NODRIVER)) + || + // Check the FlightModeNumber is valid + settings.FlightModeNumber < 1 || settings.FlightModeNumber > FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM + || + // Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care + ((settings.FlightModeNumber > 1) + && (settings.ChannelGroups.FlightMode >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_INVALID + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_NODRIVER))) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); - break; - case FLIGHTMODE_MANUAL: - updateActuatorDesired(&cmd); - break; - case FLIGHTMODE_STABILIZED: - updateStabilizationDesired(&cmd, &modeSettings); - break; - case FLIGHTMODE_TUNING: - // Tuning takes settings directly from manualcontrolcommand. No need to - // call anything else. This just avoids errors. - break; - case FLIGHTMODE_GUIDANCE: - switch (flightStatus.FlightMode) { - case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: - case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO: - altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode); + cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; + ManualControlCommandSet(&cmd); + + continue; + } + + // decide if we have valid manual input or not + valid_input_detected &= validInputRange(settings.ChannelMin.Throttle, + settings.ChannelMax.Throttle, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) + && validInputRange(settings.ChannelMin.Roll, + settings.ChannelMax.Roll, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) + && validInputRange(settings.ChannelMin.Yaw, + settings.ChannelMax.Yaw, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) + && validInputRange(settings.ChannelMin.Pitch, + settings.ChannelMax.Pitch, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); + + if (settings.ChannelGroups.Collective != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + valid_input_detected &= validInputRange(settings.ChannelMin.Collective, + settings.ChannelMax.Collective, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE]); + } + if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + valid_input_detected &= validInputRange(settings.ChannelMin.Accessory0, + settings.ChannelMax.Accessory0, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0]); + } + if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + valid_input_detected &= validInputRange(settings.ChannelMin.Accessory1, + settings.ChannelMax.Accessory1, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1]); + } + if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + valid_input_detected &= validInputRange(settings.ChannelMin.Accessory2, + settings.ChannelMax.Accessory2, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2]); + } + + // 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 = settings.FailsafeChannel.Throttle; + cmd.Roll = settings.FailsafeChannel.Roll; + cmd.Pitch = settings.FailsafeChannel.Pitch; + cmd.Yaw = settings.FailsafeChannel.Yaw; + cmd.Collective = settings.FailsafeChannel.Collective; + switch (thrustType) { + case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE: + cmd.Thrust = cmd.Throttle; break; - case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: - case FLIGHTSTATUS_FLIGHTMODE_POI: - updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, false); - break; - case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: - updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, true); - break; - case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: - // No need to call anything. This just avoids errors. - break; - case FLIGHTSTATUS_FLIGHTMODE_LAND: - updateLandDesired(&cmd, lastFlightMode != flightStatus.FlightMode); + case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE: + cmd.Thrust = cmd.Collective; break; default: - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); + break; + } + if (settings.FailsafeFlightModeSwitchPosition >= 0 && settings.FailsafeFlightModeSwitchPosition < settings.FlightModeNumber) { + cmd.FlightModeSwitchPosition = (uint8_t)settings.FailsafeFlightModeSwitchPosition; + } + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + + AccessoryDesiredData accessory; + // Set Accessory 0 + if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = settings.FailsafeChannel.Accessory0; + if (AccessoryDesiredInstSet(0, &accessory) != 0) { + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + } + // Set Accessory 1 + if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = settings.FailsafeChannel.Accessory1; + if (AccessoryDesiredInstSet(1, &accessory) != 0) { + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + } + // Set Accessory 2 + if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = settings.FailsafeChannel.Accessory2; + if (AccessoryDesiredInstSet(2, &accessory) != 0) { + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + } + } else if (valid_input_detected) { + AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); + + // Scale channels to -1 -> +1 range + cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]; + cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]; + cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]; + cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]; + // Convert flightMode value into the switch position in the range [0..N-1] + cmd.FlightModeSwitchPosition = ((int16_t)(scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] * 256.0f) + 256) * settings.FlightModeNumber >> 9; + if (cmd.FlightModeSwitchPosition >= settings.FlightModeNumber) { + cmd.FlightModeSwitchPosition = settings.FlightModeNumber - 1; + } + + // Apply deadband for Roll/Pitch/Yaw stick inputs + if (settings.Deadband > 0.0f) { + applyDeadband(&cmd.Roll, settings.Deadband); + applyDeadband(&cmd.Pitch, settings.Deadband); + applyDeadband(&cmd.Yaw, settings.Deadband); + } +#ifdef USE_INPUT_LPF + // Apply Low Pass Filter to input channels, time delta between calls in ms + portTickType thisSysTime = xTaskGetTickCount(); + float dT = (thisSysTime > lastSysTimeLPF) ? + (float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) : + (float)UPDATE_PERIOD_MS; + lastSysTimeLPF = thisSysTime; + + applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings, dT); + applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings, dT); + applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings, dT); +#endif // USE_INPUT_LPF + if (cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_INVALID + && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_NODRIVER + && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_TIMEOUT) { + cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE]; + if (settings.Deadband > 0.0f) { + applyDeadband(&cmd.Collective, settings.Deadband); + } +#ifdef USE_INPUT_LPF + applyLPF(&cmd.Collective, MANUALCONTROLSETTINGS_RESPONSETIME_COLLECTIVE, &settings, dT); +#endif // USE_INPUT_LPF + } + + switch (thrustType) { + case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE: + cmd.Thrust = cmd.Throttle; + break; + case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE: + cmd.Thrust = cmd.Collective; + break; + default: + break; + } + + AccessoryDesiredData accessory; + // Set Accessory 0 + if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0]; +#ifdef USE_INPUT_LPF + applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT); +#endif + if (AccessoryDesiredInstSet(0, &accessory) != 0) { + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + } + // Set Accessory 1 + if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1]; +#ifdef USE_INPUT_LPF + applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT); +#endif + if (AccessoryDesiredInstSet(1, &accessory) != 0) { + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + } + // Set Accessory 2 + if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2]; +#ifdef USE_INPUT_LPF + applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT); +#endif + + if (AccessoryDesiredInstSet(2, &accessory) != 0) { + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } } - break; } - lastFlightMode = flightStatus.FlightMode; + + // Update cmd object + ManualControlCommandSet(&cmd); +#if defined(PIOS_INCLUDE_USB_RCTX) + if (pios_usb_rctx_id) { + PIOS_USB_RCTX_Update(pios_usb_rctx_id, + cmd.Channel, + cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Roll), + cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Roll), + NELEMENTS(cmd.Channel)); + } +#endif /* PIOS_INCLUDE_USB_RCTX */ } } @@ -678,283 +587,6 @@ group_completed: return activity_updated; } -static void updateActuatorDesired(ManualControlCommandData *cmd) -{ - ActuatorDesiredData actuator; - - ActuatorDesiredGet(&actuator); - actuator.Roll = cmd->Roll; - actuator.Pitch = cmd->Pitch; - actuator.Yaw = cmd->Yaw; - actuator.Thrust = cmd->Thrust; - ActuatorDesiredSet(&actuator); -} - -static void updateStabilizationDesired(ManualControlCommandData *cmd, FlightModeSettingsData *settings) -{ - StabilizationDesiredData stabilization; - - StabilizationDesiredGet(&stabilization); - - StabilizationBankData stabSettings; - StabilizationBankGet(&stabSettings); - - uint8_t *stab_settings; - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - switch (flightStatus.FlightMode) { - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: - stab_settings = cast_struct_to_array(settings->Stabilization1Settings, settings->Stabilization1Settings.Roll); - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: - stab_settings = cast_struct_to_array(settings->Stabilization2Settings, settings->Stabilization2Settings.Roll); - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: - stab_settings = cast_struct_to_array(settings->Stabilization3Settings, settings->Stabilization3Settings.Roll); - 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; - } - - stabilization.Roll = - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate.Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate.Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? 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.Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate.Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate.Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : - 0; // this is an invalid mode - - // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order - stabilization.StabilizationMode.Roll = stab_settings[0]; - stabilization.StabilizationMode.Pitch = stab_settings[1]; - // Other axes (yaw) cannot be Rattitude, so use Rate - // Should really do this for Attitude mode as well? - if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) { - stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; - stabilization.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw; - } else { - stabilization.StabilizationMode.Yaw = stab_settings[2]; - stabilization.Yaw = - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate.Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : - 0; // this is an invalid mode - } - - stabilization.Thrust = cmd->Thrust; - StabilizationDesiredSet(&stabilization); -} - -#if defined(REVOLUTION) -// TODO: Need compile flag to exclude this from copter control -/** - * @brief Update the position desired to current location when - * enabled and allow the waypoint to be moved by transmitter - */ -static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *cmd, bool changed, bool home) -{ - /* - static portTickType lastSysTime; - portTickType thisSysTime = xTaskGetTickCount(); - dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f); - lastSysTime = thisSysTime; - */ - - if (home && changed) { - // Simple Return To Base mode - keep altitude the same, fly to home position - PositionStateData positionState; - PositionStateGet(&positionState); - FlightModeSettingsData settings; - FlightModeSettingsGet(&settings); - - PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); - pathDesired.Start.North = 0; - pathDesired.Start.East = 0; - pathDesired.Start.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; - pathDesired.End.North = 0; - pathDesired.End.East = 0; - pathDesired.End.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; - pathDesired.StartingVelocity = 1; - pathDesired.EndingVelocity = 0; - pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; - PathDesiredSet(&pathDesired); - } else if (changed) { - // After not being in this mode for a while init at current height - PositionStateData positionState; - PositionStateGet(&positionState); - - PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); - pathDesired.Start.North = positionState.North; - pathDesired.Start.East = positionState.East; - pathDesired.Start.Down = positionState.Down; - pathDesired.End.North = positionState.North; - pathDesired.End.East = positionState.East; - pathDesired.End.Down = positionState.Down; - pathDesired.StartingVelocity = 1; - pathDesired.EndingVelocity = 0; - pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; - PathDesiredSet(&pathDesired); - /* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts. - } else { - PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); - pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch; - pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll; - pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; - PathDesiredSet(&pathDesired); - */ - } -} - -static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *cmd, bool changed) -{ - /* - static portTickType lastSysTime; - portTickType thisSysTime; - float dT; - - thisSysTime = xTaskGetTickCount(); - dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f); - lastSysTime = thisSysTime; - */ - - PositionStateData positionState; - - PositionStateGet(&positionState); - - PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); - if (changed) { - // After not being in this mode for a while init at current height - pathDesired.Start.North = positionState.North; - pathDesired.Start.East = positionState.East; - pathDesired.Start.Down = positionState.Down; - pathDesired.End.North = positionState.North; - pathDesired.End.East = positionState.East; - pathDesired.End.Down = positionState.Down; - pathDesired.StartingVelocity = 1; - pathDesired.EndingVelocity = 0; - pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; - } - pathDesired.End.Down = positionState.Down + 5; - PathDesiredSet(&pathDesired); -} - -/** - * @brief Update the altitude desired to current altitude when - * enabled and enable altitude mode for stabilization - * @todo: Need compile flag to exclude this from copter control - */ -static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) -{ - const float DEADBAND = 0.20f; - const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2; - const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2; - - // this is the max speed in m/s at the extents of thrust - float thrustRate; - uint8_t thrustExp; - - static uint8_t flightMode; - static bool newaltitude = true; - - FlightStatusFlightModeGet(&flightMode); - - AltitudeHoldDesiredData altitudeHoldDesiredData; - AltitudeHoldDesiredGet(&altitudeHoldDesiredData); - - AltitudeHoldSettingsThrustExpGet(&thrustExp); - AltitudeHoldSettingsThrustRateGet(&thrustRate); - - StabilizationBankData stabSettings; - StabilizationBankGet(&stabSettings); - - PositionStateData posState; - PositionStateGet(&posState); - - altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; - altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax; - altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw; - - if (changed) { - newaltitude = true; - } - - uint8_t cutOff; - AltitudeHoldSettingsCutThrustWhenZeroGet(&cutOff); - if (cutOff && cmd->Thrust < 0) { - // Cut thrust if desired - altitudeHoldDesiredData.SetPoint = cmd->Thrust; - altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_THRUST; - newaltitude = true; - } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Thrust > DEADBAND_HIGH) { - // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 - // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255 - altitudeHoldDesiredData.SetPoint = -((thrustExp * powf((cmd->Thrust - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - thrustExp) * (cmd->Thrust - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * thrustRate); - altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY; - newaltitude = true; - } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Thrust < DEADBAND_LOW) { - altitudeHoldDesiredData.SetPoint = -(-(thrustExp * powf((DEADBAND_LOW - (cmd->Thrust < 0 ? 0 : cmd->Thrust)) / DEADBAND_LOW, 3) + (255 - thrustExp) * (DEADBAND_LOW - cmd->Thrust) / DEADBAND_LOW) / 255 * thrustRate); - altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY; - newaltitude = true; - } else if (newaltitude == true) { - altitudeHoldDesiredData.SetPoint = posState.Down; - altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE; - newaltitude = false; - } - - AltitudeHoldDesiredSet(&altitudeHoldDesiredData); -} -#else /* if defined(REVOLUTION) */ - -// TODO: These functions should never be accessible on CC. Any configuration that -// could allow them to be called should already throw an error to prevent this happening -// in flight -static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *cmd, - __attribute__((unused)) bool changed, - __attribute__((unused)) bool home) -{ - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); -} - -static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *cmd, - __attribute__((unused)) bool changed) -{ - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); -} - -static void altitudeHoldDesired(__attribute__((unused)) ManualControlCommandData *cmd, - __attribute__((unused)) bool changed) -{ - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); -} -#endif /* if defined(REVOLUTION) */ /** * Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range. */ @@ -992,261 +624,6 @@ static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) return (end_time - start_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) -{ - // update checks - configuration_check(); - - // read alarms - SystemAlarmsData alarms; - - SystemAlarmsGet(&alarms); - - // Check each alarm - for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) { - if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set - if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) { - continue; - } - - return false; - } - } - - uint8_t flightMode; - FlightStatusFlightModeGet(&flightMode); - switch (flightMode) { - case FLIGHTSTATUS_FLIGHTMODE_MANUAL: - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: - return true; - - default: - return false; - } -} -/** - * @brief Determine if the aircraft is forced to disarm by an explicit alarm - * @returns True if safe to arm, false otherwise - */ -static bool forcedDisArm(void) -{ - // read alarms - SystemAlarmsData alarms; - - SystemAlarmsGet(&alarms); - - if (alarms.Alarm.Guidance == SYSTEMALARMS_ALARM_CRITICAL) { - return true; - } - return false; -} - -/** - * @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, FlightModeSettingsData *settings, int8_t armSwitch) -{ - bool lowThrottle = cmd->Throttle < 0; - - /** - * do NOT check throttle if disarming via switch, must be instant - */ - switch (settings->Arming) { - case FLIGHTMODESETTINGS_ARMING_ACCESSORY0: - case FLIGHTMODESETTINGS_ARMING_ACCESSORY1: - case FLIGHTMODESETTINGS_ARMING_ACCESSORY2: - if (armSwitch < 0) { - lowThrottle = true; - } - break; - default: - break; - } - - 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); - } else { - // Not really needed since this function not called when disconnected - if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { - lowThrottle = true; - } - - // 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 * (float)armSwitch; - 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) > 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 = 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) > settings->DisarmingSequenceTime)) { - armState = ARM_STATE_DISARMED; - } else if (!manualDisarm) { - armState = ARM_STATE_ARMED; - } - break; - } // End Switch - } -} - -/** - * @brief Determine which of N 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, FlightModeSettingsData *modeSettings, float flightMode, ManualControlCommandData *cmd) -{ - FlightStatusData flightStatus; - - FlightStatusGet(&flightStatus); - - // Convert flightMode value into the switch position in the range [0..N-1] - uint8_t pos = ((int16_t)(flightMode * 256.0f) + 256) * settings->FlightModeNumber >> 9; - if (pos >= settings->FlightModeNumber) { - pos = settings->FlightModeNumber - 1; - } - - cmd->FlightModeSwitchPosition = pos; - - uint8_t newMode = modeSettings->FlightModePosition[pos]; - - if (flightStatus.FlightMode != newMode) { - flightStatus.FlightMode = newMode; - FlightStatusSet(&flightStatus); - } -} /** * @brief Determine if the manual input value is within acceptable limits @@ -1289,13 +666,6 @@ static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel } } #endif // USE_INPUT_LPF -/** - * Called whenever a critical configuration component changes - */ -static void configurationUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) -{ - configuration_check(); -} /** * @} diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index 3202b2e1e..025ae2470 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -37,6 +37,7 @@ MODULES += AltitudeHold MODULES += Stabilization MODULES += VtolPathFollower MODULES += ManualControl +MODULES += Receiver MODULES += Actuator MODULES += GPS MODULES += TxPID diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index d4fbe858b..acdb3a5dc 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -13,7 +13,7 @@ + elementnames="Roll,Pitch,Yaw,Collective,Accessory0,Accessory1,Accessory2"/> diff --git a/shared/uavobjectdefinition/taskinfo.xml b/shared/uavobjectdefinition/taskinfo.xml index c5ddc5058..981054a2c 100644 --- a/shared/uavobjectdefinition/taskinfo.xml +++ b/shared/uavobjectdefinition/taskinfo.xml @@ -11,6 +11,7 @@ CallbackScheduler3 ManualControl + Receiver Stabilization Actuator Sensors @@ -45,6 +46,7 @@ CallbackScheduler3 ManualControl + Receiver Stabilization Actuator Sensors @@ -83,6 +85,7 @@ CallbackScheduler3 ManualControl + Receiver Stabilization Actuator Sensors From 7dd911efdbd09574d3a6ad5b2ecdc4bd44af0e6f Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 16 Feb 2014 19:07:31 +0100 Subject: [PATCH 06/18] OP-1217 change the way modules are enabled/disabled based on flightstatus --- .../fixedwingpathfollower.c | 78 ++++++++------- flight/modules/PathPlanner/pathplanner.c | 2 +- flight/modules/Stabilization/stabilization.c | 3 +- .../VtolPathFollower/vtolpathfollower.c | 94 +++++++++---------- shared/uavobjectdefinition/flightstatus.xml | 8 ++ 5 files changed, 92 insertions(+), 93 deletions(-) diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c index 64edbe53d..cb602536e 100644 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -181,52 +181,50 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters) uint8_t result; // Check the combinations of flightmode and pathdesired mode - switch (flightStatus.FlightMode) { - case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: - case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: - if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { - updatePathVelocity(); - result = updateFixedDesiredAttitude(); - if (result) { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + if (flightStatus.ControlChain.PathFollower == FLIGHTSTATUS_CONTROLCHAIN_TRUE) { + if (flightStatus.ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_FALSE) { + if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { + updatePathVelocity(); + result = updateFixedDesiredAttitude(); + if (result) { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + } else { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); + } } else { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); } } else { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); - } - break; - case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: - pathStatus.UID = pathDesired.UID; - pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; - switch (pathDesired.Mode) { - case PATHDESIRED_MODE_FLYENDPOINT: - case PATHDESIRED_MODE_FLYVECTOR: - case PATHDESIRED_MODE_FLYCIRCLERIGHT: - case PATHDESIRED_MODE_FLYCIRCLELEFT: - updatePathVelocity(); - result = updateFixedDesiredAttitude(); - if (result) { + pathStatus.UID = pathDesired.UID; + pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; + switch (pathDesired.Mode) { + case PATHDESIRED_MODE_FLYENDPOINT: + case PATHDESIRED_MODE_FLYVECTOR: + case PATHDESIRED_MODE_FLYCIRCLERIGHT: + case PATHDESIRED_MODE_FLYCIRCLELEFT: + updatePathVelocity(); + result = updateFixedDesiredAttitude(); + if (result) { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + } else { + pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); + } + break; + case PATHDESIRED_MODE_FIXEDATTITUDE: + updateFixedAttitude(pathDesired.ModeParameters); AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); - } else { + break; + case PATHDESIRED_MODE_DISARMALARM: + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); + break; + default: pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); + break; } - break; - case PATHDESIRED_MODE_FIXEDATTITUDE: - updateFixedAttitude(pathDesired.ModeParameters); - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); - break; - case PATHDESIRED_MODE_DISARMALARM: - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); - break; - default: - pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); - break; } - break; - default: + } else { // Be cleaner and get rid of global variables northVelIntegral = 0; eastVelIntegral = 0; @@ -234,8 +232,6 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters) courseIntegral = 0; speedIntegral = 0; powerIntegral = 0; - - break; } PathStatusSet(&pathStatus); } diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index eba9e2092..364ee2480 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -138,7 +138,7 @@ static void pathPlannerTask() FlightStatusData flightStatus; FlightStatusGet(&flightStatus); - if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) { + if (flightStatus.ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE) { pathplanner_active = false; if (!validPathPlan) { // unverified path plans are only a warning while we are not in pathplanner mode diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 354bf9eef..db56e9de8 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -50,7 +50,6 @@ #include "flightstatus.h" #include "manualcontrolsettings.h" #include "flightmodesettings.h" -#include "manualcontrol.h" // Just to get a macro #include "taskinfo.h" // Math libraries @@ -669,7 +668,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) } } - if (PARSE_FLIGHT_MODE(flightStatus.FlightMode) != FLIGHTMODE_MANUAL) { + if (flightStatus.ControlChain.Stabilization == FLIGHTSTATUS_CONTROLCHAIN_TRUE) { ActuatorDesiredSet(&actuatorDesired); } else { // Force all axes to reinitialize when engaged diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index 101dbec2d..9efcffe3e 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -206,55 +206,53 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters) PathDesiredGet(&pathDesired); // Check the combinations of flightmode and pathdesired mode - switch (flightStatus.FlightMode) { - case FLIGHTSTATUS_FLIGHTMODE_LAND: - case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: - case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: - if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { - updateEndpointVelocity(); - updateVtolDesiredAttitude(false); - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + if (flightStatus.ControlChain.PathFollower == FLIGHTSTATUS_CONTROLCHAIN_TRUE) { + if (flightStatus.ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_FALSE) { + if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POI) { + if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { + updateEndpointVelocity(); + updateVtolDesiredAttitude(true); + updatePOIBearing(); + } else { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); + } + } else { + if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { + updateEndpointVelocity(); + updateVtolDesiredAttitude(false); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + } else { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); + } + } } else { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); + pathStatus.UID = pathDesired.UID; + pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; + switch (pathDesired.Mode) { + // TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly + case PATHDESIRED_MODE_FLYENDPOINT: + case PATHDESIRED_MODE_FLYVECTOR: + case PATHDESIRED_MODE_FLYCIRCLERIGHT: + case PATHDESIRED_MODE_FLYCIRCLELEFT: + updatePathVelocity(); + updateVtolDesiredAttitude(false); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + break; + case PATHDESIRED_MODE_FIXEDATTITUDE: + updateFixedAttitude(pathDesired.ModeParameters); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + break; + case PATHDESIRED_MODE_DISARMALARM: + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); + break; + default: + pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); + break; + } + PathStatusSet(&pathStatus); } - break; - case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: - pathStatus.UID = pathDesired.UID; - pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; - switch (pathDesired.Mode) { - // TODO: Make updateVtolDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly - case PATHDESIRED_MODE_FLYENDPOINT: - case PATHDESIRED_MODE_FLYVECTOR: - case PATHDESIRED_MODE_FLYCIRCLERIGHT: - case PATHDESIRED_MODE_FLYCIRCLELEFT: - updatePathVelocity(); - updateVtolDesiredAttitude(false); - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); - break; - case PATHDESIRED_MODE_FIXEDATTITUDE: - updateFixedAttitude(pathDesired.ModeParameters); - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); - break; - case PATHDESIRED_MODE_DISARMALARM: - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); - break; - default: - pathStatus.Status = PATHSTATUS_STATUS_CRITICAL; - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); - break; - } - PathStatusSet(&pathStatus); - break; - case FLIGHTSTATUS_FLIGHTMODE_POI: - if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) { - updateEndpointVelocity(); - updateVtolDesiredAttitude(true); - updatePOIBearing(); - } else { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR); - } - break; - default: + } else { // Be cleaner and get rid of global variables northVelIntegral = 0; eastVelIntegral = 0; @@ -267,8 +265,6 @@ static void vtolPathFollowerTask(__attribute__((unused)) void *parameters) StabilizationDesiredData stabDesired; StabilizationDesiredGet(&stabDesired); thrustOffset = stabDesired.Thrust; - - break; } AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE); diff --git a/shared/uavobjectdefinition/flightstatus.xml b/shared/uavobjectdefinition/flightstatus.xml index 9c9336c1a..a7009f4c4 100644 --- a/shared/uavobjectdefinition/flightstatus.xml +++ b/shared/uavobjectdefinition/flightstatus.xml @@ -6,6 +6,14 @@ + + + Stabilization + PathFollower + PathPlanner + + + From cdca8d835c83f43c67e0597867af1451de1a1151 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 2 Mar 2014 14:45:17 +0100 Subject: [PATCH 07/18] OP-1216 manualcontrol refactoring --- .../modules/ManualControl/altitudehandler.c | 133 ++ flight/modules/ManualControl/armhandler.c | 326 ++++ .../modules/ManualControl/inc/manualcontrol.h | 77 +- flight/modules/ManualControl/manualcontrol.c | 1323 ++--------------- flight/modules/ManualControl/manualhandler.c | 71 + .../ManualControl/pathfollowerhandler.c | 131 ++ .../ManualControl/pathplannerhandler.c | 64 + .../modules/ManualControl/stabilizedhandler.c | 142 ++ flight/modules/Stabilization/stabilization.c | 5 +- .../VtolPathFollower/vtolpathfollower.c | 2 +- .../boards/coptercontrol/firmware/Makefile | 1 + .../boards/revoproto/firmware/Makefile | 1 + shared/uavobjectdefinition/callbackinfo.xml | 3 + 13 files changed, 1057 insertions(+), 1222 deletions(-) create mode 100644 flight/modules/ManualControl/altitudehandler.c create mode 100644 flight/modules/ManualControl/armhandler.c create mode 100644 flight/modules/ManualControl/manualhandler.c create mode 100644 flight/modules/ManualControl/pathfollowerhandler.c create mode 100644 flight/modules/ManualControl/pathplannerhandler.c create mode 100644 flight/modules/ManualControl/stabilizedhandler.c diff --git a/flight/modules/ManualControl/altitudehandler.c b/flight/modules/ManualControl/altitudehandler.c new file mode 100644 index 000000000..451177098 --- /dev/null +++ b/flight/modules/ManualControl/altitudehandler.c @@ -0,0 +1,133 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup ManualControl + * @brief Interpretes the control input in ManualControlCommand + * @{ + * + * @file altitudehandler.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * + * @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 "inc/manualcontrol.h" +#include +#include +#include +#include +#include + +#if defined(REVOLUTION) +// Private constants + +// Private types + +// Private functions + +/** + * @brief Handler to control deprecated flight modes controlled by AltitudeHold module + * @input: ManualControlCommand + * @output: AltitudeHoldDesired + */ +void altitudeHandler(bool newinit) +{ + const float DEADBAND = 0.20f; + const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2; + const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2; + + if (newinit) { + StabilizationBankInitialize(); + AltitudeHoldDesiredInitialize(); + AltitudeHoldSettingsInitialize(); + PositionStateInitialize(); + } + + + // this is the max speed in m/s at the extents of thrust + float thrustRate; + uint8_t thrustExp; + + static uint8_t flightMode; + static bool newaltitude = true; + + ManualControlCommandData cmd; + ManualControlCommandGet(&cmd); + + FlightStatusFlightModeGet(&flightMode); + + AltitudeHoldDesiredData altitudeHoldDesiredData; + AltitudeHoldDesiredGet(&altitudeHoldDesiredData); + + AltitudeHoldSettingsThrustExpGet(&thrustExp); + AltitudeHoldSettingsThrustRateGet(&thrustRate); + + StabilizationBankData stabSettings; + StabilizationBankGet(&stabSettings); + + PositionStateData posState; + PositionStateGet(&posState); + + altitudeHoldDesiredData.Roll = cmd.Roll * stabSettings.RollMax; + altitudeHoldDesiredData.Pitch = cmd.Pitch * stabSettings.PitchMax; + altitudeHoldDesiredData.Yaw = cmd.Yaw * stabSettings.ManualRate.Yaw; + + if (newinit) { + newaltitude = true; + } + + uint8_t cutOff; + AltitudeHoldSettingsCutThrustWhenZeroGet(&cutOff); + if (cutOff && cmd.Thrust < 0) { + // Cut thrust if desired + altitudeHoldDesiredData.SetPoint = cmd.Thrust; + altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_THRUST; + newaltitude = true; + } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd.Thrust > DEADBAND_HIGH) { + // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 + // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255 + altitudeHoldDesiredData.SetPoint = -((thrustExp * powf((cmd.Thrust - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - thrustExp) * (cmd.Thrust - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * thrustRate); + altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY; + newaltitude = true; + } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd.Thrust < DEADBAND_LOW) { + altitudeHoldDesiredData.SetPoint = -(-(thrustExp * powf((DEADBAND_LOW - (cmd.Thrust < 0 ? 0 : cmd.Thrust)) / DEADBAND_LOW, 3) + (255 - thrustExp) * (DEADBAND_LOW - cmd.Thrust) / DEADBAND_LOW) / 255 * thrustRate); + altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY; + newaltitude = true; + } else if (newaltitude == true) { + altitudeHoldDesiredData.SetPoint = posState.Down; + altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE; + newaltitude = false; + } + + AltitudeHoldDesiredSet(&altitudeHoldDesiredData); +} + +#else /* if defined(REVOLUTION) */ +void altitudeHandler(__attribute__((unused)) bool newinit) +{ + PIOS_Assert(0); // should not be called +} +#endif // REVOLUTION + + +/** + * @} + * @} + */ diff --git a/flight/modules/ManualControl/armhandler.c b/flight/modules/ManualControl/armhandler.c new file mode 100644 index 000000000..b7260eaae --- /dev/null +++ b/flight/modules/ManualControl/armhandler.c @@ -0,0 +1,326 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup ManualControl + * @brief Interpretes the control input in ManualControlCommand + * @{ + * + * @file armhandler.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * + * @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 "inc/manualcontrol.h" +#include +#include +#include +#include +#include +#include + +// Private constants +#define ARMED_THRESHOLD 0.50f + +// 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 + +// Private functions +static void setArmedIfChanged(uint8_t val); +static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time); +static bool okToArm(void); +static bool forcedDisArm(void); + + +/** + * @brief Handler to interprete Command inputs in regards to arming/disarming + * @input: ManualControlCommand, AccessoryDesired + * @output: FlightStatus.Arming + */ +void armHandler(bool newinit) +{ + static portTickType lastSysTime; + static ArmState_t armState; + + if (newinit) { + AccessoryDesiredInitialize(); + lastSysTime = xTaskGetTickCount(); + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); + armState = ARM_STATE_DISARMED; + } + + 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(0, &acc); + armSwitch = true; + break; + case FLIGHTMODESETTINGS_ARMING_ACCESSORY2: + AccessoryDesiredInstGet(0, &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 = lastSysTime; + armState = ARM_STATE_ARMING_MANUAL; + } + break; + + case ARM_STATE_ARMING_MANUAL: + setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING); + + if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > 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 = 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: + // arming switch disarms immediately, + if (manualDisarm && (armSwitch || (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings.DisarmingSequenceTime))) { + armState = ARM_STATE_DISARMED; + } else if (!manualDisarm) { + armState = ARM_STATE_ARMED; + } + break; + } // End Switch +} + +static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) +{ + return (end_time - start_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) +{ + // update checks + configuration_check(); + + // read alarms + SystemAlarmsData alarms; + + SystemAlarmsGet(&alarms); + + // Check each alarm + for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) { + if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set + if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) { + continue; + } + + return false; + } + } + + uint8_t flightMode; + FlightStatusFlightModeGet(&flightMode); + switch (flightMode) { + case FLIGHTSTATUS_FLIGHTMODE_MANUAL: + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: + return true; + + break; + + default: + return false; + + break; + } +} +/** + * @brief Determine if the aircraft is forced to disarm by an explicit alarm + * @returns True if safe to arm, false otherwise + */ +static bool forcedDisArm(void) +{ + // read alarms + SystemAlarmsAlarmData alarms; + + SystemAlarmsAlarmGet(&alarms); + + if (alarms.Guidance == SYSTEMALARMS_ALARM_CRITICAL) { + return true; + } + if (alarms.ManualControl == SYSTEMALARMS_ALARM_CRITICAL) { + return true; + } + return false; +} + +/** + * @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); + } +} + +/** + * @} + * @} + */ diff --git a/flight/modules/ManualControl/inc/manualcontrol.h b/flight/modules/ManualControl/inc/manualcontrol.h index ef35f4d26..b5e2c9bf6 100644 --- a/flight/modules/ManualControl/inc/manualcontrol.h +++ b/flight/modules/ManualControl/inc/manualcontrol.h @@ -6,7 +6,7 @@ * @{ * * @file manualcontrol.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. * @brief ManualControl module. Handles safety R/C link and flight mode. * * @see The GNU Public License (GPL) Version 3 @@ -30,30 +30,57 @@ #ifndef MANUALCONTROL_H #define MANUALCONTROL_H -#include "manualcontrolcommand.h" +#include +#include -typedef enum { FLIGHTMODE_UNDEFINED = 0, FLIGHTMODE_MANUAL = 1, FLIGHTMODE_STABILIZED = 2, FLIGHTMODE_GUIDANCE = 3, FLIGHTMODE_TUNING = 4 } flightmode_path; +typedef void (*handlerFunc)(bool newinit); -#define PARSE_FLIGHT_MODE(x) \ - ( \ - (x == FLIGHTSTATUS_FLIGHTMODE_MANUAL) ? FLIGHTMODE_MANUAL : \ - (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) ? FLIGHTMODE_STABILIZED : \ - (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) ? FLIGHTMODE_STABILIZED : \ - (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) ? FLIGHTMODE_STABILIZED : \ - (x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) ? FLIGHTMODE_GUIDANCE : \ - (x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO) ? FLIGHTMODE_GUIDANCE : \ - (x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) ? FLIGHTMODE_GUIDANCE : \ - (x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) ? FLIGHTMODE_GUIDANCE : \ - (x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) ? FLIGHTMODE_GUIDANCE : \ - (x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) ? FLIGHTMODE_GUIDANCE : \ - (x == FLIGHTSTATUS_FLIGHTMODE_LAND) ? FLIGHTMODE_GUIDANCE : \ - (x == FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) ? FLIGHTMODE_TUNING : \ - (x == FLIGHTSTATUS_FLIGHTMODE_POI) ? FLIGHTMODE_GUIDANCE : \ - FLIGHTMODE_UNDEFINED \ - ) +typedef struct controlHandlerStruct { + FlightStatusControlChainData controlChain; + handlerFunc handler; +} controlHandler; -int32_t ManualControlInitialize(); +/** + * @brief Handler to interprete Command inputs in regards to arming/disarming (called in all flight modes) + * @input: ManualControlCommand, AccessoryDesired + * @output: FlightStatus.Arming + */ +void armHandler(bool newinit); +/** + * @brief Handler to control Manual flightmode - input directly steers actuators + * @input: ManualControlCommand + * @output: ActuatorDesired + */ +void manualHandler(bool newinit); + +/** + * @brief Handler to control Stabilized flightmodes. FlightControl is governed by "Stabilization" + * @input: ManualControlCommand + * @output: StabilizationDesired + */ +void stabilizedHandler(bool newinit); + +/** + * @brief Handler to control deprecated flight modes controlled by AltitudeHold module + * @input: ManualControlCommand + * @output: AltitudeHoldDesired + */ +void altitudeHandler(bool newinit); + +/** + * @brief Handler to control Guided flightmodes. FlightControl is governed by PathFollower, control via PathDesired + * @input: NONE: fully automated mode -- TODO recursively call handler for advanced stick commands + * @output: PathDesired + */ +void pathFollowerHandler(bool newinit); + +/** + * @brief Handler to control Navigated flightmodes. FlightControl is governed by PathFollower, controlled indirectly via PathPlanner + * @input: NONE: fully automated mode -- TODO recursively call handler for advanced stick commands to affect navigation + * @output: NONE + */ +void pathPlannerHandler(bool newinit); /* * These are assumptions we make in the flight code about the order of settings and their consistency between @@ -103,12 +130,4 @@ int32_t ManualControlInitialize(); ((int)FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI == (int)FLIGHTSTATUS_FLIGHTMODE_POI) \ ) -#define assumptions_channelcount \ - ( \ - ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM) && \ - ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNUMBER_NUMELEM) && \ - ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMIN_NUMELEM) && \ - ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMAX_NUMELEM) && \ - ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNEUTRAL_NUMELEM)) - #endif // MANUALCONTROL_H diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 940e5cf5c..aebfee0f0 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -11,7 +11,7 @@ * AttitudeDesired object (stabilized mode) * * @file manualcontrol.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. * @brief ManualControl module. Handles safety R/C link and flight mode. * * @see The GNU Public License (GPL) Version 3 @@ -33,30 +33,16 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include -#include -#include "accessorydesired.h" -#include "actuatordesired.h" -#include "altitudeholddesired.h" -#include "flighttelemetrystats.h" -#include "flightstatus.h" -#include "sanitycheck.h" -#include "manualcontrol.h" -#include "manualcontrolsettings.h" -#include "manualcontrolcommand.h" -#include "flightmodesettings.h" -#include "positionstate.h" -#include "pathdesired.h" -#include "stabilizationbank.h" -#include "stabilizationdesired.h" -#include "receiveractivity.h" -#include "systemsettings.h" -#include -#include +#include "inc/manualcontrol.h" +#include +#include +#include +#include +#include +#include +#include +#include -#if defined(PIOS_INCLUDE_USB_RCTX) -#include "pios_usb_rctx.h" -#endif /* PIOS_INCLUDE_USB_RCTX */ // Private constants #if defined(PIOS_MANUAL_STACK_SIZE) @@ -65,79 +51,88 @@ #define STACK_SIZE_BYTES 1152 #endif -#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) // 3 = flight control -#define UPDATE_PERIOD_MS 20 -#define THROTTLE_FAILSAFE -0.1f -#define ARMED_THRESHOLD 0.50f -// safe band to allow a bit of calibration error or trim offset (in microseconds) -#define CONNECTION_OFFSET 250 +#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR +#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL + + +// defined handlers + +static controlHandler handler_MANUAL = { + .controlChain = { + .Stabilization = false, + .PathFollower = false, + .PathPlanner = false, + }, + .handler = &manualHandler, +}; +static controlHandler handler_STABILIZED = { + .controlChain = { + .Stabilization = true, + .PathFollower = false, + .PathPlanner = false, + }, + .handler = &stabilizedHandler, +}; + +// TODO: move the altitude handling into stabi +static controlHandler handler_ALTITUDE = { + .controlChain = { + .Stabilization = true, + .PathFollower = false, + .PathPlanner = false, + }, + .handler = &altitudeHandler, +}; + +static controlHandler handler_PATHFOLLOWER = { + .controlChain = { + .Stabilization = true, + .PathFollower = true, + .PathPlanner = false, + }, + .handler = &pathFollowerHandler, +}; + +static controlHandler handler_PATHPLANNER = { + .controlChain = { + .Stabilization = true, + .PathFollower = true, + .PathPlanner = true, + }, + .handler = &pathPlannerHandler, +}; -// 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; - -#ifdef USE_INPUT_LPF -static portTickType lastSysTimeLPF; -static float inputFiltered[MANUALCONTROLSETTINGS_RESPONSETIME_NUMELEM]; -#endif +static DelayedCallbackInfo *callbackHandle; // Private functions -static void updateActuatorDesired(ManualControlCommandData *cmd); -static void updateStabilizationDesired(ManualControlCommandData *cmd, FlightModeSettingsData *settings); -static void updateLandDesired(ManualControlCommandData *cmd, bool changed); -static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed); -static void updatePathDesired(ManualControlCommandData *cmd, bool changed, bool home); -static void processFlightMode(ManualControlSettingsData *settings, FlightModeSettingsData *modeSettings, float flightMode, ManualControlCommandData *cmd); -static void processArm(ManualControlCommandData *cmd, FlightModeSettingsData *settings, int8_t armSwitch); -static void setArmedIfChanged(uint8_t val); static void configurationUpdatedCb(UAVObjEvent *ev); +static void commandUpdatedCb(UAVObjEvent *ev); -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); -static void applyDeadband(float *value, float deadband); +static void manualControlTask(void); -#ifdef USE_INPUT_LPF -static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT); -#endif - -#define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12 -#define RCVR_ACTIVITY_MONITOR_MIN_RANGE 10 -struct rcvr_activity_fsm { - ManualControlSettingsChannelGroupsOptions group; - uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP]; - uint8_t sample_count; -}; -static struct rcvr_activity_fsm activity_fsm; - -static void resetRcvrActivity(struct rcvr_activity_fsm *fsm); -static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm); - -#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions_flightmode && assumptions_channelcount) +#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions_flightmode) /** * Module starting */ int32_t ManualControlStart() { + // Run this initially to make sure the configuration is checked + configuration_check(); + + // Whenever the configuration changes, make sure it is safe to fly + SystemSettingsConnectCallback(configurationUpdatedCb); + ManualControlSettingsConnectCallback(configurationUpdatedCb); + ManualControlCommandConnectCallback(commandUpdatedCb); + + + // Make sure unarmed on power up + armHandler(true); + // Start main task - xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_MANUALCONTROL, taskHandle); -#ifdef PIOS_INCLUDE_WDG - PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL); -#endif + PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); return 0; } @@ -148,17 +143,15 @@ int32_t ManualControlStart() int32_t ManualControlInitialize() { /* Check the assumptions about uavobject enum's are correct */ - if (!assumptions) { - return -1; - } + PIOS_STATIC_ASSERT(assumptions); - AccessoryDesiredInitialize(); ManualControlCommandInitialize(); FlightStatusInitialize(); - StabilizationDesiredInitialize(); - ReceiverActivityInitialize(); ManualControlSettingsInitialize(); FlightModeSettingsInitialize(); + SystemSettingsInitialize(); + + callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&manualControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_MANUALCONTROL, STACK_SIZE_BYTES); return 0; } @@ -167,1128 +160,70 @@ MODULE_INITCALL(ManualControlInitialize, ManualControlStart); /** * Module task */ -static void manualControlTask(__attribute__((unused)) void *parameters) +static void manualControlTask(void) { - ManualControlSettingsData settings; - FlightModeSettingsData modeSettings; + // Process Arming + armHandler(false); + + // Process flight mode + FlightStatusData flightStatus; + + FlightStatusGet(&flightStatus); 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(); - - // Run this initially to make sure the configuration is checked - configuration_check(); - - // Whenever the configuration changes, make sure it is safe to fly - SystemSettingsConnectCallback(configurationUpdatedCb); - ManualControlSettingsConnectCallback(configurationUpdatedCb); - - // Whenever the configuration changes, make sure it is safe to fly - - // Make sure unarmed on power up ManualControlCommandGet(&cmd); - FlightStatusGet(&flightStatus); - flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED; - armState = ARM_STATE_DISARMED; - /* Initialize the RcvrActivty FSM */ - portTickType lastActivityTime = xTaskGetTickCount(); - resetRcvrActivity(&activity_fsm); + FlightModeSettingsData modeSettings; + FlightModeSettingsGet(&modeSettings); - // Main task loop - lastSysTime = xTaskGetTickCount(); - - float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM] = { 0 }; - SystemSettingsThrustControlOptions thrustType; - - while (1) { - // Wait until next update - vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS); -#ifdef PIOS_INCLUDE_WDG - PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL); -#endif - - // Read settings - ManualControlSettingsGet(&settings); - FlightModeSettingsGet(&modeSettings); - SystemSettingsThrustControlGet(&thrustType); - - /* Update channel activity monitor */ - if (flightStatus.Armed == ARM_STATE_DISARMED) { - if (updateRcvrActivity(&activity_fsm)) { - /* Reset the aging timer because activity was detected */ - lastActivityTime = lastSysTime; - } - } - if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) { - resetRcvrActivity(&activity_fsm); - lastActivityTime = lastSysTime; - } - - if (ManualControlCommandReadOnly()) { - FlightTelemetryStatsData flightTelemStats; - FlightTelemetryStatsGet(&flightTelemStats); - if (flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { - /* trying to fly via GCS and lost connection. fall back to transmitter */ - UAVObjMetadata metadata; - ManualControlCommandGetMetadata(&metadata); - UAVObjSetAccess(&metadata, ACCESS_READWRITE); - ManualControlCommandSetMetadata(&metadata); - } - } - - if (!ManualControlCommandReadOnly()) { - bool valid_input_detected = true; - - // Read channel values in us - for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { - extern uint32_t pios_rcvr_group_map[]; - - if (cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Roll)[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - cmd.Channel[n] = PIOS_RCVR_INVALID; - } else { - cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[ - cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Pitch)[n]], - cast_struct_to_array(settings.ChannelNumber, settings.ChannelNumber.Pitch)[n]); - } - - // If a channel has timed out this is not valid data and we shouldn't update anything - // until we decide to go to failsafe - if (cmd.Channel[n] == (uint16_t)PIOS_RCVR_TIMEOUT) { - valid_input_detected = false; - } else { - scaledChannel[n] = scaleChannel(cmd.Channel[n], - cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Pitch)[n], - cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Pitch)[n], - cast_struct_to_array(settings.ChannelNeutral, settings.ChannelNeutral.Pitch)[n]); - } - } - - // Check settings, if error raise alarm - if (settings.ChannelGroups.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || settings.ChannelGroups.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || settings.ChannelGroups.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || settings.ChannelGroups.Throttle >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || - // Check all channel mappings are valid - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_INVALID - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_INVALID - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_INVALID - || - // Check the driver exists - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_NODRIVER - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_NODRIVER - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_NODRIVER - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_NODRIVER || - // Check the FlightModeNumber is valid - settings.FlightModeNumber < 1 || settings.FlightModeNumber > FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM || - // Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care - ((settings.FlightModeNumber > 1) - && (settings.ChannelGroups.FlightMode >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_INVALID - || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_NODRIVER))) { - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); - cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; - ManualControlCommandSet(&cmd); - - // Need to do this here since we don't process armed status. Since this shouldn't happen in flight (changed config) - // immediately disarm - setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); - - continue; - } - - // decide if we have valid manual input or not - valid_input_detected &= validInputRange(settings.ChannelMin.Throttle, - settings.ChannelMax.Throttle, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) - && validInputRange(settings.ChannelMin.Roll, - settings.ChannelMax.Roll, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) - && validInputRange(settings.ChannelMin.Yaw, - settings.ChannelMax.Yaw, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) - && validInputRange(settings.ChannelMin.Pitch, - settings.ChannelMax.Pitch, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_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; - } - - int8_t armSwitch = 0; - if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { - cmd.Throttle = settings.FailsafeChannel.Throttle; - cmd.Roll = settings.FailsafeChannel.Roll; - cmd.Pitch = settings.FailsafeChannel.Pitch; - cmd.Yaw = settings.FailsafeChannel.Yaw; - cmd.Collective = settings.FailsafeChannel.Collective; - switch (thrustType) { - case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE: - cmd.Thrust = cmd.Throttle; - break; - case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE: - cmd.Thrust = cmd.Collective; - break; - default: - break; - } - if (settings.FailsafeFlightModeSwitchPosition >= 0 && settings.FailsafeFlightModeSwitchPosition < settings.FlightModeNumber) { - FlightStatusGet(&flightStatus); - - cmd.FlightModeSwitchPosition = (uint8_t)settings.FailsafeFlightModeSwitchPosition; - flightStatus.FlightMode = modeSettings.FlightModePosition[settings.FailsafeFlightModeSwitchPosition]; - FlightStatusSet(&flightStatus); - } - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - - AccessoryDesiredData accessory; - // Set Accessory 0 - if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - accessory.AccessoryVal = settings.FailsafeChannel.Accessory0; - if (AccessoryDesiredInstSet(0, &accessory) != 0) { - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - } - } - // Set Accessory 1 - if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - accessory.AccessoryVal = settings.FailsafeChannel.Accessory1; - if (AccessoryDesiredInstSet(1, &accessory) != 0) { - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - } - } - // Set Accessory 2 - if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - accessory.AccessoryVal = settings.FailsafeChannel.Accessory2; - if (AccessoryDesiredInstSet(2, &accessory) != 0) { - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - } - } - } else if (valid_input_detected) { - AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); - - // Scale channels to -1 -> +1 range - cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]; - cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]; - cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]; - cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]; - flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE]; - - // Apply deadband for Roll/Pitch/Yaw stick inputs - if (settings.Deadband > 0.0f) { - applyDeadband(&cmd.Roll, settings.Deadband); - applyDeadband(&cmd.Pitch, settings.Deadband); - applyDeadband(&cmd.Yaw, settings.Deadband); - } -#ifdef USE_INPUT_LPF - // Apply Low Pass Filter to input channels, time delta between calls in ms - portTickType thisSysTime = xTaskGetTickCount(); - float dT = (thisSysTime > lastSysTimeLPF) ? - (float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) : - (float)UPDATE_PERIOD_MS; - lastSysTimeLPF = thisSysTime; - - applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings, dT); - applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings, dT); - applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings, dT); -#endif // USE_INPUT_LPF - if (cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_INVALID - && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_NODRIVER - && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_TIMEOUT) { - cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE]; - } - - switch (thrustType) { - case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE: - cmd.Thrust = cmd.Throttle; - break; - case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE: - cmd.Thrust = cmd.Collective; - break; - default: - break; - } - - AccessoryDesiredData accessory; - // Set Accessory 0 - if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0]; -#ifdef USE_INPUT_LPF - applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT); -#endif - if (modeSettings.Arming == FLIGHTMODESETTINGS_ARMING_ACCESSORY0) { - if (accessory.AccessoryVal > ARMED_THRESHOLD) { - armSwitch = 1; - } else if (accessory.AccessoryVal < -ARMED_THRESHOLD) { - armSwitch = -1; - } - } - if (AccessoryDesiredInstSet(0, &accessory) != 0) { - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - } - } - // Set Accessory 1 - if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1]; -#ifdef USE_INPUT_LPF - applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT); -#endif - if (modeSettings.Arming == FLIGHTMODESETTINGS_ARMING_ACCESSORY1) { - if (accessory.AccessoryVal > ARMED_THRESHOLD) { - armSwitch = 1; - } else if (accessory.AccessoryVal < -ARMED_THRESHOLD) { - armSwitch = -1; - } - } - if (AccessoryDesiredInstSet(1, &accessory) != 0) { - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - } - } - // Set Accessory 2 - if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2]; -#ifdef USE_INPUT_LPF - applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT); -#endif - if (modeSettings.Arming == FLIGHTMODESETTINGS_ARMING_ACCESSORY2) { - if (accessory.AccessoryVal > ARMED_THRESHOLD) { - armSwitch = 1; - } else if (accessory.AccessoryVal < -ARMED_THRESHOLD) { - armSwitch = -1; - } - } - - if (AccessoryDesiredInstSet(2, &accessory) != 0) { - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - } - } - - processFlightMode(&settings, &modeSettings, flightMode, &cmd); - } - - // Process arming outside conditional so system will disarm when disconnected - processArm(&cmd, &modeSettings, armSwitch); - - // Update cmd object - ManualControlCommandSet(&cmd); -#if defined(PIOS_INCLUDE_USB_RCTX) - if (pios_usb_rctx_id) { - PIOS_USB_RCTX_Update(pios_usb_rctx_id, - cmd.Channel, - cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Roll), - cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Roll), - NELEMENTS(cmd.Channel)); - } -#endif /* PIOS_INCLUDE_USB_RCTX */ - } else { - ManualControlCommandGet(&cmd); /* Under GCS control */ - } - - FlightStatusGet(&flightStatus); - - // Depending on the mode update the Stabilization or Actuator objects - static uint8_t lastFlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL; - 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, &modeSettings); - break; - case FLIGHTMODE_TUNING: - // Tuning takes settings directly from manualcontrolcommand. No need to - // call anything else. This just avoids errors. - break; - case FLIGHTMODE_GUIDANCE: - switch (flightStatus.FlightMode) { - case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: - case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO: - altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode); - break; - case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: - case FLIGHTSTATUS_FLIGHTMODE_POI: - updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, false); - break; - case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: - updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, true); - break; - case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: - // No need to call anything. This just avoids errors. - break; - case FLIGHTSTATUS_FLIGHTMODE_LAND: - updateLandDesired(&cmd, lastFlightMode != flightStatus.FlightMode); - break; - default: - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); - } - break; - } - lastFlightMode = flightStatus.FlightMode; - } -} - -static void resetRcvrActivity(struct rcvr_activity_fsm *fsm) -{ - ReceiverActivityData data; - bool updated = false; - - /* Clear all channel activity flags */ - ReceiverActivityGet(&data); - if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE && data.ActiveChannel != 255) { - data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE; - data.ActiveChannel = 255; - updated = true; - } - if (updated) { - ReceiverActivitySet(&data); + uint8_t position = cmd.FlightModeSwitchPosition; + uint8_t newMode = flightStatus.FlightMode; + if (position < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) { + newMode = modeSettings.FlightModePosition[position]; } - /* Reset the FSM state */ - fsm->group = 0; - fsm->sample_count = 0; -} - -static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels) -{ - for (uint8_t channel = 1; channel <= max_channels; channel++) { - // Subtract 1 because channels are 1 indexed - samples[channel - 1] = PIOS_RCVR_Read(rcvr_id, channel); - } -} - -static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm *fsm) -{ - bool activity_updated = false; - - /* Compare the current value to the previous sampled value */ - for (uint8_t channel = 1; channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; channel++) { - uint16_t delta; - uint16_t prev = fsm->prev[channel - 1]; // Subtract 1 because channels are 1 indexed - uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel); - if (curr > prev) { - delta = curr - prev; - } else { - delta = prev - curr; - } - - if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) { - /* Mark this channel as active */ - ReceiverActivityActiveGroupOptions group; - - /* Don't assume manualcontrolsettings and receiveractivity are in the same order. */ - switch (fsm->group) { - case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM: - group = RECEIVERACTIVITY_ACTIVEGROUP_PWM; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM: - group = RECEIVERACTIVITY_ACTIVEGROUP_PPM; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT: - group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT: - group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS: - group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS: - group = RECEIVERACTIVITY_ACTIVEGROUP_GCS; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK: - group = RECEIVERACTIVITY_ACTIVEGROUP_OPLINK; - break; - default: - PIOS_Assert(0); - break; - } - - ReceiverActivityActiveGroupSet((uint8_t *)&group); - ReceiverActivityActiveChannelSet(&channel); - activity_updated = true; - } - } - return activity_updated; -} - -static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm) -{ - bool activity_updated = false; - - if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - /* We're out of range, reset things */ - resetRcvrActivity(fsm); - } - - extern uint32_t pios_rcvr_group_map[]; - if (!pios_rcvr_group_map[fsm->group]) { - /* Unbound group, skip it */ - goto group_completed; - } - - if (fsm->sample_count == 0) { - /* Take a sample of each channel in this group */ - updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev)); - fsm->sample_count++; - return false; - } - - /* Compare with previous sample */ - activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm); - -group_completed: - /* Reset the sample counter */ - fsm->sample_count = 0; - - /* Find the next active group, but limit search so we can't loop forever here */ - for (uint8_t i = 0; i < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE; i++) { - /* Move to the next group */ - fsm->group++; - if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - /* Wrap back to the first group */ - fsm->group = 0; - } - if (pios_rcvr_group_map[fsm->group]) { - /* - * Found an active group, take a sample here to avoid an - * extra 20ms delay in the main thread so we can speed up - * this algorithm. - */ - updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev)); - fsm->sample_count++; - break; - } - } - - return activity_updated; -} - -static void updateActuatorDesired(ManualControlCommandData *cmd) -{ - ActuatorDesiredData actuator; - - ActuatorDesiredGet(&actuator); - actuator.Roll = cmd->Roll; - actuator.Pitch = cmd->Pitch; - actuator.Yaw = cmd->Yaw; - actuator.Thrust = cmd->Thrust; - ActuatorDesiredSet(&actuator); -} - -static void updateStabilizationDesired(ManualControlCommandData *cmd, FlightModeSettingsData *settings) -{ - StabilizationDesiredData stabilization; - - StabilizationDesiredGet(&stabilization); - - StabilizationBankData stabSettings; - StabilizationBankGet(&stabSettings); - - uint8_t *stab_settings; - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); + // Depending on the mode update the Stabilization or Actuator objects + controlHandler *handler = &handler_MANUAL; switch (flightStatus.FlightMode) { - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: - stab_settings = cast_struct_to_array(settings->Stabilization1Settings, settings->Stabilization1Settings.Roll); - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: - stab_settings = cast_struct_to_array(settings->Stabilization2Settings, settings->Stabilization2Settings.Roll); - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: - stab_settings = cast_struct_to_array(settings->Stabilization3Settings, settings->Stabilization3Settings.Roll); - 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; - } - - stabilization.Roll = - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate.Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.ManualRate.Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? 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.Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate.Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate.Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : - 0; // this is an invalid mode - - // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order - stabilization.StabilizationMode.Roll = stab_settings[0]; - stabilization.StabilizationMode.Pitch = stab_settings[1]; - // Other axes (yaw) cannot be Rattitude, so use Rate - // Should really do this for Attitude mode as well? - if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) { - stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; - stabilization.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw; - } else { - stabilization.StabilizationMode.Yaw = stab_settings[2]; - stabilization.Yaw = - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate.Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : - 0; // this is an invalid mode - } - - stabilization.Thrust = cmd->Thrust; - StabilizationDesiredSet(&stabilization); -} - -#if defined(REVOLUTION) -// TODO: Need compile flag to exclude this from copter control -/** - * @brief Update the position desired to current location when - * enabled and allow the waypoint to be moved by transmitter - */ -static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *cmd, bool changed, bool home) -{ - /* - static portTickType lastSysTime; - portTickType thisSysTime = xTaskGetTickCount(); - dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f); - lastSysTime = thisSysTime; - */ - - if (home && changed) { - // Simple Return To Base mode - keep altitude the same, fly to home position - PositionStateData positionState; - PositionStateGet(&positionState); - FlightModeSettingsData settings; - FlightModeSettingsGet(&settings); - - PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); - pathDesired.Start.North = 0; - pathDesired.Start.East = 0; - pathDesired.Start.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; - pathDesired.End.North = 0; - pathDesired.End.East = 0; - pathDesired.End.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; - pathDesired.StartingVelocity = 1; - pathDesired.EndingVelocity = 0; - pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; - PathDesiredSet(&pathDesired); - } else if (changed) { - // After not being in this mode for a while init at current height - PositionStateData positionState; - PositionStateGet(&positionState); - - PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); - pathDesired.Start.North = positionState.North; - pathDesired.Start.East = positionState.East; - pathDesired.Start.Down = positionState.Down; - pathDesired.End.North = positionState.North; - pathDesired.End.East = positionState.East; - pathDesired.End.Down = positionState.Down; - pathDesired.StartingVelocity = 1; - pathDesired.EndingVelocity = 0; - pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; - PathDesiredSet(&pathDesired); - /* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts. - } else { - PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); - pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch; - pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll; - pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; - PathDesiredSet(&pathDesired); - */ - } -} - -static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *cmd, bool changed) -{ - /* - static portTickType lastSysTime; - portTickType thisSysTime; - float dT; - - thisSysTime = xTaskGetTickCount(); - dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f); - lastSysTime = thisSysTime; - */ - - PositionStateData positionState; - - PositionStateGet(&positionState); - - PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); - if (changed) { - // After not being in this mode for a while init at current height - pathDesired.Start.North = positionState.North; - pathDesired.Start.East = positionState.East; - pathDesired.Start.Down = positionState.Down; - pathDesired.End.North = positionState.North; - pathDesired.End.East = positionState.East; - pathDesired.End.Down = positionState.Down; - pathDesired.StartingVelocity = 1; - pathDesired.EndingVelocity = 0; - pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; - } - pathDesired.End.Down = positionState.Down + 5; - PathDesiredSet(&pathDesired); -} - -/** - * @brief Update the altitude desired to current altitude when - * enabled and enable altitude mode for stabilization - * @todo: Need compile flag to exclude this from copter control - */ -static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) -{ - const float DEADBAND = 0.20f; - const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2; - const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2; - - // this is the max speed in m/s at the extents of thrust - float thrustRate; - uint8_t thrustExp; - - static uint8_t flightMode; - static bool newaltitude = true; - - FlightStatusFlightModeGet(&flightMode); - - AltitudeHoldDesiredData altitudeHoldDesiredData; - AltitudeHoldDesiredGet(&altitudeHoldDesiredData); - - AltitudeHoldSettingsThrustExpGet(&thrustExp); - AltitudeHoldSettingsThrustRateGet(&thrustRate); - - StabilizationBankData stabSettings; - StabilizationBankGet(&stabSettings); - - PositionStateData posState; - PositionStateGet(&posState); - - altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; - altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax; - altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw; - - if (changed) { - newaltitude = true; - } - - uint8_t cutOff; - AltitudeHoldSettingsCutThrustWhenZeroGet(&cutOff); - if (cutOff && cmd->Thrust < 0) { - // Cut thrust if desired - altitudeHoldDesiredData.SetPoint = cmd->Thrust; - altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_THRUST; - newaltitude = true; - } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Thrust > DEADBAND_HIGH) { - // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 - // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255 - altitudeHoldDesiredData.SetPoint = -((thrustExp * powf((cmd->Thrust - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - thrustExp) * (cmd->Thrust - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * thrustRate); - altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY; - newaltitude = true; - } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Thrust < DEADBAND_LOW) { - altitudeHoldDesiredData.SetPoint = -(-(thrustExp * powf((DEADBAND_LOW - (cmd->Thrust < 0 ? 0 : cmd->Thrust)) / DEADBAND_LOW, 3) + (255 - thrustExp) * (DEADBAND_LOW - cmd->Thrust) / DEADBAND_LOW) / 255 * thrustRate); - altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY; - newaltitude = true; - } else if (newaltitude == true) { - altitudeHoldDesiredData.SetPoint = posState.Down; - altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE; - newaltitude = false; - } - - AltitudeHoldDesiredSet(&altitudeHoldDesiredData); -} -#else /* if defined(REVOLUTION) */ - -// TODO: These functions should never be accessible on CC. Any configuration that -// could allow them to be called should already throw an error to prevent this happening -// in flight -static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *cmd, - __attribute__((unused)) bool changed, - __attribute__((unused)) bool home) -{ - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); -} - -static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *cmd, - __attribute__((unused)) bool changed) -{ - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); -} - -static void altitudeHoldDesired(__attribute__((unused)) ManualControlCommandData *cmd, - __attribute__((unused)) bool changed) -{ - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); -} -#endif /* if defined(REVOLUTION) */ -/** - * 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.0f) { - valueScaled = 1.0f; - } else if (valueScaled < -1.0f) { - valueScaled = -1.0f; - } - - return valueScaled; -} - -static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) -{ - return (end_time - start_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) -{ - // update checks - configuration_check(); - - // read alarms - SystemAlarmsData alarms; - - SystemAlarmsGet(&alarms); - - // Check each alarm - for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) { - if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set - if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) { - continue; - } - - return false; - } - } - - uint8_t flightMode; - FlightStatusFlightModeGet(&flightMode); - switch (flightMode) { case FLIGHTSTATUS_FLIGHTMODE_MANUAL: + handler = &handler_MANUAL; + break; case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: - return true; - - default: - return false; - } -} -/** - * @brief Determine if the aircraft is forced to disarm by an explicit alarm - * @returns True if safe to arm, false otherwise - */ -static bool forcedDisArm(void) -{ - // read alarms - SystemAlarmsData alarms; - - SystemAlarmsGet(&alarms); - - if (alarms.Alarm.Guidance == SYSTEMALARMS_ALARM_CRITICAL) { - return true; - } - return false; -} - -/** - * @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, FlightModeSettingsData *settings, int8_t armSwitch) -{ - bool lowThrottle = cmd->Throttle < 0; - - /** - * do NOT check throttle if disarming via switch, must be instant - */ - switch (settings->Arming) { - case FLIGHTMODESETTINGS_ARMING_ACCESSORY0: - case FLIGHTMODESETTINGS_ARMING_ACCESSORY1: - case FLIGHTMODESETTINGS_ARMING_ACCESSORY2: - if (armSwitch < 0) { - lowThrottle = true; - } + handler = &handler_STABILIZED; break; - default: + case FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL: + case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: + case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: + case FLIGHTSTATUS_FLIGHTMODE_LAND: + case FLIGHTSTATUS_FLIGHTMODE_POI: + handler = &handler_PATHFOLLOWER; break; + case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: + handler = &handler_PATHPLANNER; + break; + case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD: + case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO: + handler = &handler_ALTITUDE; + break; + case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE: + handler = NULL; + break; + // There is no default, so if a flightmode is forgotten the compiler can throw a warning! } - 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); - } else { - // Not really needed since this function not called when disconnected - if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { - lowThrottle = true; + if (handler) { + bool newinit = false; + if (flightStatus.FlightMode != newMode) { + flightStatus.ControlChain = handler->controlChain; + flightStatus.FlightMode = newMode; + FlightStatusSet(&flightStatus); + newinit = true; } - - // 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 * (float)armSwitch; - 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) > 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 = 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) > settings->DisarmingSequenceTime)) { - armState = ARM_STATE_DISARMED; - } else if (!manualDisarm) { - armState = ARM_STATE_ARMED; - } - break; - } // End Switch + handler->handler(newinit); } } -/** - * @brief Determine which of N 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, FlightModeSettingsData *modeSettings, float flightMode, ManualControlCommandData *cmd) -{ - FlightStatusData flightStatus; - - FlightStatusGet(&flightStatus); - - // Convert flightMode value into the switch position in the range [0..N-1] - uint8_t pos = ((int16_t)(flightMode * 256.0f) + 256) * settings->FlightModeNumber >> 9; - if (pos >= settings->FlightModeNumber) { - pos = settings->FlightModeNumber - 1; - } - - cmd->FlightModeSwitchPosition = pos; - - uint8_t newMode = modeSettings->FlightModePosition[pos]; - - 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; -} - -/** - * @brief Apply deadband to Roll/Pitch/Yaw channels - */ -static void applyDeadband(float *value, float deadband) -{ - if (fabsf(*value) < deadband) { - *value = 0.0f; - } else if (*value > 0.0f) { - *value -= deadband; - } else { - *value += deadband; - } -} - -#ifdef USE_INPUT_LPF -/** - * @brief Apply Low Pass Filter to Throttle/Roll/Pitch/Yaw or Accessory channel - */ -static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT) -{ - if (cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel]) { - float rt = (float)cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel]; - inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT); - *value = inputFiltered[channel]; - } -} -#endif // USE_INPUT_LPF /** * Called whenever a critical configuration component changes */ @@ -1297,6 +232,14 @@ static void configurationUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) configuration_check(); } +/** + * Called whenever a critical configuration component changes + */ +static void commandUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); +} + /** * @} * @} diff --git a/flight/modules/ManualControl/manualhandler.c b/flight/modules/ManualControl/manualhandler.c new file mode 100644 index 000000000..71bdfb3b2 --- /dev/null +++ b/flight/modules/ManualControl/manualhandler.c @@ -0,0 +1,71 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup ManualControl + * @brief Interpretes the control input in ManualControlCommand + * @{ + * + * @file manualhandler.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * + * @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 "inc/manualcontrol.h" +#include +#include + +// Private constants + +// Private types + +// Private functions + + +/** + * @brief Handler to control Manual flightmode - input directly steers actuators + * @input: ManualControlCommand + * @output: ActuatorDesired + */ +void manualHandler(bool newinit) +{ + if (newinit) { + ActuatorDesiredInitialize(); + } + + ManualControlCommandData cmd; + ManualControlCommandGet(&cmd); + + ActuatorDesiredData actuator; + ActuatorDesiredGet(&actuator); + + actuator.Roll = cmd.Roll; + actuator.Pitch = cmd.Pitch; + actuator.Yaw = cmd.Yaw; + actuator.Thrust = cmd.Thrust; + + ActuatorDesiredSet(&actuator); +} + + +/** + * @} + * @} + */ diff --git a/flight/modules/ManualControl/pathfollowerhandler.c b/flight/modules/ManualControl/pathfollowerhandler.c new file mode 100644 index 000000000..1820d0778 --- /dev/null +++ b/flight/modules/ManualControl/pathfollowerhandler.c @@ -0,0 +1,131 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup ManualControl + * @brief Interpretes the control input in ManualControlCommand + * @{ + * + * @file pathfollowerhandler.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * + * @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 "inc/manualcontrol.h" +#include +#include +#include +#include +#include + + +#if defined(REVOLUTION) +// Private constants + +// Private types + +// Private functions + +/** + * @brief Handler to control Guided flightmodes. FlightControl is governed by PathFollower, control via PathDesired + * @input: NONE: fully automated mode -- TODO recursively call handler for advanced stick commands + * @output: PathDesired + */ +void pathFollowerHandler(bool newinit) +{ + if (newinit) { + PathDesiredInitialize(); + PositionStateInitialize(); + } + + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + + if (newinit) { + // After not being in this mode for a while init at current height + PositionStateData positionState; + PositionStateGet(&positionState); + FlightModeSettingsData settings; + FlightModeSettingsGet(&settings); + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + switch (flightStatus.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: + // Simple Return To Base mode - keep altitude the same, fly to home position + + + pathDesired.Start.North = 0; + pathDesired.Start.East = 0; + pathDesired.Start.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; + pathDesired.End.North = 0; + pathDesired.End.East = 0; + pathDesired.End.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; + pathDesired.StartingVelocity = 1; + pathDesired.EndingVelocity = 0; + pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + break; + default: + + pathDesired.Start.North = positionState.North; + pathDesired.Start.East = positionState.East; + pathDesired.Start.Down = positionState.Down; + pathDesired.End.North = positionState.North; + pathDesired.End.East = positionState.East; + pathDesired.End.Down = positionState.Down; + pathDesired.StartingVelocity = 1; + pathDesired.EndingVelocity = 0; + pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + /* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts. + } else { + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch; + pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll; + pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + PathDesiredSet(&pathDesired); + */ + break; + } + PathDesiredSet(&pathDesired); + } + + // special handling of autoland - behaves like positon hold but with slow altitude decrease + if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) { + PositionStateData positionState; + PositionStateGet(&positionState); + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + pathDesired.End.Down = positionState.Down + 5; + PathDesiredSet(&pathDesired); + } +} + +#else /* if defined(REVOLUTION) */ +void pathFollowerHandler(__attribute__((unused)) bool newinit) +{ + PIOS_Assert(0); // should not be called +} +#endif // REVOLUTION + + +/** + * @} + * @} + */ diff --git a/flight/modules/ManualControl/pathplannerhandler.c b/flight/modules/ManualControl/pathplannerhandler.c new file mode 100644 index 000000000..0c29ef397 --- /dev/null +++ b/flight/modules/ManualControl/pathplannerhandler.c @@ -0,0 +1,64 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup ManualControl + * @brief Interpretes the control input in ManualControlCommand + * @{ + * + * @file pathplannerhandler.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * + * @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 "inc/manualcontrol.h" + +// Private constants + +// Private types + +// Private functions + + +/** + * @brief Handler to control Navigated flightmodes. FlightControl is governed by PathFollower, controlled indirectly via PathPlanner + * @input: NONE: fully automated mode -- TODO recursively call handler for advanced stick commands to affect navigation + * @output: NONE + */ +void pathPlannerHandler(__attribute__((unused)) bool newinit) +{ + /** + * + * TODO: In fully autonomous mode, commands to the navigation facility + * can be encoded through standard input cmd channels, as the pathplanner + * pathfollower generally ignores them + * + * this should be provided by a separate handler invoked here, as the + * handler for pathFollower should likely invoke them as well!!! + * (inputs also ignored) + * + */ +} + + +/** + * @} + * @} + */ diff --git a/flight/modules/ManualControl/stabilizedhandler.c b/flight/modules/ManualControl/stabilizedhandler.c new file mode 100644 index 000000000..8d3a00295 --- /dev/null +++ b/flight/modules/ManualControl/stabilizedhandler.c @@ -0,0 +1,142 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup ManualControl + * @brief Interpretes the control input in ManualControlCommand + * @{ + * + * @file stabilizedhandler.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * + * @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 "inc/manualcontrol.h" +#include +#include +#include +#include +#include + +// Private constants + +// Private types + +// Private functions + + +/** + * @brief Handler to control Stabilized flightmodes. FlightControl is governed by "Stabilization" + * @input: ManualControlCommand + * @output: StabilizationDesired + */ +void stabilizedHandler(bool newinit) +{ + if (newinit) { + StabilizationDesiredInitialize(); + StabilizationBankInitialize(); + } + ManualControlCommandData cmd; + ManualControlCommandGet(&cmd); + + FlightModeSettingsData settings; + FlightModeSettingsGet(&settings); + + StabilizationDesiredData stabilization; + StabilizationDesiredGet(&stabilization); + + StabilizationBankData stabSettings; + StabilizationBankGet(&stabSettings); + + uint8_t *stab_settings; + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + switch (flightStatus.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: + stab_settings = cast_struct_to_array(settings.Stabilization1Settings, settings.Stabilization1Settings.Roll); + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: + stab_settings = cast_struct_to_array(settings.Stabilization2Settings, settings.Stabilization2Settings.Roll); + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: + stab_settings = cast_struct_to_array(settings.Stabilization3Settings, settings.Stabilization3Settings.Roll); + break; + default: + // Major error, this should not occur because only enter this block when one of these is true + stab_settings = cast_struct_to_array(settings.Stabilization1Settings, settings.Stabilization1Settings.Roll); + return; + } + + stabilization.Roll = + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Roll * stabSettings.RollMax : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? 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.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Pitch * stabSettings.PitchMax : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd.Pitch * stabSettings.PitchMax : + 0; // this is an invalid mode + + // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order + stabilization.StabilizationMode.Roll = stab_settings[0]; + stabilization.StabilizationMode.Pitch = stab_settings[1]; + // Other axes (yaw) cannot be Rattitude, so use Rate + // Should really do this for Attitude mode as well? + if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) { + stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabilization.Yaw = cmd.Yaw * stabSettings.ManualRate.Yaw; + } else { + stabilization.StabilizationMode.Yaw = stab_settings[2]; + stabilization.Yaw = + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd.Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd.Yaw * stabSettings.YawMax : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd.Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd.Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd.Yaw * stabSettings.YawMax : + 0; // this is an invalid mode + } + + stabilization.Thrust = cmd.Thrust; + StabilizationDesiredSet(&stabilization); +} + + +/** + * @} + * @} + */ diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index db56e9de8..3cfeb2914 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -49,6 +49,7 @@ #include "gyrostate.h" #include "flightstatus.h" #include "manualcontrolsettings.h" +#include "manualcontrolcommand.h" #include "flightmodesettings.h" #include "taskinfo.h" @@ -596,8 +597,8 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) actuatorDesired.UpdateTime = dT * 1000; actuatorDesired.Thrust = stabDesired.Thrust; - // Suppress desired output while disarmed or thrust low, for configured axis - if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || throttleDesired < 0) { + // Suppress desired output while disarmed + if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED) { if (lowThrottleZeroAxis[ROLL]) { actuatorDesired.Roll = 0.0f; } diff --git a/flight/modules/VtolPathFollower/vtolpathfollower.c b/flight/modules/VtolPathFollower/vtolpathfollower.c index 9efcffe3e..372a97763 100644 --- a/flight/modules/VtolPathFollower/vtolpathfollower.c +++ b/flight/modules/VtolPathFollower/vtolpathfollower.c @@ -55,7 +55,7 @@ #include "hwsettings.h" #include "pathdesired.h" // object that will be updated by the module #include "positionstate.h" -#include "manualcontrol.h" +#include "manualcontrolcommand.h" #include "flightstatus.h" #include "pathstatus.h" #include "gpsvelocitysensor.h" diff --git a/flight/targets/boards/coptercontrol/firmware/Makefile b/flight/targets/boards/coptercontrol/firmware/Makefile index ec0d5664d..1d53149ce 100644 --- a/flight/targets/boards/coptercontrol/firmware/Makefile +++ b/flight/targets/boards/coptercontrol/firmware/Makefile @@ -34,6 +34,7 @@ ERASE_FLASH ?= NO MODULES += Attitude MODULES += Stabilization MODULES += Actuator +MODULES += Receiver MODULES += ManualControl MODULES += FirmwareIAP MODULES += Telemetry diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile index f8f72e0c5..e282fc35b 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile +++ b/flight/targets/boards/revoproto/firmware/Makefile @@ -35,6 +35,7 @@ MODULES += Altitude/revolution MODULES += Airspeed MODULES += AltitudeHold MODULES += Stabilization +MODULES += Receiver MODULES += ManualControl MODULES += Actuator MODULES += GPS diff --git a/shared/uavobjectdefinition/callbackinfo.xml b/shared/uavobjectdefinition/callbackinfo.xml index 6850662de..80015361c 100644 --- a/shared/uavobjectdefinition/callbackinfo.xml +++ b/shared/uavobjectdefinition/callbackinfo.xml @@ -8,6 +8,7 @@ AltitudeHold PathPlanner0 PathPlanner1 + ManualControl @@ -17,6 +18,7 @@ AltitudeHold PathPlanner0 PathPlanner1 + ManualControl @@ -30,6 +32,7 @@ AltitudeHold PathPlanner0 PathPlanner1 + ManualControl From f61231f610a7447642485e1b167ec55609e7f449 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 2 Mar 2014 15:25:49 +0100 Subject: [PATCH 08/18] OP-1216 moved safety zeroing for channels from Stabilization to Actuators (so it also affects manual mode) --- flight/modules/Actuator/actuator.c | 26 +++++++++++++++---- flight/modules/Stabilization/stabilization.c | 23 +--------------- .../uavobjectdefinition/actuatorsettings.xml | 1 + .../stabilizationsettings.xml | 2 -- 4 files changed, 23 insertions(+), 29 deletions(-) diff --git a/flight/modules/Actuator/actuator.c b/flight/modules/Actuator/actuator.c index cb63111b7..889d5f634 100644 --- a/flight/modules/Actuator/actuator.c +++ b/flight/modules/Actuator/actuator.c @@ -167,7 +167,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters) MixerStatusData mixerStatus; FlightStatusData flightStatus; SystemSettingsThrustControlOptions thrustType; - SystemSettingsAllowReverseThrottleOptions noClamping; + SystemSettingsAllowReverseThrottleOptions allowReverseThrottle; float throttleDesired; float collectiveDesired; @@ -225,7 +225,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters) ActuatorDesiredGet(&desired); ActuatorCommandGet(&command); SystemSettingsThrustControlGet(&thrustType); - SystemSettingsAllowReverseThrottleGet(&noClamping); + SystemSettingsAllowReverseThrottleGet(&allowReverseThrottle); // read in throttle and collective -demultiplex thrust switch (thrustType) { @@ -242,8 +242,25 @@ static void actuatorTask(__attribute__((unused)) void *parameters) ManualControlCommandCollectiveGet(&collectiveDesired); } - if (noClamping == SYSTEMSETTINGS_ALLOWREVERSETHROTTLE_FALSE) { - throttleDesired = (throttleDesired < 0) ? -1 : throttleDesired; + bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED; + + // safety settings + if (allowReverseThrottle == SYSTEMSETTINGS_ALLOWREVERSETHROTTLE_FALSE && (throttleDesired < 0 || !armed)) { + throttleDesired = -1; + } else if (allowReverseThrottle == SYSTEMSETTINGS_ALLOWREVERSETHROTTLE_TRUE && !armed) { + throttleDesired = 0; + } + if (throttleDesired < 0 || !armed) { + // force set all other controls to zero if throttle is cut (previously set in Stabilization) + if (actuatorSettings.LowThrottleZeroAxis.Roll == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) { + desired.Roll = 0; + } + if (actuatorSettings.LowThrottleZeroAxis.Pitch == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) { + desired.Pitch = 0; + } + if (actuatorSettings.LowThrottleZeroAxis.Yaw == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) { + desired.Yaw = 0; + } } #ifdef DIAG_MIXERSTATUS @@ -263,7 +280,6 @@ static void actuatorTask(__attribute__((unused)) void *parameters) AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR); - bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED; bool positiveThrottle = throttleDesired >= 0.00f; bool spinWhileArmed = actuatorSettings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE; diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 3cfeb2914..51b07209d 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -104,7 +104,6 @@ uint8_t max_axislock_rate = 0; float weak_leveling_kp = 0; uint8_t weak_leveling_max = 0; bool lowThrottleZeroIntegral; -bool lowThrottleZeroAxis[MAX_AXES]; float vbar_decay = 0.991f; struct pid pids[PID_MAX]; @@ -597,21 +596,6 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) actuatorDesired.UpdateTime = dT * 1000; actuatorDesired.Thrust = stabDesired.Thrust; - // Suppress desired output while disarmed - if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED) { - if (lowThrottleZeroAxis[ROLL]) { - actuatorDesired.Roll = 0.0f; - } - - if (lowThrottleZeroAxis[PITCH]) { - actuatorDesired.Pitch = 0.0f; - } - - if (lowThrottleZeroAxis[YAW]) { - actuatorDesired.Yaw = 0.0f; - } - } - // modify thrust according to 1/cos(bank angle) // to maintain same altitdue with changing bank angle // but without manually adjusting thrust @@ -910,12 +894,7 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) weak_leveling_max = settings.MaxWeakLevelingRate; // Whether to zero the PID integrals while thrust is low - lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE; - - // Whether to suppress (zero) the StabilizationDesired output for each axis while disarmed or thrust is low - lowThrottleZeroAxis[ROLL] = settings.LowThrottleZeroAxis.Roll == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; - lowThrottleZeroAxis[PITCH] = settings.LowThrottleZeroAxis.Pitch == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; - lowThrottleZeroAxis[YAW] = settings.LowThrottleZeroAxis.Yaw == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; + lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE; // The dT has some jitter iteration to iteration that we don't want to // make thie result unpredictable. Still, it's nicer to specify the constant diff --git a/shared/uavobjectdefinition/actuatorsettings.xml b/shared/uavobjectdefinition/actuatorsettings.xml index c32be9c6c..051fdceb1 100644 --- a/shared/uavobjectdefinition/actuatorsettings.xml +++ b/shared/uavobjectdefinition/actuatorsettings.xml @@ -8,6 +8,7 @@ + diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index 1a9a3d601..8aeaff3a5 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -41,8 +41,6 @@ - - From 2f57ea2ab35239bfed19fbbade2705a57475919b Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 2 Mar 2014 18:59:36 +0100 Subject: [PATCH 09/18] added missing initializations to allow boot without manualcontrol module --- flight/modules/Actuator/actuator.c | 1 + flight/modules/Stabilization/stabilization.c | 4 ++++ 2 files changed, 5 insertions(+) diff --git a/flight/modules/Actuator/actuator.c b/flight/modules/Actuator/actuator.c index 889d5f634..dbe875a59 100644 --- a/flight/modules/Actuator/actuator.c +++ b/flight/modules/Actuator/actuator.c @@ -125,6 +125,7 @@ int32_t ActuatorInitialize() MixerSettingsConnectCallback(MixerSettingsUpdatedCb); // Listen for ActuatorDesired updates (Primary input to this module) + AccessoryDesiredInitialize(); ActuatorDesiredInitialize(); queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); ActuatorDesiredConnectQueue(queue); diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 51b07209d..2594853fd 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -171,6 +171,10 @@ int32_t StabilizationInitialize() PIOS_STATIC_ASSERT(NUM_FMS_POSITIONS == sizeof(((FlightModeSettingsData *)0)->FlightModePosition) / sizeof((((FlightModeSettingsData *)0)->FlightModePosition)[0])); // Initialize variables + ManualControlCommandInitialize(); + ManualControlSettingsInitialize(); + FlightStatusInitialize(); + StabilizationDesiredInitialize(); StabilizationSettingsInitialize(); StabilizationBankInitialize(); StabilizationSettingsBank1Initialize(); From 564a9912227c7665e438c8bcc3adc1ca47068b0e Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 2 Mar 2014 19:11:49 +0100 Subject: [PATCH 10/18] adjusted Stabilization stack size to get rid of stack alarm --- flight/modules/Stabilization/stabilization.c | 2 +- flight/targets/boards/coptercontrol/firmware/inc/pios_config.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index 2594853fd..e97d11681 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -76,7 +76,7 @@ #if defined(PIOS_STABILIZATION_STACK_SIZE) #define STACK_SIZE_BYTES PIOS_STABILIZATION_STACK_SIZE #else -#define STACK_SIZE_BYTES 840 +#define STACK_SIZE_BYTES 860 #endif #define TASK_PRIORITY (tskIDLE_PRIORITY + 3) // FLIGHT CONTROL priority diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h index 2b9417616..698da7ba6 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h @@ -164,7 +164,7 @@ #else #define PIOS_SYSTEM_STACK_SIZE 660 #endif -#define PIOS_STABILIZATION_STACK_SIZE 790 +/*#define PIOS_STABILIZATION_STACK_SIZE 790*/ #define PIOS_TELEM_STACK_SIZE 540 #define PIOS_EVENTDISPATCHER_STACK_SIZE 160 From 12eed41e58761988ce4167a27fbf76c2b423cf0e Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 2 Mar 2014 20:06:05 +0100 Subject: [PATCH 11/18] OP-1216 modified alarms for manualcontrol/receiver to reflect new architecture --- .../modules/ManualControl/altitudehandler.c | 2 +- flight/modules/ManualControl/armhandler.c | 18 ++++++------ flight/modules/ManualControl/manualcontrol.c | 28 +++++++++++++------ .../ManualControl/pathfollowerhandler.c | 2 +- .../modules/ManualControl/stabilizedhandler.c | 1 + flight/modules/Receiver/receiver.c | 18 ++++++------ .../diagrams/default/system-health.svg | 18 ++++++------ ...l-Critical.html => Receiver-Critical.html} | 0 ...rol-Warning.html => Receiver-Warning.html} | 0 ...l-Critical.html => Receiver-Critical.html} | 0 ...rol-Warning.html => Receiver-Warning.html} | 0 .../src/plugins/systemhealth/systemhealth.qrc | 8 +++--- shared/uavobjectdefinition/systemalarms.xml | 1 + 13 files changed, 54 insertions(+), 42 deletions(-) rename ground/openpilotgcs/src/plugins/systemhealth/html/{ManualControl-Critical.html => Receiver-Critical.html} (100%) rename ground/openpilotgcs/src/plugins/systemhealth/html/{ManualControl-Warning.html => Receiver-Warning.html} (100%) rename ground/openpilotgcs/src/plugins/systemhealth/html/fr/{ManualControl-Critical.html => Receiver-Critical.html} (100%) rename ground/openpilotgcs/src/plugins/systemhealth/html/fr/{ManualControl-Warning.html => Receiver-Warning.html} (100%) diff --git a/flight/modules/ManualControl/altitudehandler.c b/flight/modules/ManualControl/altitudehandler.c index 451177098..0ca5a0d32 100644 --- a/flight/modules/ManualControl/altitudehandler.c +++ b/flight/modules/ManualControl/altitudehandler.c @@ -122,7 +122,7 @@ void altitudeHandler(bool newinit) #else /* if defined(REVOLUTION) */ void altitudeHandler(__attribute__((unused)) bool newinit) { - PIOS_Assert(0); // should not be called + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); // should not be called } #endif // REVOLUTION diff --git a/flight/modules/ManualControl/armhandler.c b/flight/modules/ManualControl/armhandler.c index b7260eaae..797a6cf1c 100644 --- a/flight/modules/ManualControl/armhandler.c +++ b/flight/modules/ManualControl/armhandler.c @@ -64,16 +64,16 @@ static bool forcedDisArm(void); */ void armHandler(bool newinit) { - static portTickType lastSysTime; static ArmState_t armState; if (newinit) { AccessoryDesiredInitialize(); - lastSysTime = xTaskGetTickCount(); setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); - armState = ARM_STATE_DISARMED; + armState = ARM_STATE_DISARMED; } + portTickType sysTime = xTaskGetTickCount(); + FlightModeSettingsData settings; FlightModeSettingsGet(&settings); ManualControlCommandData cmd; @@ -190,7 +190,7 @@ void armHandler(bool newinit) // only allow arming if it's OK too if (manualArm && okToArm()) { - armedDisarmStart = lastSysTime; + armedDisarmStart = sysTime; armState = ARM_STATE_ARMING_MANUAL; } break; @@ -198,7 +198,7 @@ void armHandler(bool newinit) case ARM_STATE_ARMING_MANUAL: setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING); - if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings.ArmingSequenceTime)) { + if (manualArm && (timeDifferenceMs(armedDisarmStart, sysTime) > settings.ArmingSequenceTime)) { armState = ARM_STATE_ARMED; } else if (!manualArm) { armState = ARM_STATE_DISARMED; @@ -208,27 +208,27 @@ void armHandler(bool newinit) 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; + armedDisarmStart = sysTime; 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)) { + 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 = lastSysTime; + armedDisarmStart = sysTime; armState = ARM_STATE_DISARMING_MANUAL; } break; case ARM_STATE_DISARMING_MANUAL: // arming switch disarms immediately, - if (manualDisarm && (armSwitch || (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings.DisarmingSequenceTime))) { + if (manualDisarm && (armSwitch || (timeDifferenceMs(armedDisarmStart, sysTime) > settings.DisarmingSequenceTime))) { armState = ARM_STATE_DISARMED; } else if (!manualDisarm) { armState = ARM_STATE_ARMED; diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index aebfee0f0..7534eba47 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -83,6 +83,14 @@ static controlHandler handler_ALTITUDE = { }, .handler = &altitudeHandler, }; +static controlHandler handler_AUTOTUNE = { + .controlChain = { + .Stabilization = false, + .PathFollower = false, + .PathPlanner = false, + }, + .handler = NULL, +}; static controlHandler handler_PATHFOLLOWER = { .controlChain = { @@ -127,6 +135,8 @@ int32_t ManualControlStart() ManualControlSettingsConnectCallback(configurationUpdatedCb); ManualControlCommandConnectCallback(commandUpdatedCb); + // clear alarms + AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); // Make sure unarmed on power up armHandler(true); @@ -207,19 +217,19 @@ static void manualControlTask(void) handler = &handler_ALTITUDE; break; case FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE: - handler = NULL; + handler = &handler_AUTOTUNE; break; // There is no default, so if a flightmode is forgotten the compiler can throw a warning! } - if (handler) { - bool newinit = false; - if (flightStatus.FlightMode != newMode) { - flightStatus.ControlChain = handler->controlChain; - flightStatus.FlightMode = newMode; - FlightStatusSet(&flightStatus); - newinit = true; - } + bool newinit = false; + if (flightStatus.FlightMode != newMode) { + flightStatus.ControlChain = handler->controlChain; + flightStatus.FlightMode = newMode; + FlightStatusSet(&flightStatus); + newinit = true; + } + if (handler->handler) { handler->handler(newinit); } } diff --git a/flight/modules/ManualControl/pathfollowerhandler.c b/flight/modules/ManualControl/pathfollowerhandler.c index 1820d0778..0f9bfe902 100644 --- a/flight/modules/ManualControl/pathfollowerhandler.c +++ b/flight/modules/ManualControl/pathfollowerhandler.c @@ -120,7 +120,7 @@ void pathFollowerHandler(bool newinit) #else /* if defined(REVOLUTION) */ void pathFollowerHandler(__attribute__((unused)) bool newinit) { - PIOS_Assert(0); // should not be called + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); // should not be called } #endif // REVOLUTION diff --git a/flight/modules/ManualControl/stabilizedhandler.c b/flight/modules/ManualControl/stabilizedhandler.c index 8d3a00295..d7912d936 100644 --- a/flight/modules/ManualControl/stabilizedhandler.c +++ b/flight/modules/ManualControl/stabilizedhandler.c @@ -80,6 +80,7 @@ void stabilizedHandler(bool newinit) 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); stab_settings = cast_struct_to_array(settings.Stabilization1Settings, settings.Stabilization1Settings.Roll); return; } diff --git a/flight/modules/Receiver/receiver.c b/flight/modules/Receiver/receiver.c index 8182e3fb1..579ae0e89 100644 --- a/flight/modules/Receiver/receiver.c +++ b/flight/modules/Receiver/receiver.c @@ -260,7 +260,7 @@ static void receiverTask(__attribute__((unused)) void *parameters) && (settings.ChannelGroups.FlightMode >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_INVALID || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_NODRIVER))) { - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); + AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_CRITICAL); cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; ManualControlCommandSet(&cmd); @@ -324,32 +324,32 @@ static void receiverTask(__attribute__((unused)) void *parameters) if (settings.FailsafeFlightModeSwitchPosition >= 0 && settings.FailsafeFlightModeSwitchPosition < settings.FlightModeNumber) { cmd.FlightModeSwitchPosition = (uint8_t)settings.FailsafeFlightModeSwitchPosition; } - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING); AccessoryDesiredData accessory; // Set Accessory 0 if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = settings.FailsafeChannel.Accessory0; if (AccessoryDesiredInstSet(0, &accessory) != 0) { - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING); } } // Set Accessory 1 if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = settings.FailsafeChannel.Accessory1; if (AccessoryDesiredInstSet(1, &accessory) != 0) { - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING); } } // Set Accessory 2 if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = settings.FailsafeChannel.Accessory2; if (AccessoryDesiredInstSet(2, &accessory) != 0) { - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING); } } } else if (valid_input_detected) { - AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); + AlarmsClear(SYSTEMALARMS_ALARM_RECEIVER); // Scale channels to -1 -> +1 range cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]; @@ -411,7 +411,7 @@ static void receiverTask(__attribute__((unused)) void *parameters) applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT); #endif if (AccessoryDesiredInstSet(0, &accessory) != 0) { - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING); } } // Set Accessory 1 @@ -421,7 +421,7 @@ static void receiverTask(__attribute__((unused)) void *parameters) applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT); #endif if (AccessoryDesiredInstSet(1, &accessory) != 0) { - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING); } } // Set Accessory 2 @@ -432,7 +432,7 @@ static void receiverTask(__attribute__((unused)) void *parameters) #endif if (AccessoryDesiredInstSet(2, &accessory) != 0) { - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING); } } } diff --git a/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg b/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg index fb82a980c..10c6a9d53 100644 --- a/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg +++ b/ground/openpilotgcs/share/openpilotgcs/diagrams/default/system-health.svg @@ -701,7 +701,7 @@