diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c
index cc188e602..cad5d065d 100644
--- a/flight/Modules/ManualControl/manualcontrol.c
+++ b/flight/Modules/ManualControl/manualcontrol.c
@@ -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);
}
-
}
/**
diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp
index ba1dbb74e..7952b3d70 100644
--- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp
+++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp
@@ -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()
diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.h b/ground/openpilotgcs/src/plugins/config/configinputwidget.h
index eeb6178fc..418fbdfee 100644
--- a/ground/openpilotgcs/src/plugins/config/configinputwidget.h
+++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.h
@@ -146,6 +146,7 @@ private slots:
void moveSticks();
void dimOtherControls(bool value);
void moveFMSlider();
+ void updatePositionSlider();
void invertControls();
void simpleCalibration(bool state);
void updateCalibration();
diff --git a/ground/openpilotgcs/src/plugins/config/input.ui b/ground/openpilotgcs/src/plugins/config/input.ui
index 422904bb5..c116a812f 100644
--- a/ground/openpilotgcs/src/plugins/config/input.ui
+++ b/ground/openpilotgcs/src/plugins/config/input.ui
@@ -17,7 +17,7 @@
-
- 0
+ 1
@@ -204,31 +204,12 @@
Flight Mode Switch Settings
-
-
-
- 310
- 30
- 201
- 17
-
-
-
-
- 75
- true
-
-
-
- for multirotors!
-
-
30
- 160
- 451
+ 260
+ 541
161
@@ -384,19 +365,6 @@ margin:1px;
- -
-
-
- Qt::Vertical
-
-
-
- 20
- 20
-
-
-
-
-
@@ -414,25 +382,177 @@ margin:1px;
+ -
+
+
+ Qt::Vertical
+
+
+
+ 20
+ 20
+
+
+
+
30
- 30
- 451
- 121
+ 20
+ 541
+ 211
FlightMode Switch Positions
+
+
+ false
+
+
+
+ 100
+ 140
+ 151
+ 26
+
+
+
+ Qt::StrongFocus
+
+
+
+
+ false
+
+
+
+ 100
+ 110
+ 151
+ 26
+
+
+
+ Qt::StrongFocus
+
+
+ Select the stabilization mode on this position (manual/stabilized/auto)
+
+
+
+
+
+ 10
+ 115
+ 62
+ 17
+
+
+
+ Pos. 4
+
+
+
+
+ false
+
+
+
+ 100
+ 170
+ 151
+ 26
+
+
+
+ Qt::StrongFocus
+
+
+
+
+
+ 10
+ 145
+ 62
+ 17
+
+
+
+ Pos. 5
+
+
+
+
+
+ 10
+ 175
+ 62
+ 17
+
+
+
+ Pos. 6
+
+
+
+
+ false
+
+
+
+ 70
+ 28
+ 20
+ 160
+
+
+
+ Qt::StrongFocus
+
+
+ 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.
+
+
+ 0
+
+
+ 5
+
+
+ 10
+
+
+ 0
+
+
+ 0
+
+
+ Qt::Vertical
+
+
+ true
+
+
+ QSlider::TicksBelow
+
+
+ 1
+
+
100
- 55
+ 50
151
26
@@ -445,7 +565,20 @@ margin:1px;
100
- 25
+ 80
+ 151
+ 26
+
+
+
+ Qt::StrongFocus
+
+
+
+
+
+ 100
+ 20
151
26
@@ -457,37 +590,11 @@ margin:1px;
Select the stabilization mode on this position (manual/stabilized/auto)
-
+
- 10
- 30
- 62
- 17
-
-
-
- Pos. 3
-
-
-
-
-
- 100
- 85
- 151
- 26
-
-
-
- Qt::StrongFocus
-
-
-
-
-
- 10
- 60
+ 11
+ 55
62
17
@@ -496,11 +603,11 @@ margin:1px;
Pos. 2
-
+
- 10
- 90
+ 11
+ 25
62
17
@@ -509,55 +616,68 @@ margin:1px;
Pos. 1
-
-
- false
-
+
- 70
- 30
- 20
- 81
+ 11
+ 85
+ 62
+ 17
-
- Qt::StrongFocus
+
+ Pos. 3
+
+
+
+
+
+ 458
+ 20
+ 61
+ 20
+
- 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.
+ 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.
- -100
+ 1
- 100
+ 6
-
- 10
+
+ 3
-
- -100
+
+
+
+
+ 277
+ 22
+ 141
+ 16
+
-
- Qt::Vertical
-
-
- QSlider::TicksBelow
-
-
- 100
+
+ Number of flight modes:
- 270
- 30
- 141
- 31
+ 310
+ 120
+ 191
+ 30
@@ -567,13 +687,15 @@ if you have not done so already.
- Warning: avoid "Manual"
+ Avoid "Manual" for multirotors!
true
+ groupBox_2
+ groupBox
@@ -763,9 +885,6 @@ Applies and Saves all settings to SD
fmsSlider
- fmsModePos3
- fmsModePos2
- fmsModePos1
fmsSsPos1Roll
fmsSsPos1Pitch
fmsSsPos1Yaw
diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml
index c2c8ec715..09d4ca618 100644
--- a/shared/uavobjectdefinition/manualcontrolsettings.xml
+++ b/shared/uavobjectdefinition/manualcontrolsettings.xml
@@ -23,7 +23,8 @@
-
+
+