rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

Merge remote-tracking branch 'origin/os/extended-flightmode-switch' into next

Browse Source
This commit is contained in:
James Cotton 2012-07-04 11:52:38 +02:00
commit 58abe910d0
5 changed files with 320 additions and 133 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
flight/Modules/ManualControl/manualcontrol.c
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)
@ -236,19 +235,23 @@ static void manualControlTask(void *parameters)
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= 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 ||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_INVALID ||
// Check the driver is exists
// 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 ||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (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[MANUALCONTROLSETTINGS_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;
@ -357,7 +360,6 @@ static void manualControlTask(void *parameters)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
processFlightMode(&settings, flightMode);
}
@ -847,30 +849,27 @@ 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];
// 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;
if(flightStatus.FlightMode != newMode) {
uint8_t newMode = settings->FlightModePosition[pos];
if (flightStatus.FlightMode != newMode) {
flightStatus.FlightMode = newMode;
FlightStatusSet(&flightStatus);
}
}
/**

81
ground/openpilotgcs/src/plugins/config/configinputwidget.cpp
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();
@ -632,7 +637,8 @@ void ConfigInputWidget::setChannel(int newChan)
m_config->wzText->setText(QString(tr("Please move each control once at a time according to the instructions and picture below.\n\n"
"Move the %1 stick")).arg(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan)));
if(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan).contains("Accessory")) {
if(manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan).contains("Accessory") ||
manualSettingsObj->getField("ChannelGroups")->getElementNames().at(newChan).contains("FlightMode")) {
m_config->wzNext->setEnabled(true);
m_config->wzText->setText(m_config->wzText->text() + tr(" or click next to skip this channel."));
} else
@ -1113,10 +1119,11 @@ void ConfigInputWidget::invertControls()
}
manualSettingsObj->setData(manualSettingsData);
}
void ConfigInputWidget::moveFMSlider()
{
ManualControlSettings::DataFields manualSettingsDataPriv = manualSettingsObj->getData();
ManualControlCommand::DataFields manualCommandDataPriv=manualCommandObj->getData();
ManualControlCommand::DataFields manualCommandDataPriv = manualCommandObj->getData();
float valueScaled;
int chMin = manualSettingsDataPriv.ChannelMin[ManualControlSettings::CHANNELMIN_FLIGHTMODE];
@ -1139,12 +1146,72 @@ 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);
// Bound and scale FlightMode from [-1..+1] to [0..1] range
if (valueScaled < -1.0)
valueScaled = -1.0;
else
m_config->fmsSlider->setValue(0);
if (valueScaled > 1.0)
valueScaled = 1.0;
// Convert flightMode value into the switch position in the range [0..N-1]
// This uses the same optimized computation as flight code to be consistent
uint8_t pos = ((int16_t)(valueScaled * 256) + 256) * manualSettingsDataPriv.FlightModeNumber >> 9;
if (pos >= manualSettingsDataPriv.FlightModeNumber)
pos = manualSettingsDataPriv.FlightModeNumber - 1;
m_config->fmsSlider->setValue(pos);
}
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()

1
ground/openpilotgcs/src/plugins/config/configinputwidget.h
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();

333
ground/openpilotgcs/src/plugins/config/input.ui
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,177 @@ 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="enabled">
<bool>false</bool>
</property>
<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="enabled">
<bool>false</bool>
</property>
<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="enabled">
<bool>false</bool>
</property>
<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. It shows currently active flight mode.
Setup the flight mode 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 +565,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 +590,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 +603,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 +616,68 @@ 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>458</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>277</x>
<y>22</y>
<width>141</width>
<height>16</height>
</rect>
</property>
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="tickPosition">
<enum>QSlider::TicksBelow</enum>
</property>
<property name="tickInterval">
<number>100</number>
<property name="text">
<string>Number of flight modes:</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>310</x>
<y>120</y>
<width>191</width>
<height>30</height>
</rect>
</property>
<property name="font">
@ -567,13 +687,15 @@ 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>
<zorder>groupBox_2</zorder>
<zorder>groupBox</zorder>
</widget>
<widget class="QWidget" name="tab_4">
<attribute name="title">
@ -763,9 +885,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>

3
shared/uavobjectdefinition/manualcontrolsettings.xml
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"/>