Merge pull request #6130 from DonLakeFlyer/HighLatency

Support remainder of HIGH_LATENCY2 fields

v2.0 @ f36b9c4c

-Subproject commit 5db5e944048cd6126a3e58c67c9c4dcdb4d4e0ff
+Subproject commit f36b9c4c5c0c9b6d33621779469de0c1e7eea457

src/FirmwarePlugin/FirmwarePlugin.cc

@@ -620,4 +620,9 @@ QGCCameraControl* FirmwarePlugin::createCameraControl(const mavlink_camera_infor
     return NULL;
 }
 
+uint32_t FirmwarePlugin::highLatencyCustomModeTo32Bits(uint16_t hlCustomMode)
+{
+    // Standard implementation assumes no special handling. Upper part of 32 bit value is not used.
+    return hlCustomMode;
+}
 

src/FirmwarePlugin/FirmwarePlugin.h

@@ -304,6 +304,9 @@ public:
     /// Returns true if the vehicle is a VTOL
     virtual bool isVtol(const Vehicle* vehicle) const;
 
+    /// Convert from HIGH_LATENCY2.custom_mode value to correct 32 bit value.
+    virtual uint32_t highLatencyCustomModeTo32Bits(uint16_t hlCustomMode);
+
     // FIXME: Hack workaround for non pluginize FollowMe support
     static const QString px4FollowMeFlightMode;
 

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc

