1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-19 09:54:15 +01:00

Add support for more than 3-pos FlightMode switches

It is now possible to have 1 to 6 flight mode switch positions
(usefull for guidance, position hold and similar use).

The input channel range is divided into N (1 to 6) zones and each
zone represents a flight mode. Default is 3 zones (backward compatible),
but more can be chosen.

How to use: configure Tx mixers in a way they provide required number
of different values for the same FlightMode channel. For instance,
using Turnigy 9X radio with ER9X firmware, one can create a mixer like
this:

     -100 MAX ID0   Manual
   R  -50 MAX ID1   Stabilized1 (Rate)
   R    0 MAX ID2   Stabilized2 (Attitude)
   R   50 MAX RUD   PositionHold
   R  100 MAX ELE   ReturnToBase

And set number of flight mode positions to 5. As a result, the 3-pos
switch (ID0, ID1, ID2) will provide first three flight modes, the rudder
D/R switch will override those and enable the 4th flight mode, and
elevator D/R switch will have highest precedence and activate the 5th
flight mode.

This will change the ManualControlSettings objectID.
This commit is contained in:
Oleg Semyonov 2012-06-23 17:14:59 +03:00
parent a242c2f383
commit cd6387d0a6
5 changed files with 350 additions and 125 deletions

View File

