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..d33772606 100644 --- a/flight/modules/Actuator/actuator.c +++ b/flight/modules/Actuator/actuator.c @@ -129,6 +129,9 @@ int32_t ActuatorInitialize() queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); ActuatorDesiredConnectQueue(queue); + // Register AccessoryDesired (Secondary input to this module) + AccessoryDesiredInitialize(); + // Primary output of this module ActuatorCommandInitialize(); @@ -166,6 +169,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 +226,41 @@ 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); + } + + bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED; + + // safety settings + if (!armed) { + throttleDesired = 0; + } + if (throttleDesired <= 0.00f || !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 MixerStatusGet(&mixerStatus); @@ -238,18 +279,18 @@ 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 activeThrottle = (throttleDesired < 0.00f || throttleDesired > 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 +303,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: @@ -295,7 +335,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters) continue; } - if ((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO)) { + if ((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO)) { status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dTSeconds); } else { status[ct] = -1; @@ -317,6 +357,16 @@ static void actuatorTask(__attribute__((unused)) void *parameters) } } + // Reversable Motors are like Motors but go to neutral instead of minimum + if (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) { + // If not armed or motor is inactive - no "spinwhilearmed" for this engine type + if (!armed || !activeThrottle) { + filterAccumulator[ct] = 0; + lastResult[ct] = 0; + status[ct] = 0; // force neutral throttle + } + } + // If an accessory channel is selected for direct bypass mode // In this configuration the accessory channel is scaled and mapped // directly to output. Note: THERE IS NO SAFETY CHECK HERE FOR ARMING @@ -419,6 +469,7 @@ float ProcessMixer(const int index, const float curve1, const float curve2, (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_PITCH] / 128.0f) * desired->Pitch) + (((float)mixer->matrix[MIXERSETTINGS_MIXER1VECTOR_YAW] / 128.0f) * desired->Yaw); + // note: no feedforward for reversable motors yet for safety reasons if (mixer->type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { if (result < 0.0f) { // idle throttle result = 0.0f; @@ -537,7 +588,7 @@ static void setFailsafe(const ActuatorSettingsData *actuatorSettings, const Mixe for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n) { if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) { Channel[n] = actuatorSettings->ChannelMin[n]; - } else if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_SERVO) { + } else if (mixers[n].type == MIXERSETTINGS_MIXER1TYPE_SERVO || mixers[n].type == MIXERSETTINGS_MIXER1TYPE_REVERSABLEMOTOR) { Channel[n] = actuatorSettings->ChannelNeutral[n]; } else { Channel[n] = 0; diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index ce7c556c8..7bf98bfbf 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -116,7 +116,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; @@ -129,7 +129,7 @@ static void altitudeHoldTask(void) default: pid_zero(&pid0); pid_zero(&pid1); - StabilizationDesiredThrottleGet(&startThrottle); + StabilizationDesiredThrustGet(&startThrust); PIOS_CALLBACKSCHEDULER_Schedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER); return; @@ -162,30 +162,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..cb602536e 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" @@ -182,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; @@ -235,8 +232,6 @@ static void pathfollowerTask(__attribute__((unused)) void *parameters) courseIntegral = 0; speedIntegral = 0; powerIntegral = 0; - - break; } PathStatusSet(&pathStatus); } @@ -357,10 +352,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 +415,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 +469,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 +486,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 +496,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 +511,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/altitudehandler.c b/flight/modules/ManualControl/altitudehandler.c new file mode 100644 index 000000000..0ca5a0d32 --- /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) +{ + 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 new file mode 100644 index 000000000..b16f1e39b --- /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 ArmState_t armState; + + if (newinit) { + AccessoryDesiredInitialize(); + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); + armState = ARM_STATE_DISARMED; + } + + portTickType sysTime = xTaskGetTickCount(); + + FlightModeSettingsData settings; + FlightModeSettingsGet(&settings); + ManualControlCommandData cmd; + ManualControlCommandGet(&cmd); + AccessoryDesiredData acc; + + bool lowThrottle = cmd.Throttle < 0; + + bool armSwitch = false; + + switch (settings.Arming) { + case FLIGHTMODESETTINGS_ARMING_ACCESSORY0: + AccessoryDesiredInstGet(0, &acc); + armSwitch = true; + break; + case FLIGHTMODESETTINGS_ARMING_ACCESSORY1: + AccessoryDesiredInstGet(1, &acc); + armSwitch = true; + break; + case FLIGHTMODESETTINGS_ARMING_ACCESSORY2: + AccessoryDesiredInstGet(2, &acc); + armSwitch = true; + break; + default: + break; + } + + // immediate disarm on switch + if (armSwitch && acc.AccessoryVal <= -ARMED_THRESHOLD) { + lowThrottle = true; + } + + if (forcedDisArm()) { + // PathPlanner forces explicit disarming due to error condition (crash, impact, fire, ...) + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); + return; + } + + if (settings.Arming == FLIGHTMODESETTINGS_ARMING_ALWAYSDISARMED) { + // In this configuration we always disarm + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); + return; + } + + + // The throttle is not low, in case we where arming or disarming, abort + if (!lowThrottle) { + switch (armState) { + case ARM_STATE_DISARMING_MANUAL: + case ARM_STATE_DISARMING_TIMEOUT: + armState = ARM_STATE_ARMED; + break; + case ARM_STATE_ARMING_MANUAL: + armState = ARM_STATE_DISARMED; + break; + default: + // Nothing needs to be done in the other states + break; + } + return; + } + + // The rest of these cases throttle is low + if (settings.Arming == FLIGHTMODESETTINGS_ARMING_ALWAYSARMED) { + // In this configuration, we go into armed state as soon as the throttle is low, never disarm + setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); + return; + } + + // When the configuration is not "Always armed" and no "Always disarmed", + // the state will not be changed when the throttle is not low + static portTickType armedDisarmStart; + float armingInputLevel = 0; + + // Calc channel see assumptions7 + switch (settings.Arming) { + case FLIGHTMODESETTINGS_ARMING_ROLLLEFT: + armingInputLevel = 1.0f * cmd.Roll; + break; + case FLIGHTMODESETTINGS_ARMING_ROLLRIGHT: + armingInputLevel = -1.0f * cmd.Roll; + break; + case FLIGHTMODESETTINGS_ARMING_PITCHFORWARD: + armingInputLevel = 1.0f * cmd.Pitch; + break; + case FLIGHTMODESETTINGS_ARMING_PITCHAFT: + armingInputLevel = -1.0f * cmd.Pitch; + break; + case FLIGHTMODESETTINGS_ARMING_YAWLEFT: + armingInputLevel = 1.0f * cmd.Yaw; + break; + case FLIGHTMODESETTINGS_ARMING_YAWRIGHT: + armingInputLevel = -1.0f * cmd.Yaw; + break; + case FLIGHTMODESETTINGS_ARMING_ACCESSORY0: + case FLIGHTMODESETTINGS_ARMING_ACCESSORY1: + case FLIGHTMODESETTINGS_ARMING_ACCESSORY2: + armingInputLevel = -1.0f * acc.AccessoryVal; + break; + } + + bool manualArm = false; + bool manualDisarm = false; + + if (armingInputLevel <= -ARMED_THRESHOLD) { + manualArm = true; + } else if (armingInputLevel >= +ARMED_THRESHOLD) { + manualDisarm = true; + } + + switch (armState) { + case ARM_STATE_DISARMED: + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); + + // only allow arming if it's OK too + if (manualArm && okToArm()) { + armedDisarmStart = sysTime; + armState = ARM_STATE_ARMING_MANUAL; + } + break; + + case ARM_STATE_ARMING_MANUAL: + setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING); + + if (manualArm && (timeDifferenceMs(armedDisarmStart, sysTime) > settings.ArmingSequenceTime)) { + armState = ARM_STATE_ARMED; + } else if (!manualArm) { + armState = ARM_STATE_DISARMED; + } + break; + + case ARM_STATE_ARMED: + // When we get here, the throttle is low, + // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled + armedDisarmStart = sysTime; + armState = ARM_STATE_DISARMING_TIMEOUT; + setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); + break; + + case ARM_STATE_DISARMING_TIMEOUT: + // We get here when armed while throttle low, even when the arming timeout is not enabled + if ((settings.ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, sysTime) > settings.ArmedTimeout)) { + armState = ARM_STATE_DISARMED; + } + + // Switch to disarming due to manual control when needed + if (manualDisarm) { + armedDisarmStart = sysTime; + armState = ARM_STATE_DISARMING_MANUAL; + } + break; + + case ARM_STATE_DISARMING_MANUAL: + // arming switch disarms immediately, + if (manualDisarm && (timeDifferenceMs(armedDisarmStart, sysTime) > settings.DisarmingSequenceTime)) { + armState = ARM_STATE_DISARMED; + } else if (!manualDisarm) { + armState = ARM_STATE_ARMED; + } + break; + } // End Switch +} + +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.Receiver == 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 09b819a01..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 @@ -61,54 +88,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 \ - ( \ - ((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 ea1c41ff6..95c06de72 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,29 +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 "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) @@ -64,79 +51,98 @@ #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_AUTOTUNE = { + .controlChain = { + .Stabilization = false, + .PathFollower = false, + .PathPlanner = false, + }, + .handler = NULL, +}; + +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, ManualControlSettingsData *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 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); + + // clear alarms + AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); + + // 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; } @@ -147,16 +153,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; } @@ -165,1103 +170,75 @@ MODULE_INITCALL(ManualControlInitialize, ManualControlStart); /** * Module task */ -static void manualControlTask(__attribute__((unused)) void *parameters) +static void manualControlTask(void) { - ManualControlSettingsData settings; + // 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 }; - - 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); - - /* 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 > MANUALCONTROLSETTINGS_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 = -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) { - FlightStatusGet(&flightStatus); - - cmd.FlightModeSwitchPosition = (uint8_t)settings.FailsafeBehavior - 1; - flightStatus.FlightMode = settings.FlightModePosition[settings.FailsafeBehavior - 1]; - FlightStatusSet(&flightStatus); - } - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - - AccessoryDesiredData accessory; - // Set Accessory 0 - if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { - accessory.AccessoryVal = 0; - 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; - 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; - 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]; - } - - 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 (settings.Arming == MANUALCONTROLSETTINGS_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 (settings.Arming == MANUALCONTROLSETTINGS_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 (settings.Arming == MANUALCONTROLSETTINGS_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, flightMode, &cmd); - } - - // Process arming outside conditional so system will disarm when disconnected - processArm(&cmd, &settings, 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, &settings); - 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.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; - ActuatorDesiredSet(&actuator); -} - -static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualControlSettingsData *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.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; - 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); - ManualControlSettingsData settings; - ManualControlSettingsGet(&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 throttle - float throttleRate; - uint8_t throttleExp; - - static uint8_t flightMode; - static bool newaltitude = true; - - FlightStatusFlightModeGet(&flightMode); - - AltitudeHoldDesiredData altitudeHoldDesiredData; - AltitudeHoldDesiredGet(&altitudeHoldDesiredData); - - AltitudeHoldSettingsThrottleExpGet(&throttleExp); - AltitudeHoldSettingsThrottleRateGet(&throttleRate); - - 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; - AltitudeHoldSettingsCutThrottleWhenZeroGet(&cutOff); - if (cutOff && cmd->Throttle < 0) { - // Cut throttle if desired - altitudeHoldDesiredData.SetPoint = cmd->Throttle; - altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_THROTTLE; - newaltitude = true; - } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle > 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.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); - 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) { + // Depending on the mode update the Stabilization or Actuator objects + controlHandler *handler = &handler_MANUAL; + switch (newMode) { 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, ManualControlSettingsData *settings, int8_t armSwitch) -{ - bool lowThrottle = cmd->Throttle < 0; - - /** - * 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: - 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 = &handler_AUTOTUNE; + 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; - } + bool newinit = false; - if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) { - // In this configuration we always disarm - setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); - } else { - // Not really needed since this function not called when disconnected - if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { - lowThrottle = true; - } + // FlightMode needs to be set correctly on first run (otherwise ControlChain is invalid) + static bool firstRun = 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 == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) { - // In this configuration, we go into armed state as soon as the throttle is low, never disarm - setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); - return; - } - - // When the configuration is not "Always armed" and no "Always disarmed", - // the state will not be changed when the throttle is not low - static portTickType armedDisarmStart; - float armingInputLevel = 0; - - // Calc channel see assumptions7 - switch (settings->Arming) { - case MANUALCONTROLSETTINGS_ARMING_ROLLLEFT: - armingInputLevel = 1.0f * cmd->Roll; - break; - case MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT: - armingInputLevel = -1.0f * cmd->Roll; - break; - case MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD: - armingInputLevel = 1.0f * cmd->Pitch; - break; - case MANUALCONTROLSETTINGS_ARMING_PITCHAFT: - armingInputLevel = -1.0f * cmd->Pitch; - break; - case MANUALCONTROLSETTINGS_ARMING_YAWLEFT: - armingInputLevel = 1.0f * cmd->Yaw; - break; - case MANUALCONTROLSETTINGS_ARMING_YAWRIGHT: - armingInputLevel = -1.0f * cmd->Yaw; - break; - case MANUALCONTROLSETTINGS_ARMING_ACCESSORY0: - case MANUALCONTROLSETTINGS_ARMING_ACCESSORY1: - case MANUALCONTROLSETTINGS_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, 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 = settings->FlightModePosition[pos]; - - if (flightStatus.FlightMode != newMode) { - flightStatus.FlightMode = newMode; + if (flightStatus.FlightMode != newMode || firstRun) { + firstRun = false; + flightStatus.ControlChain = handler->controlChain; + flightStatus.FlightMode = newMode; FlightStatusSet(&flightStatus); + newinit = true; + } + if (handler->handler) { + handler->handler(newinit); } } -/** - * @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 */ @@ -1270,6 +247,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..0f9bfe902 --- /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) +{ + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); // 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..d7912d936 --- /dev/null +++ b/flight/modules/ManualControl/stabilizedhandler.c @@ -0,0 +1,143 @@ +/** + ****************************************************************************** + * @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 + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); + 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/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index 632f4a8e9..8cc50c4aa 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -42,7 +42,7 @@ #include "velocitystate.h" #include "waypoint.h" #include "waypointactive.h" -#include "manualcontrolsettings.h" +#include "flightmodesettings.h" #include #include "paths.h" @@ -139,7 +139,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 @@ -171,8 +171,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/Receiver/receiver.c b/flight/modules/Receiver/receiver.c new file mode 100644 index 000000000..579ae0e89 --- /dev/null +++ b/flight/modules/Receiver/receiver.c @@ -0,0 +1,673 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup ReceiverModule Manual Control Module + * @brief Provide manual control or allow it alter flight mode. + * @{ + * + * Reads in the ManualControlCommand from receiver then + * pass it to ManualControl + * + * @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 + * + *****************************************************************************/ +/* + * 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 +#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_RECEIVER_STACK_SIZE) +#define STACK_SIZE_BYTES PIOS_RECEIVER_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 + +// Private variables +static xTaskHandle taskHandle; +static portTickType lastSysTime; + +#ifdef USE_INPUT_LPF +static portTickType lastSysTimeLPF; +static float inputFiltered[MANUALCONTROLSETTINGS_RESPONSETIME_NUMELEM]; +#endif + +// Private functions +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 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 \ + ( \ + ((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 ReceiverStart() +{ + // Start main task + 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 + + return 0; +} + +/** + * Module initialization + */ +int32_t ReceiverInitialize() +{ + /* Check the assumptions about uavobject enum's are correct */ + PIOS_STATIC_ASSERT(assumptions); + + ManualControlCommandInitialize(); + ReceiverActivityInitialize(); + ManualControlSettingsInitialize(); + + return 0; +} +MODULE_INITCALL(ReceiverInitialize, ReceiverStart); + +/** + * Module task + */ +static void receiverTask(__attribute__((unused)) void *parameters) +{ + ManualControlSettingsData settings; + ManualControlCommandData cmd; + FlightStatusData flightStatus; + + 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(); + + // Whenever the configuration changes, make sure it is safe to fly + + ManualControlCommandGet(&cmd); + FlightStatusGet(&flightStatus); + + /* 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); + SystemSettingsThrustControlGet(&thrustType); + + /* Update channel activity monitor */ + if (flightStatus.Armed == FLIGHTSTATUS_ARMED_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); + } + } + + 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 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_RECEIVER, SYSTEMALARMS_ALARM_CRITICAL); + 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 SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE: + cmd.Thrust = cmd.Collective; + break; + default: + break; + } + if (settings.FailsafeFlightModeSwitchPosition >= 0 && settings.FailsafeFlightModeSwitchPosition < settings.FlightModeNumber) { + cmd.FlightModeSwitchPosition = (uint8_t)settings.FailsafeFlightModeSwitchPosition; + } + 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_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_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_RECEIVER, SYSTEMALARMS_ALARM_WARNING); + } + } + } else if (valid_input_detected) { + AlarmsClear(SYSTEMALARMS_ALARM_RECEIVER); + + // 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_RECEIVER, 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_RECEIVER, 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_RECEIVER, SYSTEMALARMS_ALARM_WARNING); + } + } + } + + // 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 */ + } +} + +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; +} + +/** + * 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 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 + +/** + * @} + * @} + */ diff --git a/flight/modules/Stabilization/stabilization.c b/flight/modules/Stabilization/stabilization.c index a956bb62e..e97d11681 100644 --- a/flight/modules/Stabilization/stabilization.c +++ b/flight/modules/Stabilization/stabilization.c @@ -49,7 +49,8 @@ #include "gyrostate.h" #include "flightstatus.h" #include "manualcontrolsettings.h" -#include "manualcontrol.h" // Just to get a macro +#include "manualcontrolcommand.h" +#include "flightmodesettings.h" #include "taskinfo.h" // Math libraries @@ -75,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 @@ -103,15 +104,14 @@ 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]; 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,9 +168,13 @@ 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 + ManualControlCommandInitialize(); + ManualControlSettingsInitialize(); + FlightStatusInitialize(); + StabilizationDesiredInitialize(); StabilizationSettingsInitialize(); StabilizationBankInitialize(); StabilizationSettingsBank1Initialize(); @@ -205,6 +209,7 @@ static void stabilizationTask(__attribute__((unused)) void *parameters) ActuatorDesiredData actuatorDesired; StabilizationDesiredData stabDesired; + float throttleDesired; RateDesiredData rateDesired; AttitudeStateData attitudeState; GyroStateData gyroStateData; @@ -236,6 +241,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,26 +598,11 @@ 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) { - if (lowThrottleZeroAxis[ROLL]) { - actuatorDesired.Roll = 0.0f; - } - - if (lowThrottleZeroAxis[PITCH]) { - actuatorDesired.Pitch = 0.0f; - } - - if (lowThrottleZeroAxis[YAW]) { - actuatorDesired.Yaw = 0.0f; - } - } - - // 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 +636,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,20 +644,20 @@ 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; } } } - 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 @@ -676,7 +667,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,13 +897,8 @@ 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 - lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE; - - // Whether to suppress (zero) the StabilizationDesired output for each axis while disarmed or throttle 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; + // Whether to zero the PID integrals while thrust is low + 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 @@ -933,10 +919,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..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" @@ -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. */ @@ -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; @@ -263,12 +261,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; - - break; + thrustOffset = stabDesired.Thrust; } AlarmsClear(SYSTEMALARMS_ALARM_GUIDANCE); @@ -550,10 +546,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 +649,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 +666,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 5aba76d2b..6c0038305 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 @@ -98,6 +99,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/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h index 2b9417616..8acb0e0c5 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h @@ -164,7 +164,6 @@ #else #define PIOS_SYSTEM_STACK_SIZE 660 #endif -#define PIOS_STABILIZATION_STACK_SIZE 790 #define PIOS_TELEM_STACK_SIZE 540 #define PIOS_EVENTDISPATCHER_STACK_SIZE 160 diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index 26b49d13a..5b89372a1 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/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 059c7f376..72f51cbec 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/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/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/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index d8deca113..566c3820b 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -62,6 +62,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 318137d47..7044cada6 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/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 @@