@@ -589,3 +589,12 @@ QGCCameraControl* PX4FirmwarePlugin::createCameraControl(const mavlink_camera_in
 {
     return new QGCCameraControl(info, vehicle, compID, parent);
 }
+
+uint32_t PX4FirmwarePlugin::highLatencyCustomModeTo32Bits(uint16_t hlCustomMode)
+{
+    union px4_custom_mode px4_cm;
+    px4_cm.data = 0;
+    px4_cm.custom_mode_hl = hlCustomMode;
+
+    return px4_cm.data;
+}

src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h

@@ -70,6 +70,7 @@ public:
     QString             autoDisarmParameter             (Vehicle* vehicle) override { Q_UNUSED(vehicle); return QStringLiteral("COM_DISARM_LAND"); }
     QGCCameraManager*   createCameraManager             (Vehicle* vehicle) override;
     QGCCameraControl*   createCameraControl             (const mavlink_camera_information_t* info, Vehicle* vehicle, int compID, QObject* parent = NULL) override;
+    uint32_t            highLatencyCustomModeTo32Bits   (uint16_t hlCustomMode) override;
 
 protected:
     typedef struct {

src/FirmwarePlugin/PX4/px4_custom_mode.h

@@ -71,7 +71,11 @@ union px4_custom_mode {
 		uint8_t main_mode;
 		uint8_t sub_mode;
 	};
-	uint32_t data;
+    struct {
+        uint16_t reserved_hl;
+        uint16_t custom_mode_hl;
+    };
+    uint32_t data;
 	float data_float;
 };
 

src/Vehicle/Vehicle.cc

@@ -814,6 +814,24 @@ void Vehicle::_handleHighLatency2(mavlink_message_t& message)
     mavlink_high_latency2_t highLatency2;
     mavlink_msg_high_latency2_decode(&message, &highLatency2);
 
+    QString previousFlightMode;
+    if (_base_mode != 0 || _custom_mode != 0){
+        // Vehicle is initialized with _base_mode=0 and _custom_mode=0. Don't pass this to flightMode() since it will complain about
+        // bad modes while unit testing.
+        previousFlightMode = flightMode();
+    }
+    _base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+    _custom_mode = _firmwarePlugin->highLatencyCustomModeTo32Bits(highLatency2.custom_mode);
+    if (previousFlightMode != flightMode()) {
+        emit flightModeChanged(flightMode());
+    }
+
+    // Assume armed since we don't know
+    if (_armed != true) {
+        _armed = true;
+        emit armedChanged(_armed);
+    }
+
     _coordinate.setLatitude(highLatency2.latitude  / (double)1E7);
     _coordinate.setLongitude(highLatency2.longitude / (double)1E7);
     _coordinate.setAltitude(highLatency2.altitude);
@@ -840,21 +858,29 @@ void Vehicle::_handleHighLatency2(mavlink_message_t& message)
     _gpsFactGroup.vdop()->setRawValue(highLatency2.epv == UINT8_MAX ? std::numeric_limits<double>::quiet_NaN() : highLatency2.epv / 10.0);
 
     struct failure2Sensor_s {
-        MAV_FAILURE_FLAG        failureBit;
+        HL_FAILURE_FLAG         failureBit;
         MAV_SYS_STATUS_SENSOR   sensorBit;
     };
 
     static const failure2Sensor_s rgFailure2Sensor[] = {
-        { MAV_FAILURE_FLAG_GPS,             MAV_SYS_STATUS_SENSOR_GPS },
-        { MAV_FAILURE_FLAG_AIRSPEED,        MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE },
-        { MAV_FAILURE_FLAG_BAROMETER,       MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE },
-        { MAV_FAILURE_FLAG_ACCELEROMETER,   MAV_SYS_STATUS_SENSOR_3D_ACCEL },
-        { MAV_FAILURE_FLAG_GYROSCOPE,       MAV_SYS_STATUS_SENSOR_3D_GYRO },
-        { MAV_FAILURE_FLAG_MAGNETOMETER,    MAV_SYS_STATUS_SENSOR_3D_MAG },
-        // { MAV_FAILURE_FLAG_MISSION,          ???? },
-        // { MAV_FAILURE_FLAG_ESTIMATOR,        ???? },
-        // { MAV_FAILURE_FLAG_LIDAR,            ???? },
-        // { MAV_FAILURE_FLAG_OFFBOARD_LINK,    ???? },
+        { HL_FAILURE_FLAG_GPS,                      MAV_SYS_STATUS_SENSOR_GPS },
+        { HL_FAILURE_FLAG_DIFFERENTIAL_PRESSURE,    MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE },
+        { HL_FAILURE_FLAG_ABSOLUTE_PRESSURE,        MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE },
+        { HL_FAILURE_FLAG_3D_ACCEL,                 MAV_SYS_STATUS_SENSOR_3D_ACCEL },
+        { HL_FAILURE_FLAG_3D_GYRO,                  MAV_SYS_STATUS_SENSOR_3D_GYRO },
+        { HL_FAILURE_FLAG_3D_MAG,                   MAV_SYS_STATUS_SENSOR_3D_MAG },
+#if 0
+        // FIXME: These don't currently map to existing sensor health bits. Support needs to be added to show them
+        // on health page of instrument panel as well.
+        { HL_FAILURE_FLAG_TERRAIN=64, /* Terrain subsystem failure. | */
+        { HL_FAILURE_FLAG_BATTERY=128, /* Battery failure/critical low battery. | */
+        { HL_FAILURE_FLAG_RC_RECEIVER=256, /* RC receiver failure/no rc connection. | */
+        { HL_FAILURE_FLAG_OFFBOARD_LINK=512, /* Offboard link failure. | */
+        { HL_FAILURE_FLAG_ENGINE=1024, /* Engine failure. | */
+        { HL_FAILURE_FLAG_GEOFENCE=2048, /* Geofence violation. | */
+        { HL_FAILURE_FLAG_ESTIMATOR=4096, /* Estimator failure, for example measurement rejection or large variances. | */
+        { HL_FAILURE_FLAG_MISSION=8192, /* Mission failure. | */
+#endif
     };
 
     // Map from MAV_FAILURE bits to standard SYS_STATUS message handling

src/comm/MockLink.cc

@@ -312,6 +312,10 @@ void MockLink::_sendHighLatency2(void)
 {
     mavlink_message_t   msg;
 
+    union px4_custom_mode   px4_cm;
+    px4_cm.data = _mavCustomMode;
+
+    qDebug() << "Sending" << _mavCustomMode;
     mavlink_msg_high_latency2_pack_chan(_vehicleSystemId,
                                         _vehicleComponentId,
                                         _mavlinkChannel,
@@ -319,7 +323,7 @@ void MockLink::_sendHighLatency2(void)
                                         0,                          // timestamp
                                         _vehicleType,               // MAV_TYPE
                                         _firmwareType,              // MAV_AUTOPILOT
-                                        _flightModeEnumValue(),     // flight_mode
+                                        px4_cm.custom_mode_hl,      // custom_mode
                                         (int32_t)(_vehicleLatitude  * 1E7),
                                         (int32_t)(_vehicleLongitude * 1E7),
                                         (int16_t)_vehicleAltitude,
@@ -340,9 +344,7 @@ void MockLink::_sendHighLatency2(void)
                                         -1,                         // battery, do not use?
                                         0,                          // wp_num
                                         0,                          // failure_flags
-                                        0,                          // failsafe
                                         0, 0, 0);                   // custom0, custom1, custom2
-
     respondWithMavlinkMessage(msg);
 }
 
@@ -1370,8 +1372,3 @@ void MockLink::_sendADSBVehicles(void)
 
     respondWithMavlinkMessage(responseMsg);
 }
-
-uint8_t MockLink::_flightModeEnumValue(void)
-{
-    return FLIGHT_MODE_STABILIZED;
-}

src/comm/MockLink.h

@@ -202,7 +202,6 @@ private:
     void _logDownloadWorker(void);
     void _sendADSBVehicles(void);
     void _moveADSBVehicle(void);
-    uint8_t _flightModeEnumValue(void);
 
     static MockLink* _startMockLink(MockConfiguration* mockConfig);