From 406c381071b529d71f6c5e0e911cab81fbbc9a7c Mon Sep 17 00:00:00 2001 From: Laurent Lalanne Date: Sat, 5 Mar 2016 18:34:17 +0100 Subject: [PATCH] LP-206 Add Always stabilized accessory switch - Fix: Allow Accessory3 for arming --- flight/modules/Actuator/actuator.c | 18 ++--- flight/modules/ManualControl/armhandler.c | 14 ++++ flight/modules/ManualControl/manualcontrol.c | 53 ++++++++++++-- .../src/plugins/config/configoutputwidget.cpp | 45 ++++++++++++ .../src/plugins/config/configoutputwidget.h | 2 + ground/gcs/src/plugins/config/output.ui | 73 ++++++++++++++++++- .../flightmodesettings.xml | 11 ++- shared/uavobjectdefinition/flightstatus.xml | 1 + 8 files changed, 198 insertions(+), 19 deletions(-) diff --git a/flight/modules/Actuator/actuator.c b/flight/modules/Actuator/actuator.c index c0adc82f1..54b6fba7f 100644 --- a/flight/modules/Actuator/actuator.c +++ b/flight/modules/Actuator/actuator.c @@ -98,7 +98,7 @@ static int mixer_settings_count = 2; // Private functions static void actuatorTask(void *parameters); static int16_t scaleChannel(float value, int16_t max, int16_t min, int16_t neutral); -static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral, float maxMotor, float minMotor, bool armed, bool AlwaysStabilizeWhenArmed, float throttleDesired); +static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral, float maxMotor, float minMotor, bool armed, bool alwaysStabilizeWhenArmed, float throttleDesired); static void setFailsafe(); static float MixerCurveFullRangeProportional(const float input, const float *curve, uint8_t elements, bool multirotor); static float MixerCurveFullRangeAbsolute(const float input, const float *curve, uint8_t elements, bool multirotor); @@ -270,10 +270,10 @@ static void actuatorTask(__attribute__((unused)) void *parameters) bool multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR); // check if frame is a multirotor. bool fixedwing = (GetCurrentFrameType() == FRAME_TYPE_FIXED_WING); // check if frame is a fixedwing. bool alwaysArmed = settings.Arming == FLIGHTMODESETTINGS_ARMING_ALWAYSARMED; - bool AlwaysStabilizeWhenArmed = settings.AlwaysStabilizeWhenArmed == FLIGHTMODESETTINGS_ALWAYSSTABILIZEWHENARMED_TRUE; + bool alwaysStabilizeWhenArmed = flightStatus.AlwaysStabilizeWhenArmed == FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_TRUE; if (alwaysArmed) { - AlwaysStabilizeWhenArmed = false; // Do not allow always stabilize when alwaysArmed is active. This is dangerous. + alwaysStabilizeWhenArmed = false; // Do not allow always stabilize when alwaysArmed is active. This is dangerous. } // safety settings if (!armed) { @@ -284,7 +284,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters) // throttleDesired should never be 0 or go below 0. // force set all other controls to zero if throttle is cut (previously set in Stabilization) // todo: can probably remove this - if (!(multirotor && AlwaysStabilizeWhenArmed && armed)) { // we don't do this if this is a multirotor AND AlwaysStabilizeWhenArmed is true and the model is armed + if (!(multirotor && alwaysStabilizeWhenArmed && armed)) { // we don't do this if this is a multirotor AND AlwaysStabilizeWhenArmed is true and the model is armed if (actuatorSettings.LowThrottleZeroAxis.Roll == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) { desired.Roll = 0.00f; } @@ -498,7 +498,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters) maxMotor, minMotor, armed, - AlwaysStabilizeWhenArmed, + alwaysStabilizeWhenArmed, throttleDesired); } else { // else we scale the channel command.Channel[i] = scaleChannel(status[i], @@ -746,7 +746,7 @@ static inline int16_t scaleMotorMoveAndCompress(float valueMotor, int16_t max, i /** * Constrain motor values to keep any one motor value from going too far out of range of another motor */ -static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral, float maxMotor, float minMotor, bool armed, bool AlwaysStabilizeWhenArmed, float throttleDesired) +static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral, float maxMotor, float minMotor, bool armed, bool alwaysStabilizeWhenArmed, float throttleDesired) { int16_t valueScaled; @@ -757,7 +757,7 @@ static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral valueScaled = scaleChannel(value, max, min, neutral); } - // I've added the bool AlwaysStabilizeWhenArmed to this function. Right now we command the motors at min or a range between neutral and max. + // I've added the bool alwaysStabilizeWhenArmed to this function. Right now we command the motors at min or a range between neutral and max. // NEVER should a motor be command at between min and neutral. I don't like the idea of stabilization ever commanding a motor to min, but we give people the option // This prevents motors startup sync issues causing possible ESC failures. @@ -765,8 +765,8 @@ static int16_t scaleMotor(float value, int16_t max, int16_t min, int16_t neutral if (!armed) { // if not armed return min EVERYTIME! valueScaled = min; - } else if (!AlwaysStabilizeWhenArmed && (throttleDesired <= 0.0f) && spinWhileArmed) { - // all motors idle is AlwaysStabilizeWhenArmed is false, throttle is less than or equal to neutral and spin while armed + } else if (!alwaysStabilizeWhenArmed && (throttleDesired <= 0.0f) && spinWhileArmed) { + // all motors idle is alwaysStabilizeWhenArmed is false, throttle is less than or equal to neutral and spin while armed // stabilize when armed? valueScaled = neutral; } else if (!spinWhileArmed && (throttleDesired <= 0.0f)) { diff --git a/flight/modules/ManualControl/armhandler.c b/flight/modules/ManualControl/armhandler.c index dedd4b03c..6157339a5 100644 --- a/flight/modules/ManualControl/armhandler.c +++ b/flight/modules/ManualControl/armhandler.c @@ -106,6 +106,10 @@ void armHandler(bool newinit, FrameType_t frameType) AccessoryDesiredInstGet(2, &acc); armSwitch = true; break; + case FLIGHTMODESETTINGS_ARMING_ACCESSORY3: + AccessoryDesiredInstGet(3, &acc); + armSwitch = true; + break; default: break; } @@ -181,6 +185,7 @@ void armHandler(bool newinit, FrameType_t frameType) case FLIGHTMODESETTINGS_ARMING_ACCESSORY0: case FLIGHTMODESETTINGS_ARMING_ACCESSORY1: case FLIGHTMODESETTINGS_ARMING_ACCESSORY2: + case FLIGHTMODESETTINGS_ARMING_ACCESSORY3: armingInputLevel = -1.0f * acc.AccessoryVal; break; default: @@ -313,6 +318,11 @@ static bool okToArm(void) return false; } + // Avoid arming while AlwaysStabilizeWhenArmed is active + if (flightStatus.AlwaysStabilizeWhenArmed == FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_TRUE) { + return false; + } + return true; break; @@ -321,6 +331,10 @@ static bool okToArm(void) case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF: case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: + // Avoid arming while AlwaysStabilizeWhenArmed is active + if (flightStatus.AlwaysStabilizeWhenArmed == FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_TRUE) { + return false; + } return true; default: diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 56247e836..4610e31f2 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #include @@ -62,6 +63,7 @@ #define ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_LO 0.96f #define ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_HI 1.04f +#define ALWAYSTABILIZEACCESSORY_THRESHOLD 0.50f // defined handlers @@ -166,6 +168,7 @@ int32_t ManualControlInitialize() FlightModeSettingsInitialize(); SystemSettingsInitialize(); StabilizationSettingsInitialize(); + AccessoryDesiredInitialize(); #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES VtolSelfTuningStatsInitialize(); VtolPathFollowerSettingsInitialize(); @@ -218,6 +221,7 @@ static void manualControlTask(void) FlightStatusGet(&flightStatus); ManualControlCommandData cmd; ManualControlCommandGet(&cmd); + AccessoryDesiredData acc; #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES VtolPathFollowerSettingsThrustLimitsData thrustLimits; VtolPathFollowerSettingsThrustLimitsGet(&thrustLimits); @@ -228,6 +232,7 @@ static void manualControlTask(void) uint8_t position = cmd.FlightModeSwitchPosition; uint8_t newMode = flightStatus.FlightMode; + uint8_t newAlwaysStabilized = flightStatus.AlwaysStabilizeWhenArmed; uint8_t newFlightModeAssist = flightStatus.FlightModeAssist; uint8_t newAssistedControlState = flightStatus.AssistedControlState; uint8_t newAssistedThrottleState = flightStatus.AssistedThrottleState; @@ -449,21 +454,57 @@ static void manualControlTask(void) // There is no default, so if a flightmode is forgotten the compiler can throw a warning! } + bool alwaysStabilizedSwitch = false; + + // Check for a AlwaysStabilizeWhenArmed accessory switch + switch (modeSettings.AlwaysStabilizeWhenArmedSwitch) { + case FLIGHTMODESETTINGS_ALWAYSSTABILIZEWHENARMEDSWITCH_ACCESSORY0: + AccessoryDesiredInstGet(0, &acc); + alwaysStabilizedSwitch = true; + break; + case FLIGHTMODESETTINGS_ALWAYSSTABILIZEWHENARMEDSWITCH_ACCESSORY1: + AccessoryDesiredInstGet(1, &acc); + alwaysStabilizedSwitch = true; + break; + case FLIGHTMODESETTINGS_ALWAYSSTABILIZEWHENARMEDSWITCH_ACCESSORY2: + AccessoryDesiredInstGet(2, &acc); + alwaysStabilizedSwitch = true; + break; + case FLIGHTMODESETTINGS_ALWAYSSTABILIZEWHENARMEDSWITCH_ACCESSORY3: + AccessoryDesiredInstGet(3, &acc); + alwaysStabilizedSwitch = true; + break; + default: + break; + } + + if (alwaysStabilizedSwitch) { + if (acc.AccessoryVal <= -ALWAYSTABILIZEACCESSORY_THRESHOLD) { + newAlwaysStabilized = FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_FALSE; + } else if (acc.AccessoryVal >= ALWAYSTABILIZEACCESSORY_THRESHOLD) { + newAlwaysStabilized = FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_TRUE; + } + } else { + newAlwaysStabilized = FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_FALSE; + } + bool newinit = false; // FlightMode needs to be set correctly on first run (otherwise ControlChain is invalid) static bool firstRun = true; - if (flightStatus.FlightMode != newMode || firstRun || + if (flightStatus.AlwaysStabilizeWhenArmed != newAlwaysStabilized || + flightStatus.FlightMode != newMode || firstRun || newFlightModeAssist != flightStatus.FlightModeAssist || newAssistedControlState != flightStatus.AssistedControlState || flightStatus.AssistedThrottleState != newAssistedThrottleState) { firstRun = false; - flightStatus.ControlChain = handler->controlChain; - flightStatus.FlightMode = newMode; - flightStatus.FlightModeAssist = newFlightModeAssist; - flightStatus.AssistedControlState = newAssistedControlState; - flightStatus.AssistedThrottleState = newAssistedThrottleState; + flightStatus.ControlChain = handler->controlChain; + flightStatus.FlightMode = newMode; + flightStatus.AlwaysStabilizeWhenArmed = newAlwaysStabilized; + flightStatus.FlightModeAssist = newFlightModeAssist; + flightStatus.AssistedControlState = newAssistedControlState; + flightStatus.AssistedThrottleState = newAssistedThrottleState; FlightStatusSet(&flightStatus); newinit = true; } diff --git a/ground/gcs/src/plugins/config/configoutputwidget.cpp b/ground/gcs/src/plugins/config/configoutputwidget.cpp index 60b925017..82ac86891 100644 --- a/ground/gcs/src/plugins/config/configoutputwidget.cpp +++ b/ground/gcs/src/plugins/config/configoutputwidget.cpp @@ -41,6 +41,8 @@ #include "mixersettings.h" #include "actuatorcommand.h" #include "actuatorsettings.h" +#include "flightmodesettings.h" +#include "flightstatus.h" #include "systemsettings.h" #include @@ -99,6 +101,13 @@ ConfigOutputWidget::ConfigOutputWidget(QWidget *parent) : ConfigTaskWidget(paren // Associate the buttons with their UAVO fields addWidget(m_ui->spinningArmed); + connect(m_ui->spinningArmed, SIGNAL(clicked(bool)), this, SLOT(updateSpinStabilizeCheckComboBoxes())); + + addUAVObject("FlightModeSettings"); + addWidgetBinding("FlightModeSettings", "AlwaysStabilizeWhenArmedSwitch", m_ui->alwaysStabilizedSwitch); + + connect(FlightStatus::GetInstance(getObjectManager()), SIGNAL(objectUpdated(UAVObject *)), this, SLOT(updateAlwaysStabilizeStatus())); + MixerSettings *mixer = MixerSettings::GetInstance(getObjectManager()); Q_ASSERT(mixer); m_banks << OutputBankControls(mixer, m_ui->chBank1, QColor("#C6ECAE"), m_ui->cb_outputRate1, m_ui->cb_outputMode1); @@ -407,6 +416,8 @@ void ConfigOutputWidget::refreshWidgetsValues(UAVObject *obj) outputChannelForm->setNeutral(neutral); } + updateSpinStabilizeCheckComboBoxes(); + setDirty(dirty); } @@ -446,6 +457,40 @@ void ConfigOutputWidget::updateObjectsFromWidgets() // Apply settings actuatorSettings->setData(actuatorSettingsData); } + + FlightModeSettings *flightModeSettings = FlightModeSettings::GetInstance(getObjectManager()); + Q_ASSERT(flightModeSettings); + + if (flightModeSettings) { + FlightModeSettings::DataFields flightModeSettingsData = flightModeSettings->getData(); + flightModeSettingsData.AlwaysStabilizeWhenArmedSwitch = m_ui->alwaysStabilizedSwitch->currentIndex(); + + // Apply settings + flightModeSettings->setData(flightModeSettingsData); + } +} + +void ConfigOutputWidget::updateSpinStabilizeCheckComboBoxes() +{ + m_ui->alwayStabilizedLabel1->setEnabled(m_ui->spinningArmed->isChecked()); + m_ui->alwayStabilizedLabel2->setEnabled(m_ui->spinningArmed->isChecked()); + m_ui->alwaysStabilizedSwitch->setEnabled(m_ui->spinningArmed->isChecked()); + + if (!m_ui->spinningArmed->isChecked()) { + m_ui->alwaysStabilizedSwitch->setCurrentIndex(FlightModeSettings::ALWAYSSTABILIZEWHENARMEDSWITCH_DISABLED); + } +} + +void ConfigOutputWidget::updateAlwaysStabilizeStatus() +{ + FlightStatus *flightStatusObj = FlightStatus::GetInstance(getObjectManager()); + FlightStatus::DataFields flightStatus = flightStatusObj->getData(); + + if (flightStatus.AlwaysStabilizeWhenArmed == FlightStatus::ALWAYSSTABILIZEWHENARMED_TRUE) { + m_ui->alwayStabilizedLabel2->setText("AlwaysStabilizeWhenArmed is ACTIVE. This prevents arming!."); + } else { + m_ui->alwayStabilizedLabel2->setText("(Really be careful!)."); + } } void ConfigOutputWidget::openHelp() diff --git a/ground/gcs/src/plugins/config/configoutputwidget.h b/ground/gcs/src/plugins/config/configoutputwidget.h index 2663f060c..3dcbfac43 100644 --- a/ground/gcs/src/plugins/config/configoutputwidget.h +++ b/ground/gcs/src/plugins/config/configoutputwidget.h @@ -104,6 +104,8 @@ private: private slots: void updateWarnings(UAVObject *); + void updateSpinStabilizeCheckComboBoxes(); + void updateAlwaysStabilizeStatus(); void stopTests(); virtual void refreshWidgetsValues(UAVObject *obj = NULL); void updateObjectsFromWidgets(); diff --git a/ground/gcs/src/plugins/config/output.ui b/ground/gcs/src/plugins/config/output.ui index 1f5c9577f..7bdf484e3 100644 --- a/ground/gcs/src/plugins/config/output.ui +++ b/ground/gcs/src/plugins/config/output.ui @@ -123,7 +123,7 @@ 0 0 743 - 668 + 662 @@ -817,10 +817,79 @@ - Motors spin at neutral output when armed and throttle below zero (be careful) + Motors spin at neutral output when armed and throttle below zero (Be careful). + + + + 6 + + + 0 + + + + + Qt::Horizontal + + + QSizePolicy::Fixed + + + + 20 + 20 + + + + + + + + Multirotor is Always Stabilized When Armed using: + + + + + + + + 0 + 0 + + + + + 120 + 0 + + + + + + + + (Really be careful!). + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml index aa329d010..b6c4cc89e 100644 --- a/shared/uavobjectdefinition/flightmodesettings.xml +++ b/shared/uavobjectdefinition/flightmodesettings.xml @@ -2,7 +2,7 @@ Settings to control arming and flight mode - + - + + diff --git a/shared/uavobjectdefinition/flightstatus.xml b/shared/uavobjectdefinition/flightstatus.xml index 78dd6a4c5..5f3661cc8 100644 --- a/shared/uavobjectdefinition/flightstatus.xml +++ b/shared/uavobjectdefinition/flightstatus.xml @@ -2,6 +2,7 @@ Contains major flight status information for other modules. +