rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

LP-206 Add Always stabilized accessory switch - Fix: Allow Accessory3 for arming

Browse Source
This commit is contained in:
Laurent Lalanne 2016-03-05 18:34:17 +01:00
parent 0f25a510f5
commit 406c381071
8 changed files with 198 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
flight/modules/Actuator/actuator.c
View File

@ -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)) {

14
flight/modules/ManualControl/armhandler.c
View File

@ -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:

53
flight/modules/ManualControl/manualcontrol.c
View File

@ -37,6 +37,7 @@
#include <sanitycheck.h>
#include <manualcontrolsettings.h>
#include <manualcontrolcommand.h>
#include <accessorydesired.h>
#include <vtolselftuningstats.h>
#include <flightmodesettings.h>
#include <flightstatus.h>
@ -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;
}

45
ground/gcs/src/plugins/config/configoutputwidget.cpp
View File

@ -41,6 +41,8 @@
#include "mixersettings.h"
#include "actuatorcommand.h"
#include "actuatorsettings.h"
#include "flightmodesettings.h"
#include "flightstatus.h"
#include "systemsettings.h"
#include <QDebug>
@ -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 <b>ACTIVE</b>. This prevents arming!.");
} else {
m_ui->alwayStabilizedLabel2->setText("(Really be careful!).");
}
}
void ConfigOutputWidget::openHelp()

2
ground/gcs/src/plugins/config/configoutputwidget.h
View File

@ -104,6 +104,8 @@ private:
private slots:
void updateWarnings(UAVObject *);
void updateSpinStabilizeCheckComboBoxes();
void updateAlwaysStabilizeStatus();
void stopTests();
virtual void refreshWidgetsValues(UAVObject *obj = NULL);
void updateObjectsFromWidgets();

73
ground/gcs/src/plugins/config/output.ui
View File

@ -123,7 +123,7 @@
<x>0</x>
<y>0</y>
<width>743</width>
<height>668</height>
<height>662</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_3" stretch="0,0,0">
@ -817,10 +817,79 @@
</size>
</property>
<property name="text">
<string>Motors spin at neutral output when armed and throttle below zero (be careful)</string>
<string>Motors spin at neutral output when armed and throttle below zero (Be careful).</string>
</property>
</widget>
</item>
<item>
<layout class="QHBoxLayout" name="alwaysStabilizeGroup">
<property name="spacing">
<number>6</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<item>
<spacer name="horizontalSpacer_5">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeType">
<enum>QSizePolicy::Fixed</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QLabel" name="alwayStabilizedLabel1">
<property name="text">
<string>Multirotor is Always Stabilized When Armed using:</string>
</property>
</widget>
</item>
<item>
<widget class="QComboBox" name="alwaysStabilizedSwitch">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="minimumSize">
<size>
<width>120</width>
<height>0</height>
</size>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="alwayStabilizedLabel2">
<property name="text">
<string>(Really be careful!).</string>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer_4">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</item>
<item>
<spacer name="verticalSpacer">
<property name="orientation">

11
shared/uavobjectdefinition/flightmodesettings.xml
View File

@ -2,7 +2,7 @@
<object name="FlightModeSettings" singleinstance="true" settings="true" category="Control">
<description>Settings to control arming and flight mode</description>
<field name="Arming" units="" type="enum" elements="1" options="Always Disarmed,Always Armed,Roll Left,Roll Right,Pitch Forward,Pitch Aft,Yaw Left,Yaw Right,Accessory 0,Accessory 1,Accessory 2" defaultvalue="Always Disarmed"/>
<field name="Arming" units="" type="enum" elements="1" options="Always Disarmed,Always Armed,Roll Left,Roll Right,Pitch Forward,Pitch Aft,Yaw Left,Yaw Right,Accessory 0,Accessory 1,Accessory 2,Accessory 3" defaultvalue="Always Disarmed"/>
<!-- Note these options should be identical to those in StabilizationDesired.StabilizationMode -->
<field name="Stabilization1Settings" units="" type="enum"
@ -75,7 +75,14 @@
%NE:POI:AutoCruise; \
%NE:POI:AutoCruise;" />
<field name="AlwaysStabilizeWhenArmed" units="" type="enum" elements="1" options="False,True" defaultvalue="False" description="For Multirotors. Always stabilize no matter the throttle setting when vehicle is armed. Does not work when vehicle is set to Always Armed."/>
<field name="AlwaysStabilizeWhenArmedSwitch"
units=""
type="enum"
elements="1"
options="Disabled,Accessory 0,Accessory 1,Accessory 2,Accessory 3"
defaultvalue="Disabled"
description="For Multirotors. Always stabilize no matter the throttle setting when vehicle is armed. Does not work when vehicle is set to Always Armed."/>
<field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
<field name="ArmingSequenceTime" units="ms" type="uint16" elements="1" defaultvalue="1000"/>
<field name="DisarmingSequenceTime" units="ms" type="uint16" elements="1" defaultvalue="1000"/>

1
shared/uavobjectdefinition/flightstatus.xml
View File

@ -2,6 +2,7 @@
<object name="FlightStatus" singleinstance="true" settings="false" category="Control">
<description>Contains major flight status information for other modules.</description>
<field name="Armed" units="" type="enum" elements="1" options="Disarmed,Arming,Armed" defaultvalue="Disarmed"/>
<field name="AlwaysStabilizeWhenArmed" units="" type="enum" elements="1" options="False,True" defaultvalue="False"/>
<!-- Note these enumerated values should be the same as ManualControlSettings -->
<field name="FlightMode" units="" type="enum" elements="1" options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,PositionHold,CourseLock,VelocityRoam,HomeLeash,AbsolutePosition,ReturnToBase,Land,PathPlanner,POI,AutoCruise,AutoTakeoff"/>