diff --git a/flight/modules/Battery/battery.c b/flight/modules/Battery/battery.c index 436480bce..8de4b841c 100644 --- a/flight/modules/Battery/battery.c +++ b/flight/modules/Battery/battery.c @@ -8,7 +8,8 @@ * @{ * * @file battery.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Module to read the battery Voltage and Current periodically and set alarms appropriately. * * @see The GNU Public License (GPL) Version 3 @@ -204,7 +205,7 @@ static void onTimer(__attribute__((unused)) UAVObjEvent *ev) // FIXME: should make the battery voltage detection dependent on battery type. /*Not so sure. Some users will want to run their batteries harder than others, so it should be the user's choice. [KDS]*/ - if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.Alarm * flightBatteryData.NbCells) { + if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.Critical * flightBatteryData.NbCells) { AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_CRITICAL); } else if (flightBatteryData.Voltage < batterySettings.CellVoltageThresholds.Warning * flightBatteryData.NbCells) { AlarmsSet(SYSTEMALARMS_ALARM_BATTERY, SYSTEMALARMS_ALARM_WARNING); @@ -255,6 +256,10 @@ static int8_t GetNbCells(const FlightBatterySettingsData *batterySettings, Fligh voltageMin = 3.6f; voltageMax = 4.2f; break; + case FLIGHTBATTERYSETTINGS_TYPE_LIHV: + voltageMin = 3.6f; + voltageMax = 4.35f; + break; case FLIGHTBATTERYSETTINGS_TYPE_A123: voltageMin = 2.01f; voltageMax = 3.59f; diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index b18f8d8cb..8e167d6f4 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -46,6 +46,7 @@ #include #include #include +#include #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES #include #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ @@ -119,9 +120,9 @@ static void commandUpdatedCb(UAVObjEvent *ev); static void manualControlTask(void); #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, FlightModeSettingsData *modeSettings); +static void HandleBatteryFailsafe(uint8_t *position, FlightModeSettingsData *modeSettings); #endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ static void SettingsUpdatedCb(UAVObjEvent *ev); - #define assumptions (assumptions1 && assumptions2 && assumptions3 && assumptions4 && assumptions5 && assumptions6 && assumptions7 && assumptions_flightmode) /** @@ -172,6 +173,7 @@ int32_t ManualControlInitialize() StabilizationSettingsInitialize(); AccessoryDesiredInitialize(); #ifndef PIOS_EXCLUDE_ADVANCED_FEATURES + SystemAlarmsInitialize(); VtolSelfTuningStatsInitialize(); VtolPathFollowerSettingsInitialize(); VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb); @@ -239,6 +241,11 @@ static void manualControlTask(void) uint8_t newFlightModeAssist = flightStatus.FlightModeAssist; uint8_t newAssistedControlState = flightStatus.AssistedControlState; uint8_t newAssistedThrottleState = flightStatus.AssistedThrottleState; + +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES + HandleBatteryFailsafe(&position, &modeSettings); +#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ + if (position < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) { newMode = modeSettings.FlightModePosition[position]; } @@ -249,7 +256,6 @@ static void manualControlTask(void) newMode = flightStatus.FlightMode; position = lastPosition; } - // if a mode change occurs we default the assist mode and states here // to avoid having to add it to all of the below modes that are // otherwise unrelated @@ -526,6 +532,87 @@ static void manualControlTask(void) } } +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES +void HandleBatteryFailsafe(uint8_t *position, FlightModeSettingsData *modeSettings) +{ + // static uint8_t lastInputPosition = -1; + typedef enum { BATTERYFAILSAFE_NONE = 0, BATTERYFAILSAFE_WARNING = 1, BATTERYFAILSAFE_CRITICAL = 2 } batteryfailsafemode_t; + static batteryfailsafemode_t lastFailsafeStatus = BATTERYFAILSAFE_NONE; + static bool failsafeOverridden; + static uint8_t lastFlightPosition; + static uint32_t changeTimestamp; + SystemAlarmsAlarmData alarms; + batteryfailsafemode_t failsafeStatus; + FlightStatusArmedOptions armed; + FlightStatusArmedGet(&armed); + + // reset the status and do not change anything when not armed + if (armed != FLIGHTSTATUS_ARMED_ARMED) { + lastFailsafeStatus = BATTERYFAILSAFE_NONE; + failsafeOverridden = false; + changeTimestamp = PIOS_DELAY_GetRaw(); + lastFlightPosition = *position; + return; + } + + SystemAlarmsAlarmGet(&alarms); + + switch (alarms.Battery) { + case SYSTEMALARMS_ALARM_WARNING: + failsafeStatus = BATTERYFAILSAFE_WARNING; + break; + case SYSTEMALARMS_ALARM_CRITICAL: + failsafeStatus = BATTERYFAILSAFE_CRITICAL; + break; + default: + failsafeStatus = BATTERYFAILSAFE_NONE; + break; + } + uint32_t debounceTimerms = PIOS_DELAY_DiffuS(changeTimestamp) / 1000; + + if (failsafeStatus == lastFailsafeStatus) { + changeTimestamp = PIOS_DELAY_GetRaw(); + } else if ((debounceTimerms < modeSettings->BatteryFailsafeDebounceTimer) || failsafeStatus < lastFailsafeStatus) { + // do not change within the "grace" period and do not "downgrade" the failsafe mode + failsafeStatus = lastFailsafeStatus; + } else { + // a higher failsafe status was met and grace period elapsed. Trigger the new state + lastFailsafeStatus = failsafeStatus; + lastFlightPosition = *position; + failsafeOverridden = false; + } + + if ((failsafeStatus == BATTERYFAILSAFE_NONE) || failsafeOverridden) { + return; + } + + // failsafe has been triggered. Check for override + if (lastFlightPosition != *position) { + // flag the override and reset the grace period + failsafeOverridden = true; + changeTimestamp = PIOS_DELAY_GetRaw(); + return; + } + + switch (failsafeStatus) { + case BATTERYFAILSAFE_CRITICAL: + // if critical is not set, jump to the other case to use the warning setting. + if (modeSettings->BatteryFailsafeSwitchPositions.Critical != -1) { + *position = modeSettings->BatteryFailsafeSwitchPositions.Critical; + break; + } + case BATTERYFAILSAFE_WARNING: + if (modeSettings->BatteryFailsafeSwitchPositions.Warning != -1) { + *position = modeSettings->BatteryFailsafeSwitchPositions.Warning; + } + break; + default: + break; + } +} + +#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ + /** * Called whenever a critical configuration component changes */ diff --git a/ground/gcs/src/plugins/config/configinputwidget.cpp b/ground/gcs/src/plugins/config/configinputwidget.cpp index c0a7f3d8f..6ce93d59f 100644 --- a/ground/gcs/src/plugins/config/configinputwidget.cpp +++ b/ground/gcs/src/plugins/config/configinputwidget.cpp @@ -100,7 +100,7 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : rssiDesiredObj4 = AccessoryDesired::GetInstance(getObjectManager(), 4); actuatorSettingsObj = ActuatorSettings::GetInstance(getObjectManager()); systemSettingsObj = SystemSettings::GetInstance(getObjectManager()); - + hwSettingsObj = HwSettings::GetInstance(getObjectManager()); // Only instance 0 is present if the board is not connected. // The other instances are populated lazily. Q_ASSERT(accessoryDesiredObj0); @@ -177,6 +177,9 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : addWidgetBinding("ManualControlSettings", "FailsafeFlightModeSwitchPosition", ui->failsafeFlightMode, 0, 1, true, new QList(failsafeReloadGroup)); + addWidgetBinding("FlightModeSettings", "BatteryFailsafeSwitchPositions", ui->failsafeBatteryWarningFlightMode, 0, 1, true, new QList(failsafeReloadGroup)); + addWidgetBinding("FlightModeSettings", "BatteryFailsafeSwitchPositions", ui->failsafeBatteryCriticalFlightMode, 1, 1, true, new QList(failsafeReloadGroup)); + // Generate the rows for the failsafe channel form GUI index = 0; foreach(QString name, manualSettingsObj->getField("FailsafeChannel")->getElementNames()) { @@ -252,9 +255,18 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : connect(ui->failsafeFlightMode, SIGNAL(currentIndexChanged(int)), this, SLOT(failsafeFlightModeChanged(int))); connect(ui->failsafeFlightModeCb, SIGNAL(toggled(bool)), this, SLOT(failsafeFlightModeCbToggled(bool))); + connect(ui->failsafeBatteryWarningFlightMode, SIGNAL(currentIndexChanged(int)), this, SLOT(failsafeBatteryWarningFlightModeChanged(int))); + connect(ui->failsafeBatteryWarningFlightModeCb, SIGNAL(toggled(bool)), this, SLOT(failsafeBatteryWarningFlightModeCbToggled(bool))); + + connect(ui->failsafeBatteryCriticalFlightMode, SIGNAL(currentIndexChanged(int)), this, SLOT(failsafeBatteryCriticalFlightModeChanged(int))); + connect(ui->failsafeBatteryCriticalFlightModeCb, SIGNAL(toggled(bool)), this, SLOT(failsafeBatteryCriticalFlightModeCbToggled(bool))); + + addWidget(ui->configurationWizard); addWidget(ui->runCalibration); addWidget(ui->failsafeFlightModeCb); + addWidget(ui->failsafeBatteryWarningFlightModeCb); + addWidget(ui->failsafeBatteryCriticalFlightModeCb); // Wizard wizardUi = new Ui_InputWizardWidget(); @@ -448,9 +460,9 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : void ConfigInputWidget::buildOptionComboBox(QComboBox *combo, UAVObjectField *field, int index, bool applyLimits) { - if (combo == ui->failsafeFlightMode) { + if (combo == ui->failsafeFlightMode || combo == ui->failsafeBatteryCriticalFlightMode || combo == ui->failsafeBatteryWarningFlightMode) { for (quint32 i = 0; i < FlightModeSettings::FLIGHTMODEPOSITION_NUMELEM; i++) { - ui->failsafeFlightMode->addItem(QString("Position %1").arg(i + 1), QVariant(i)); + combo->addItem(QString("Position %1").arg(i + 1), QVariant(i)); } } else { ConfigTaskWidget::buildOptionComboBox(combo, field, index, applyLimits); @@ -1787,88 +1799,40 @@ void ConfigInputWidget::updatePositionSlider() { ManualControlSettings::DataFields manualSettingsDataPriv = manualSettingsObj->getData(); - switch (manualSettingsDataPriv.FlightModeNumber) { - default: - case 6: - ui->fmsModePos6->setEnabled(true); - ui->pidBankSs1_5->setEnabled(true); - ui->assistControlPos6->setEnabled(true); - setComboBoxItemEnabled(ui->failsafeFlightMode, 5); - // pass through - case 5: - ui->fmsModePos5->setEnabled(true); - ui->pidBankSs1_4->setEnabled(true); - ui->assistControlPos5->setEnabled(true); - setComboBoxItemEnabled(ui->failsafeFlightMode, 4); - // pass through - case 4: - ui->fmsModePos4->setEnabled(true); - ui->pidBankSs1_3->setEnabled(true); - ui->assistControlPos4->setEnabled(true); - setComboBoxItemEnabled(ui->failsafeFlightMode, 3); - // pass through - case 3: - ui->fmsModePos3->setEnabled(true); - ui->pidBankSs1_2->setEnabled(true); - ui->assistControlPos3->setEnabled(true); - setComboBoxItemEnabled(ui->failsafeFlightMode, 2); - // pass through - case 2: - ui->fmsModePos2->setEnabled(true); - ui->pidBankSs1_1->setEnabled(true); - ui->assistControlPos2->setEnabled(true); - setComboBoxItemEnabled(ui->failsafeFlightMode, 1); - // pass through - case 1: - ui->fmsModePos1->setEnabled(true); - ui->pidBankSs1_0->setEnabled(true); - ui->assistControlPos1->setEnabled(true); - setComboBoxItemEnabled(ui->failsafeFlightMode, 0); - // pass through - case 0: - break; - } + QWidget *fmsModes[] = { + ui->fmsModePos1, + ui->fmsModePos2, + ui->fmsModePos3, + ui->fmsModePos4, + ui->fmsModePos5, + ui->fmsModePos6 + }; + QWidget *pidBanks[] = { + ui->pidBankSs1_0, + ui->pidBankSs1_1, + ui->pidBankSs1_2, + ui->pidBankSs1_3, + ui->pidBankSs1_4, + ui->pidBankSs1_5 + }; + QWidget *assistControls[] = { + ui->assistControlPos1, + ui->assistControlPos2, + ui->assistControlPos3, + ui->assistControlPos4, + ui->assistControlPos5, + ui->assistControlPos6 + }; - switch (manualSettingsDataPriv.FlightModeNumber) { - case 0: - ui->fmsModePos1->setEnabled(false); - ui->pidBankSs1_0->setEnabled(false); - ui->assistControlPos1->setEnabled(false); - setComboBoxItemEnabled(ui->failsafeFlightMode, 0, false); - // pass through - case 1: - ui->fmsModePos2->setEnabled(false); - ui->pidBankSs1_1->setEnabled(false); - ui->assistControlPos2->setEnabled(false); - setComboBoxItemEnabled(ui->failsafeFlightMode, 1, false); - // pass through - case 2: - ui->fmsModePos3->setEnabled(false); - ui->pidBankSs1_2->setEnabled(false); - ui->assistControlPos3->setEnabled(false); - setComboBoxItemEnabled(ui->failsafeFlightMode, 2, false); - // pass through - case 3: - ui->fmsModePos4->setEnabled(false); - ui->pidBankSs1_3->setEnabled(false); - ui->assistControlPos4->setEnabled(false); - setComboBoxItemEnabled(ui->failsafeFlightMode, 3, false); - // pass through - case 4: - ui->fmsModePos5->setEnabled(false); - ui->pidBankSs1_4->setEnabled(false); - ui->assistControlPos5->setEnabled(false); - setComboBoxItemEnabled(ui->failsafeFlightMode, 4, false); - // pass through - case 5: - ui->fmsModePos6->setEnabled(false); - ui->pidBankSs1_5->setEnabled(false); - ui->assistControlPos6->setEnabled(false); - setComboBoxItemEnabled(ui->failsafeFlightMode, 5, false); - // pass through - case 6: - default: - break; + for (quint32 i = 0; i < FlightModeSettings::FLIGHTMODEPOSITION_NUMELEM; i++) { + bool enabled = i < manualSettingsDataPriv.FlightModeNumber; + + fmsModes[i]->setEnabled(enabled); + pidBanks[i]->setEnabled(enabled); + assistControls[i]->setEnabled(enabled); + setComboBoxItemEnabled(ui->failsafeFlightMode, i, enabled); + setComboBoxItemEnabled(ui->failsafeBatteryCriticalFlightMode, i, enabled); + setComboBoxItemEnabled(ui->failsafeBatteryWarningFlightMode, i, enabled); } QString fmNumber = QString().setNum(manualSettingsDataPriv.FlightModeNumber); @@ -2175,6 +2139,24 @@ void ConfigInputWidget::updateReceiverActivityStatus() } } +void ConfigInputWidget::failsafeBatteryWarningFlightModeChanged(int index) +{ + HwSettings::DataFields hwSettingsData = hwSettingsObj->getData(); + bool batteryModuleEnabled = (hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_BATTERY] == HwSettings::OPTIONALMODULES_ENABLED); + + ui->failsafeBatteryWarningFlightMode->setEnabled(batteryModuleEnabled && index != -1); + ui->failsafeBatteryWarningFlightModeCb->setChecked(index != -1); +} + +void ConfigInputWidget::failsafeBatteryCriticalFlightModeChanged(int index) +{ + HwSettings::DataFields hwSettingsData = hwSettingsObj->getData(); + bool batteryModuleEnabled = (hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_BATTERY] == HwSettings::OPTIONALMODULES_ENABLED); + + ui->failsafeBatteryCriticalFlightMode->setEnabled(batteryModuleEnabled && index != -1); + ui->failsafeBatteryCriticalFlightModeCb->setChecked(index != -1); +} + void ConfigInputWidget::failsafeFlightModeChanged(int index) { ui->failsafeFlightMode->setEnabled(index != -1); @@ -2186,7 +2168,24 @@ void ConfigInputWidget::failsafeFlightModeCbToggled(bool checked) ui->failsafeFlightMode->setCurrentIndex(checked ? 0 : -1); } +void ConfigInputWidget::failsafeBatteryWarningFlightModeCbToggled(bool checked) +{ + ui->failsafeBatteryWarningFlightMode->setCurrentIndex(checked ? 0 : -1); +} + +void ConfigInputWidget::failsafeBatteryCriticalFlightModeCbToggled(bool checked) +{ + ui->failsafeBatteryCriticalFlightMode->setCurrentIndex(checked ? 0 : -1); +} + void ConfigInputWidget::enableControlsChanged(bool enabled) { - ui->failsafeFlightMode->setEnabled(enabled && (ui->failsafeFlightMode->currentIndex() != -1)); + ui->failsafeFlightMode->setEnabled(enabled && ui->failsafeFlightMode->currentIndex() != -1); + + HwSettings::DataFields hwSettingsData = hwSettingsObj->getData(); + bool batteryModuleEnabled = (hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_BATTERY] == HwSettings::OPTIONALMODULES_ENABLED); + ui->failsafeBatteryWarningFlightMode->setEnabled(batteryModuleEnabled && enabled && ui->failsafeBatteryWarningFlightMode->currentIndex() != -1); + ui->failsafeBatteryCriticalFlightMode->setEnabled(batteryModuleEnabled && enabled && ui->failsafeBatteryCriticalFlightMode->currentIndex() != -1); + ui->failsafeBatteryWarningFlightModeCb->setEnabled(enabled && batteryModuleEnabled); + ui->failsafeBatteryCriticalFlightModeCb->setEnabled(enabled && batteryModuleEnabled); } diff --git a/ground/gcs/src/plugins/config/configinputwidget.h b/ground/gcs/src/plugins/config/configinputwidget.h index 458744024..996e83d94 100644 --- a/ground/gcs/src/plugins/config/configinputwidget.h +++ b/ground/gcs/src/plugins/config/configinputwidget.h @@ -41,7 +41,7 @@ #include "flightstatus.h" #include "accessorydesired.h" #include "systemsettings.h" - +#include "hwsettings.h" #include #include #include @@ -145,6 +145,8 @@ private: SystemSettings *systemSettingsObj; SystemSettings::DataFields systemSettingsData; + HwSettings *hwSettingsObj; + typedef struct { ManualControlSettings::DataFields manualSettingsData; ActuatorSettings::DataFields actuatorSettingsData; @@ -232,6 +234,10 @@ private slots: void failsafeFlightModeChanged(int index); void failsafeFlightModeCbToggled(bool checked); + void failsafeBatteryWarningFlightModeChanged(int index); + void failsafeBatteryWarningFlightModeCbToggled(bool checked); + void failsafeBatteryCriticalFlightModeChanged(int index); + void failsafeBatteryCriticalFlightModeCbToggled(bool checked); void enableControlsChanged(bool enabled); protected: diff --git a/ground/gcs/src/plugins/config/input.ui b/ground/gcs/src/plugins/config/input.ui index 99788d227..02ed492fe 100644 --- a/ground/gcs/src/plugins/config/input.ui +++ b/ground/gcs/src/plugins/config/input.ui @@ -2336,6 +2336,106 @@ font:bold; 12 + + + + Battery Failsafe Settings + + + + 9 + + + 9 + + + 9 + + + 9 + + + 6 + + + + + + + On battery warning alarm change flight mode to: + + + + + + + + 200 + 0 + + + + Qt::StrongFocus + + + When triggering battery Warning alarm switch to this flight mode. + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + On battery critical alarm change flight mode to: + + + + + + + + 200 + 0 + + + + Qt::StrongFocus + + + When triggering battery Critical alarm switch to this flight mode. + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + @@ -2468,7 +2568,9 @@ font:bold; Failsafe is a function that is triggered when the connection between the transmitter and receiver is lost. Failsafe gives the user a chance to configure some basic behaviour and specify what input the flight controller should get even if no control signals from the transmitter is present. -The failsafe is triggered differently for different receivers. Failsafe should always be tested before every flight. +The failsafe is triggered differently for different receivers. Failsafe should always be tested before every flight. + +Battery Failsafe triggers the selected flight modes as soon as the related alarm is raised. It needs Battery module enabled and configured. It is not available on CC3D. false diff --git a/shared/uavobjectdefinition/flightbatterysettings.xml b/shared/uavobjectdefinition/flightbatterysettings.xml index b5a81a869..1d60ff6fa 100644 --- a/shared/uavobjectdefinition/flightbatterysettings.xml +++ b/shared/uavobjectdefinition/flightbatterysettings.xml @@ -2,10 +2,10 @@ Flight Battery configuration. - + - - + + diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml index 394d814a6..4ecbfea4a 100644 --- a/shared/uavobjectdefinition/flightmodesettings.xml +++ b/shared/uavobjectdefinition/flightmodesettings.xml @@ -95,6 +95,8 @@ + +