From b3e6246fc1571b09191560a2880b9f80025905e5 Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Fri, 9 Feb 2018 11:06:33 -0800 Subject: [PATCH] Support remainder of HIGH_LATENCY2 fields --- libs/mavlink/include/mavlink/v2.0 | 2 +- src/FirmwarePlugin/FirmwarePlugin.cc | 5 +++ src/FirmwarePlugin/FirmwarePlugin.h | 3 ++ src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc | 9 ++++ src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h | 1 + src/FirmwarePlugin/PX4/px4_custom_mode.h | 6 ++- src/Vehicle/Vehicle.cc | 48 ++++++++++++++++----- src/comm/MockLink.cc | 13 +++--- src/comm/MockLink.h | 1 - 9 files changed, 66 insertions(+), 22 deletions(-) diff --git a/libs/mavlink/include/mavlink/v2.0 b/libs/mavlink/include/mavlink/v2.0 index 5db5e9440..f36b9c4c5 160000 --- a/libs/mavlink/include/mavlink/v2.0 +++ b/libs/mavlink/include/mavlink/v2.0 @@ -1 +1 @@ -Subproject commit 5db5e944048cd6126a3e58c67c9c4dcdb4d4e0ff +Subproject commit f36b9c4c5c0c9b6d33621779469de0c1e7eea457 diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index e1fd2d659..54112718f 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/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; +} diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index 4016383f1..7d95f4053 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/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; diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc index 3a5c7972e..8ea457b67 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc +++ b/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; +} diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h index 0df75a1d5..6497793e9 100644 --- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.h +++ b/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 { diff --git a/src/FirmwarePlugin/PX4/px4_custom_mode.h b/src/FirmwarePlugin/PX4/px4_custom_mode.h index 2c0087e8f..48a29f2cd 100644 --- a/src/FirmwarePlugin/PX4/px4_custom_mode.h +++ b/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; }; diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index a7b1be338..8903bc06d 100644 --- a/src/Vehicle/Vehicle.cc +++ b/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::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 diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index e2ce6a995..9ad225d6a 100644 --- a/src/comm/MockLink.cc +++ b/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; -} diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index f58730d1f..105839a0d 100644 --- a/src/comm/MockLink.h +++ b/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); -- 2.22.0