@ -58,7 +58,6 @@
#define TASK_PRIORITY (tskIDLE_PRIORITY+4)
#define UPDATE_PERIOD_MS 20
#define THROTTLE_FAILSAFE -0.1f
#define FLIGHT_MODE_LIMIT 1.0f/3.0f
#define ARMED_TIME_MS 1000
#define ARMED_THRESHOLD 0.50f
//safe band to allow a bit of calibration error or trim offset (in microseconds)
@ -847,30 +846,38 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
}
/**
* @brief Determine which of three positions the flight mode switch is in and set flight mode accordingly
* @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)
static void processFlightMode(ManualControlSettingsData *settings, float flightMode)
{
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
uint8_t newMode;
// Note here the code is ass
if (flightMode < -FLIGHT_MODE_LIMIT)
newMode = settings->FlightModePosition[0];
else if (flightMode > FLIGHT_MODE_LIMIT)
newMode = settings->FlightModePosition[2];
else
newMode = settings->FlightModePosition[1];
// Check the FlightModeNumber: it shouldn't be set higher than number of FlightModePosition elements
if ((settings->FlightModeNumber < 1) || (settings->FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM)) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
return;
}
if(flightStatus.FlightMode != newMode) {
// Scale flightMode from [-1..+1] to [0..1] range and calculate a delta
float scaledFlightMode = (flightMode + 1.0) / 2.0;
float delta = 1.0 / (float)(settings->FlightModeNumber);
float bound = delta;
uint8_t i;
for (i = 1; i < settings->FlightModeNumber; i++, bound += delta) {
if (scaledFlightMode < bound)
break;
}
uint8_t newMode = settings->FlightModePosition[i - 1];
if (flightStatus.FlightMode != newMode) {
flightStatus.FlightMode = newMode;
FlightStatusSet(&flightStatus);
}
}
/**

View File

@ -85,6 +85,10 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent)
addUAVObjectToWidgetRelation("ManualControlSettings","FlightModePosition",m_config->fmsModePos1,0);
addUAVObjectToWidgetRelation("ManualControlSettings","FlightModePosition",m_config->fmsModePos2,1);
addUAVObjectToWidgetRelation("ManualControlSettings","FlightModePosition",m_config->fmsModePos3,2);
addUAVObjectToWidgetRelation("ManualControlSettings","FlightModePosition",m_config->fmsModePos4,3);
addUAVObjectToWidgetRelation("ManualControlSettings","FlightModePosition",m_config->fmsModePos5,4);
addUAVObjectToWidgetRelation("ManualControlSettings","FlightModePosition",m_config->fmsModePos6,5);
addUAVObjectToWidgetRelation("ManualControlSettings","FlightModeNumber",m_config->fmsPosNum);
addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization1Settings",m_config->fmsSsPos1Roll,"Roll");
addUAVObjectToWidgetRelation("ManualControlSettings","Stabilization2Settings",m_config->fmsSsPos2Roll,"Roll");
@ -99,6 +103,7 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : ConfigTaskWidget(parent)
addUAVObjectToWidgetRelation("ManualControlSettings","Arming",m_config->armControl);
addUAVObjectToWidgetRelation("ManualControlSettings","ArmedTimeout",m_config->armTimeout,0,1000);
connect( ManualControlCommand::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(moveFMSlider()));
connect( ManualControlSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(updatePositionSlider()));
enableControls(false);
populateWidgets();
@ -1113,6 +1118,7 @@ void ConfigInputWidget::invertControls()
}
manualSettingsObj->setData(manualSettingsData);
}
void ConfigInputWidget::moveFMSlider()
{
ManualControlSettings::DataFields manualSettingsDataPriv = manualSettingsObj->getData();
@ -1139,12 +1145,78 @@ void ConfigInputWidget::moveFMSlider()
valueScaled = 0;
}
if(valueScaled < -(1.0 / 3.0))
m_config->fmsSlider->setValue(-100);
else if (valueScaled > (1.0/3.0))
m_config->fmsSlider->setValue(100);
else
m_config->fmsSlider->setValue(0);
// Bound and scale FlightMode from [-1..+1] to [0..1] range
if (valueScaled < -1.0)
valueScaled = -1.0;
if (valueScaled > 1.0)
valueScaled = 1.0;
float scaledFlightMode = (valueScaled + 1.0) / 2.0;
// Display current channel value for tuning (in percents)
m_config->fmsValue->setText(QString::number(scaledFlightMode * 100.0, 'f', 0));
// Find and display the current FlightMode using the same logic as in a flight code
float delta = 1.0 / (float)(manualSettingsDataPriv.FlightModeNumber);
float bound = delta;
int i;
for (i = 1; i < manualSettingsDataPriv.FlightModeNumber; i++, bound += delta) {
if (scaledFlightMode < bound)
break;
}
m_config->fmsSlider->setValue(i - 1);
}
void ConfigInputWidget::updatePositionSlider()
{
ManualControlSettings::DataFields manualSettingsDataPriv = manualSettingsObj->getData();
switch(manualSettingsDataPriv.FlightModeNumber) {
default:
case 6:
m_config->fmsModePos6->setEnabled(true);
// pass through
case 5:
m_config->fmsModePos5->setEnabled(true);
// pass through
case 4:
m_config->fmsModePos4->setEnabled(true);
// pass through
case 3:
m_config->fmsModePos3->setEnabled(true);
// pass through
case 2:
m_config->fmsModePos2->setEnabled(true);
// pass through
case 1:
m_config->fmsModePos1->setEnabled(true);
// pass through
case 0:
break;
}
switch(manualSettingsDataPriv.FlightModeNumber) {
case 0:
m_config->fmsModePos1->setEnabled(false);
// pass through
case 1:
m_config->fmsModePos2->setEnabled(false);
// pass through
case 2:
m_config->fmsModePos3->setEnabled(false);
// pass through
case 3:
m_config->fmsModePos4->setEnabled(false);
// pass through
case 4:
m_config->fmsModePos5->setEnabled(false);
// pass through
case 5:
m_config->fmsModePos6->setEnabled(false);
// pass through
case 6:
default:
break;
}
}
void ConfigInputWidget::updateCalibration()

View File

@ -146,6 +146,7 @@ private slots:
void moveSticks();
void dimOtherControls(bool value);
void moveFMSlider();
void updatePositionSlider();
void invertControls();
void simpleCalibration(bool state);
void updateCalibration();

View File

@ -17,7 +17,7 @@
<item>
<widget class="QTabWidget" name="tabWidget">
<property name="currentIndex">
<number>0</number>
<number>1</number>
</property>
<widget class="QWidget" name="RCInput">
<attribute name="title">
@ -204,31 +204,12 @@
<attribute name="title">
<string>Flight Mode Switch Settings</string>
</attribute>
<widget class="QLabel" name="label_16">
<property name="geometry">
<rect>
<x>310</x>
<y>30</y>
<width>201</width>
<height>17</height>
</rect>
</property>
<property name="font">
<font>
<weight>75</weight>
<bold>true</bold>
</font>
</property>
<property name="text">
<string>for multirotors!</string>
</property>
</widget>
<widget class="QGroupBox" name="groupBox">
<property name="geometry">
<rect>
<x>30</x>
<y>160</y>
<width>451</width>
<y>260</y>
<width>541</width>
<height>161</height>
</rect>
</property>
@ -384,19 +365,6 @@ margin:1px;</string>
</property>
</widget>
</item>
<item row="0" column="1">
<spacer name="verticalSpacer_3">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item row="1" column="2">
<widget class="QLabel" name="label_9">
<property name="styleSheet">
@ -414,25 +382,167 @@ margin:1px;</string>
</property>
</widget>
</item>
<item row="0" column="1">
<spacer name="verticalSpacer_3">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
<widget class="QGroupBox" name="groupBox_2">
<property name="geometry">
<rect>
<x>30</x>
<y>30</y>
<width>451</width>
<height>121</height>
<y>20</y>
<width>541</width>
<height>211</height>
</rect>
</property>
<property name="title">
<string>FlightMode Switch Positions</string>
</property>
<widget class="QComboBox" name="fmsModePos5">
<property name="geometry">
<rect>
<x>100</x>
<y>140</y>
<width>151</width>
<height>26</height>
</rect>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
<widget class="QComboBox" name="fmsModePos4">
<property name="geometry">
<rect>
<x>100</x>
<y>110</y>
<width>151</width>
<height>26</height>
</rect>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
<property name="toolTip">
<string>Select the stabilization mode on this position (manual/stabilized/auto)</string>
</property>
</widget>
<widget class="QLabel" name="label_pos4">
<property name="geometry">
<rect>
<x>10</x>
<y>115</y>
<width>62</width>
<height>17</height>
</rect>
</property>
<property name="text">
<string>Pos. 4</string>
</property>
</widget>
<widget class="QComboBox" name="fmsModePos6">
<property name="geometry">
<rect>
<x>100</x>
<y>170</y>
<width>151</width>
<height>26</height>
</rect>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
<widget class="QLabel" name="label_pos5">
<property name="geometry">
<rect>
<x>10</x>
<y>145</y>
<width>62</width>
<height>17</height>
</rect>
</property>
<property name="text">
<string>Pos. 5</string>
</property>
</widget>
<widget class="QLabel" name="label_pos6">
<property name="geometry">
<rect>
<x>10</x>
<y>175</y>
<width>62</width>
<height>17</height>
</rect>
</property>
<property name="text">
<string>Pos. 6</string>
</property>
</widget>
<widget class="QSlider" name="fmsSlider">
<property name="enabled">
<bool>false</bool>
</property>
<property name="geometry">
<rect>
<x>70</x>
<y>28</y>
<width>20</width>
<height>160</height>
</rect>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
<property name="toolTip">
<string>This slider moves when you move the flight mode switch
on your remote. Setup the flightmode channel on the RC Input tab
if you have not done so already.</string>
</property>
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>5</number>
</property>
<property name="pageStep">
<number>10</number>
</property>
<property name="value">
<number>0</number>
</property>
<property name="sliderPosition">
<number>0</number>
</property>
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="invertedAppearance">
<bool>true</bool>
</property>
<property name="tickPosition">
<enum>QSlider::TicksBelow</enum>
</property>
<property name="tickInterval">
<number>1</number>
</property>
</widget>
<widget class="QComboBox" name="fmsModePos2">
<property name="geometry">
<rect>
<x>100</x>
<y>55</y>
<y>50</y>
<width>151</width>
<height>26</height>
</rect>
@ -445,7 +555,20 @@ margin:1px;</string>
<property name="geometry">
<rect>
<x>100</x>
<y>25</y>
<y>80</y>
<width>151</width>
<height>26</height>
</rect>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
<widget class="QComboBox" name="fmsModePos1">
<property name="geometry">
<rect>
<x>100</x>
<y>20</y>
<width>151</width>
<height>26</height>
</rect>
@ -457,37 +580,11 @@ margin:1px;</string>
<string>Select the stabilization mode on this position (manual/stabilized/auto)</string>
</property>
</widget>
<widget class="QLabel" name="label_11">
<widget class="QLabel" name="label_pos2">
<property name="geometry">
<rect>
<x>10</x>
<y>30</y>
<width>62</width>
<height>17</height>
</rect>
</property>
<property name="text">
<string>Pos. 3</string>
</property>
</widget>
<widget class="QComboBox" name="fmsModePos1">
<property name="geometry">
<rect>
<x>100</x>
<y>85</y>
<width>151</width>
<height>26</height>
</rect>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
</property>
</widget>
<widget class="QLabel" name="label_12">
<property name="geometry">
<rect>
<x>10</x>
<y>60</y>
<x>11</x>
<y>55</y>
<width>62</width>
<height>17</height>
</rect>
@ -496,11 +593,11 @@ margin:1px;</string>
<string>Pos. 2</string>
</property>
</widget>
<widget class="QLabel" name="label_13">
<widget class="QLabel" name="label_pos1">
<property name="geometry">
<rect>
<x>10</x>
<y>90</y>
<x>11</x>
<y>25</y>
<width>62</width>
<height>17</height>
</rect>
@ -509,55 +606,81 @@ margin:1px;</string>
<string>Pos. 1</string>
</property>
</widget>
<widget class="QSlider" name="fmsSlider">
<property name="enabled">
<bool>false</bool>
</property>
<widget class="QLabel" name="label_pos3">
<property name="geometry">
<rect>
<x>70</x>
<y>30</y>
<width>20</width>
<height>81</height>
<x>11</x>
<y>85</y>
<width>62</width>
<height>17</height>
</rect>
</property>
<property name="focusPolicy">
<enum>Qt::StrongFocus</enum>
<property name="text">
<string>Pos. 3</string>
</property>
</widget>
<widget class="QSpinBox" name="fmsPosNum">
<property name="geometry">
<rect>
<x>410</x>
<y>20</y>
<width>61</width>
<height>20</height>
</rect>
</property>
<property name="toolTip">
<string>This slider moves when you move the flight mode switch
on your remote. Setup the flightmode channel on the RC Input tab
if you have not done so already.</string>
<string>Number of positions your FlightMode switch has.
Default is 3.
It will be 2 or 3 for most of setups, but it also can be up to 6.
In that case you have to configure your radio mixers so the whole range
from min to max is split into N equal intervals, and you may set arbitrary
channel value for each flight mode.</string>
</property>
<property name="minimum">
<number>-100</number>
<number>1</number>
</property>
<property name="maximum">
<number>100</number>
<number>6</number>
</property>
<property name="pageStep">
<number>10</number>
<property name="value">
<number>3</number>
</property>
<property name="sliderPosition">
<number>-100</number>
</widget>
<widget class="QLabel" name="label">
<property name="geometry">
<rect>
<x>270</x>
<y>20</y>
<width>141</width>
<height>16</height>
</rect>
</property>
<property name="orientation">
<enum>Qt::Vertical</enum>
<property name="text">
<string>Number of switch positions:</string>
</property>
<property name="tickPosition">
<enum>QSlider::TicksBelow</enum>
</widget>
<widget class="QLabel" name="label_2">
<property name="geometry">
<rect>
<x>270</x>
<y>50</y>
<width>141</width>
<height>16</height>
</rect>
</property>
<property name="tickInterval">
<number>100</number>
<property name="text">
<string>Channel value (0..100):</string>
</property>
</widget>
<widget class="QLabel" name="label_15">
<property name="geometry">
<rect>
<x>270</x>
<y>30</y>
<width>141</width>
<height>31</height>
<x>300</x>
<y>130</y>
<width>191</width>
<height>30</height>
</rect>
</property>
<property name="font">
@ -567,13 +690,37 @@ if you have not done so already.</string>
</font>
</property>
<property name="text">
<string>Warning: avoid &quot;Manual&quot;</string>
<string>Avoid &quot;Manual&quot; for multirotors!</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
<widget class="QLabel" name="fmsValue">
<property name="geometry">
<rect>
<x>410</x>
<y>51</y>
<width>46</width>
<height>13</height>
</rect>
</property>
<property name="toolTip">
<string>FlightMode channel value is shown here for reference.
Whole range [0..100] in spit into number of intervals - one per
FlightMode (switch position).
Hint: make sure that you have the FlightMode channel neutral
value defined.</string>
</property>
<property name="text">
<string>0</string>
</property>
</widget>
</widget>
<zorder>groupBox_2</zorder>
<zorder>groupBox</zorder>
</widget>
<widget class="QWidget" name="tab_4">
<attribute name="title">
@ -763,9 +910,6 @@ Applies and Saves all settings to SD</string>
</widget>
<tabstops>
<tabstop>fmsSlider</tabstop>
<tabstop>fmsModePos3</tabstop>
<tabstop>fmsModePos2</tabstop>
<tabstop>fmsModePos1</tabstop>
<tabstop>fmsSsPos1Roll</tabstop>
<tabstop>fmsSsPos1Pitch</tabstop>
<tabstop>fmsSsPos1Yaw</tabstop>

View File

@ -23,7 +23,8 @@
<field name="Stabilization3Settings" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="None,Rate,Attitude,AxisLock,WeakLeveling,VirtualBar" defaultvalue="Attitude,Attitude,Rate"/>
<!-- Note these options values should be identical to those defined in FlightMode -->
<field name="FlightModePosition" units="" type="enum" elements="3" options="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,VelocityControl,PositionHold" defaultvalue="Manual,Stabilized1,Stabilized2"/>
<field name="FlightModeNumber" units="" type="uint8" elements="1" defaultvalue="3"/>
<field name="FlightModePosition" units="" type="enum" elements="6" options="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,VelocityControl,PositionHold" defaultvalue="Manual,Stabilized1,Stabilized2,Stabilized3,AltitudeHold,PositionHold"/>
<field name="ArmedTimeout" units="ms" type="uint16" elements="1" defaultvalue="30000"/>
<access gcs="readwrite" flight="readwrite"/>