Merge pull request #3360 from mavlink/px4_guidedfixes

PX4 Guided Mode

README.md

@@ -59,7 +59,7 @@ You need to install Qt like this instead of using packages from say a Linux dist
 
 #### Vagrant
 
-A Vagrantfile is provided to build QGroundControl using the [Vagrant](https://www.vagrantup.com/) system.  This will produce a native Linux build which can be run in the Vagrant Virtual Machine or on the host machine if it is compatible.
+A Vagrantfile is provided to build QGroundControl using the [Vagrant](https://www.vagrantup.com/) system. This will produce a native Linux build which can be run in the Vagrant Virtual Machine or on the host machine if it is compatible.
 
 * [Download](https://www.vagrantup.com/downloads.html) Vagrant
 * [Install](https://www.vagrantup.com/docs/getting-started/) Vagrant

src/AutoPilotPlugins/PX4/PX4AutoPilotPlugin.cc

@@ -35,39 +35,6 @@
 ///     @brief This is the AutoPilotPlugin implementatin for the MAV_AUTOPILOT_PX4 type.
 ///     @author Don Gagne <don@thegagnes.com>
 
-enum PX4_CUSTOM_MAIN_MODE {
-    PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
-    PX4_CUSTOM_MAIN_MODE_ALTCTL,
-    PX4_CUSTOM_MAIN_MODE_POSCTL,
-    PX4_CUSTOM_MAIN_MODE_AUTO,
-    PX4_CUSTOM_MAIN_MODE_ACRO,
-    PX4_CUSTOM_MAIN_MODE_OFFBOARD,
-    PX4_CUSTOM_MAIN_MODE_STABILIZED,
-    PX4_CUSTOM_MAIN_MODE_RATTITUDE
-
-};
-
-enum PX4_CUSTOM_SUB_MODE_AUTO {
-    PX4_CUSTOM_SUB_MODE_AUTO_READY = 1,
-    PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF,
-    PX4_CUSTOM_SUB_MODE_AUTO_LOITER,
-    PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
-    PX4_CUSTOM_SUB_MODE_AUTO_RTL,
-    PX4_CUSTOM_SUB_MODE_AUTO_LAND,
-    PX4_CUSTOM_SUB_MODE_AUTO_RTGS,
-    PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_ME
-};
-
-union px4_custom_mode {
-    struct {
-        uint16_t reserved;
-        uint8_t main_mode;
-        uint8_t sub_mode;
-    };
-    uint32_t data;
-    float data_float;
-};
-
 PX4AutoPilotPlugin::PX4AutoPilotPlugin(Vehicle* vehicle, QObject* parent) :
     AutoPilotPlugin(vehicle, parent),
     _airframeComponent(NULL),
@@ -153,7 +120,8 @@ void PX4AutoPilotPlugin::_parametersReadyPreChecks(bool missingParameters)
     // Check for older parameter version set
     // FIXME: Firmware is moving to version stamp parameter set. Once that is complete the version stamp
     // should be used instead.
-    if (parameterExists(FactSystem::defaultComponentId, "SENS_GYRO_XOFF")) {
+    if (parameterExists(FactSystem::defaultComponentId, "SENS_GYRO_XOFF") ||
+            parameterExists(FactSystem::defaultComponentId, "COM_DL_LOSS_EN")) {
         _incorrectParameterVersion = true;
         qgcApp()->showMessage("This version of GroundControl can only perform vehicle setup on a newer version of firmware. "
                               "Please perform a Firmware Upgrade if you wish to use Vehicle Setup.");

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

@@ -117,8 +117,6 @@ QStringList PX4FirmwarePlugin::flightModes(void)
 {
     QStringList flightModes;
 
-    // FIXME: fixed-wing/multi-rotor differences?
-
     for (size_t i=0; i<sizeof(rgModes2Name)/sizeof(rgModes2Name[0]); i++) {
         const struct Modes2Name* pModes2Name = &rgModes2Name[i];
 
@@ -138,8 +136,6 @@ QString PX4FirmwarePlugin::flightMode(uint8_t base_mode, uint32_t custom_mode) c
         union px4_custom_mode px4_mode;
         px4_mode.data = custom_mode;
 
-        // FIXME: fixed-wing/multi-rotor differences?
-
         bool found = false;
         for (size_t i=0; i<sizeof(rgModes2Name)/sizeof(rgModes2Name[0]); i++) {
             const struct Modes2Name* pModes2Name = &rgModes2Name[i];
@@ -265,8 +261,9 @@ void PX4FirmwarePlugin::pauseVehicle(Vehicle* vehicle)
     // kick it into hold mode
     vehicle->setFlightMode(pauseFlightMode);
 
+    QGC::SLEEP::msleep(200);
+
     // then tell it to loiter at the current position
-    // above the takeoff (home) position
     mavlink_message_t msg;
     mavlink_command_long_t cmd;
 
@@ -306,19 +303,16 @@ void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
         return;
     }
 
-    // tell the system first to take off in its internal,
-    // airframe specific takeoff action
-    vehicle->setFlightMode(takeoffFlightMode);
+    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
 
-    // then tell it to loiter at the user-selected location
-    // above the takeoff (home) position
+    // Set destination altitude
     mavlink_message_t msg;
     mavlink_command_long_t cmd;
 
     cmd.command = (uint16_t)MAV_CMD_DO_REPOSITION;
     cmd.confirmation = 0;
     cmd.param1 = -1.0f;
-    cmd.param2 = 0.0;
+    cmd.param2 = 0.0f;
     cmd.param3 = 0.0f;
     cmd.param4 = NAN;
     cmd.param5 = NAN;
@@ -327,10 +321,13 @@ void PX4FirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
     cmd.target_system = vehicle->id();
     cmd.target_component = 0;
 
-    MAVLinkProtocol* mavlink = qgcApp()->toolbox()->mavlinkProtocol();
     mavlink_msg_command_long_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &cmd);
-
     vehicle->sendMessage(msg);
+
+    QGC::SLEEP::msleep(200);
+
+    // trigger take off
+    vehicle->setFlightMode(takeoffFlightMode);
 }
 
 void PX4FirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
@@ -401,5 +398,6 @@ void PX4FirmwarePlugin::setGuidedMode(Vehicle* vehicle, bool guidedMode)
 bool PX4FirmwarePlugin::isGuidedMode(const Vehicle* vehicle) const
 {
     // Not supported by generic vehicle
-    return (vehicle->flightMode() == pauseFlightMode);
+    return (vehicle->flightMode() == pauseFlightMode || vehicle->flightMode() == takeoffFlightMode
+            || vehicle->flightMode() == landingFlightMode);
 }

src/QmlControls/SliderSwitch.qml

@@ -55,7 +55,6 @@ Rectangle {
         MouseArea {
             id:             sliderDragArea
             anchors.fill:   parent
-            onClicked:      _root.accept()
             drag.target:    slider
             drag.axis:      Drag.XAxis
             drag.minimumX:  _border

src/comm/PX4MockLink.params

@@ -98,7 +98,6 @@
 1	50	CBRK_RATE_CTRL	0	6
 1	50	CBRK_SUPPLY_CHK	0	6
 1	50	COM_DISARM_LAND	0	3
-1	50	COM_DL_LOSS_EN	0	6
 1	50	COM_DL_LOSS_T	10	6
 1	50	COM_DL_REG_T	0	6
 1	50	COM_EF_C2T	5	9
@@ -554,4 +553,4 @@
 1	50	VT_MOT_COUNT	0	6
 1	50	VT_POWER_MAX	120	9
 1	50	VT_PROP_EFF	0	9
-1	51	COMPONENT_51	51	6
+1	51	COMPONENT_51	51	6