From f00988e1e87f4087dd75ce5560c318e9f6499dab Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Thu, 5 Apr 2018 16:01:26 -0700 Subject: [PATCH] Add DISTANCE_SENSOR support to Vehicle FactGroup --- qgroundcontrol.qrc | 1 + src/Vehicle/DistanceSensorFact.json | 30 +++++++++++ src/Vehicle/Vehicle.cc | 82 ++++++++++++++++++++++++----- src/Vehicle/Vehicle.h | 61 ++++++++++++++++----- 4 files changed, 148 insertions(+), 26 deletions(-) create mode 100644 src/Vehicle/DistanceSensorFact.json diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index 40f0871f6..e3e0b17f0 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -222,6 +222,7 @@ src/comm/USBBoardInfo.json src/Vehicle/BatteryFact.json src/Vehicle/ClockFact.json + src/Vehicle/DistanceSensorFact.json src/Vehicle/GPSFact.json src/Vehicle/GPSRTKFact.json src/Vehicle/SetpointFact.json diff --git a/src/Vehicle/DistanceSensorFact.json b/src/Vehicle/DistanceSensorFact.json new file mode 100644 index 000000000..cbada4aa2 --- /dev/null +++ b/src/Vehicle/DistanceSensorFact.json @@ -0,0 +1,30 @@ +[ +{ + "name": "rotationNone", + "shortDescription": "Forward", + "type": "double", + "decimalPlaces": 2, + "units": "m" +}, +{ + "name": "rotationYaw90", + "shortDescription": "Right", + "type": "double", + "decimalPlaces": 2, + "units": "m" +}, +{ + "name": "rotationYaw180", + "shortDescription": "Rear", + "type": "double", + "decimalPlaces": 2, + "units": "m" +}, +{ + "name": "rotationYaw270", + "shortDescription": "Left", + "type": "double", + "decimalPlaces": 2, + "units": "m" +} +] diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index fbbb2890a..eb2111f75 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -68,12 +68,13 @@ const char* Vehicle::_flightTimeFactName = "flightTime"; const char* Vehicle::_distanceToHomeFactName = "distanceToHome"; const char* Vehicle::_hobbsFactName = "hobbs"; -const char* Vehicle::_gpsFactGroupName = "gps"; -const char* Vehicle::_batteryFactGroupName = "battery"; -const char* Vehicle::_windFactGroupName = "wind"; -const char* Vehicle::_vibrationFactGroupName = "vibration"; -const char* Vehicle::_temperatureFactGroupName = "temperature"; -const char* Vehicle::_clockFactGroupName = "clock"; +const char* Vehicle::_gpsFactGroupName = "gps"; +const char* Vehicle::_batteryFactGroupName = "battery"; +const char* Vehicle::_windFactGroupName = "wind"; +const char* Vehicle::_vibrationFactGroupName = "vibration"; +const char* Vehicle::_temperatureFactGroupName = "temperature"; +const char* Vehicle::_clockFactGroupName = "clock"; +const char* Vehicle::_distanceSensorFactGroupName = "distanceSensor"; Vehicle::Vehicle(LinkInterface* link, int vehicleId, @@ -188,6 +189,7 @@ Vehicle::Vehicle(LinkInterface* link, , _vibrationFactGroup(this) , _temperatureFactGroup(this) , _clockFactGroup(this) + , _distanceSensorFactGroup(this) { _addLink(link); @@ -377,6 +379,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _windFactGroup(this) , _vibrationFactGroup(this) , _clockFactGroup(this) + , _distanceSensorFactGroup(this) { _commonInit(); _firmwarePlugin->initializeVehicle(this); @@ -438,12 +441,13 @@ void Vehicle::_commonInit(void) _hobbsFact.setRawValue(QVariant(QString("0000:00:00"))); _addFact(&_hobbsFact, _hobbsFactName); - _addFactGroup(&_gpsFactGroup, _gpsFactGroupName); - _addFactGroup(&_batteryFactGroup, _batteryFactGroupName); - _addFactGroup(&_windFactGroup, _windFactGroupName); - _addFactGroup(&_vibrationFactGroup, _vibrationFactGroupName); - _addFactGroup(&_temperatureFactGroup, _temperatureFactGroupName); - _addFactGroup(&_clockFactGroup, _clockFactGroupName); + _addFactGroup(&_gpsFactGroup, _gpsFactGroupName); + _addFactGroup(&_batteryFactGroup, _batteryFactGroupName); + _addFactGroup(&_windFactGroup, _windFactGroupName); + _addFactGroup(&_vibrationFactGroup, _vibrationFactGroupName); + _addFactGroup(&_temperatureFactGroup, _temperatureFactGroupName); + _addFactGroup(&_clockFactGroup, _clockFactGroupName); + _addFactGroup(&_distanceSensorFactGroup, _distanceSensorFactGroupName); // Add firmware-specific fact groups, if provided QMap* fwFactGroups = _firmwarePlugin->factGroups(); @@ -715,6 +719,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_ATTITUDE_TARGET: _handleAttitudeTarget(message); break; + case MAVLINK_MSG_ID_DISTANCE_SENSOR: + _handleDistanceSensor(message); + break; case MAVLINK_MSG_ID_SERIAL_CONTROL: { @@ -779,6 +786,33 @@ void Vehicle::_handleVfrHud(mavlink_message_t& message) _climbRateFact.setRawValue(qIsNaN(vfrHud.climb) ? 0 : vfrHud.climb); } +void Vehicle::_handleDistanceSensor(mavlink_message_t& message) +{ + mavlink_distance_sensor_t distanceSensor; + + mavlink_msg_distance_sensor_decode(&message, &distanceSensor); + + struct orientation2Fact_s { + MAV_SENSOR_ORIENTATION orientation; + Fact* fact; + }; + + orientation2Fact_s rgOrientation2Fact[] = + { + { MAV_SENSOR_ROTATION_NONE, _distanceSensorFactGroup.rotationNone() }, + { MAV_SENSOR_ROTATION_YAW_90, _distanceSensorFactGroup.rotationYaw90() }, + { MAV_SENSOR_ROTATION_YAW_180, _distanceSensorFactGroup.rotationYaw180() }, + { MAV_SENSOR_ROTATION_YAW_270, _distanceSensorFactGroup.rotationYaw270() }, + }; + + for (size_t i=0; isetRawValue(distanceSensor.current_distance); + } + } +} + void Vehicle::_handleAttitudeTarget(mavlink_message_t& message) { mavlink_attitude_target_t attitudeTarget; @@ -3357,3 +3391,27 @@ VehicleSetpointFactGroup::VehicleSetpointFactGroup(QObject* parent) _pitchRateFact.setRawValue(std::numeric_limits::quiet_NaN()); _yawRateFact.setRawValue(std::numeric_limits::quiet_NaN()); } + +const char* VehicleDistanceSensorFactGroup::_rotationNoneFactName = "rotationNone"; +const char* VehicleDistanceSensorFactGroup::_rotationYaw90FactName = "rotationYaw90"; +const char* VehicleDistanceSensorFactGroup::_rotationYaw180FactName = "rotationYaw180"; +const char* VehicleDistanceSensorFactGroup::_rotationYaw270FactName = "rotationYaw270"; + +VehicleDistanceSensorFactGroup::VehicleDistanceSensorFactGroup(QObject* parent) + : FactGroup (1000, ":/json/Vehicle/DistanceSensorFact.json", parent) + , _rotationNoneFact (0, _rotationNoneFactName, FactMetaData::valueTypeDouble) + , _rotationYaw90Fact (0, _rotationYaw90FactName, FactMetaData::valueTypeDouble) + , _rotationYaw180Fact (0, _rotationYaw180FactName, FactMetaData::valueTypeDouble) + , _rotationYaw270Fact (0, _rotationYaw270FactName, FactMetaData::valueTypeDouble) +{ + _addFact(&_rotationNoneFact, _rotationNoneFactName); + _addFact(&_rotationYaw90Fact, _rotationYaw90FactName); + _addFact(&_rotationYaw180Fact, _rotationYaw180FactName); + _addFact(&_rotationYaw270Fact, _rotationYaw270FactName); + + // Start out as not available "--.--" + _rotationNoneFact.setRawValue(std::numeric_limits::quiet_NaN()); + _rotationYaw90Fact.setRawValue(std::numeric_limits::quiet_NaN()); + _rotationYaw180Fact.setRawValue(std::numeric_limits::quiet_NaN()); + _rotationYaw270Fact.setRawValue(std::numeric_limits::quiet_NaN()); +} diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 827444ac6..2c7310b2e 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -40,6 +40,35 @@ Q_DECLARE_LOGGING_CATEGORY(VehicleLog) class Vehicle; +class VehicleDistanceSensorFactGroup : public FactGroup +{ + Q_OBJECT + +public: + VehicleDistanceSensorFactGroup(QObject* parent = NULL); + + Q_PROPERTY(Fact* rotationNone READ rotationNone CONSTANT) + Q_PROPERTY(Fact* rotationYaw90 READ rotationYaw90 CONSTANT) + Q_PROPERTY(Fact* rotationYaw180 READ rotationYaw180 CONSTANT) + Q_PROPERTY(Fact* rotationYaw270 READ rotationYaw270 CONSTANT) + + Fact* rotationNone (void) { return &_rotationNoneFact; } + Fact* rotationYaw90 (void) { return &_rotationYaw90Fact; } + Fact* rotationYaw180 (void) { return &_rotationYaw180Fact; } + Fact* rotationYaw270 (void) { return &_rotationYaw270Fact; } + + static const char* _rotationNoneFactName; + static const char* _rotationYaw90FactName; + static const char* _rotationYaw180FactName; + static const char* _rotationYaw270FactName; + +private: + Fact _rotationNoneFact; + Fact _rotationYaw90Fact; + Fact _rotationYaw180Fact; + Fact _rotationYaw270Fact; +}; + class VehicleSetpointFactGroup : public FactGroup { Q_OBJECT @@ -700,13 +729,14 @@ public: Fact* distanceToHome (void) { return &_distanceToHomeFact; } Fact* hobbs (void) { return &_hobbsFact; } - FactGroup* gpsFactGroup (void) { return &_gpsFactGroup; } - FactGroup* batteryFactGroup (void) { return &_batteryFactGroup; } - FactGroup* windFactGroup (void) { return &_windFactGroup; } - FactGroup* vibrationFactGroup (void) { return &_vibrationFactGroup; } - FactGroup* temperatureFactGroup (void) { return &_temperatureFactGroup; } - FactGroup* clockFactGroup (void) { return &_clockFactGroup; } - FactGroup* setpointFactGroup (void) { return &_setpointFactGroup; } + FactGroup* gpsFactGroup (void) { return &_gpsFactGroup; } + FactGroup* batteryFactGroup (void) { return &_batteryFactGroup; } + FactGroup* windFactGroup (void) { return &_windFactGroup; } + FactGroup* vibrationFactGroup (void) { return &_vibrationFactGroup; } + FactGroup* temperatureFactGroup (void) { return &_temperatureFactGroup; } + FactGroup* clockFactGroup (void) { return &_clockFactGroup; } + FactGroup* setpointFactGroup (void) { return &_setpointFactGroup; } + FactGroup* distanceSensorFactGroup (void) { return &_distanceSensorFactGroup; } void setConnectionLostEnabled(bool connectionLostEnabled); @@ -963,6 +993,7 @@ private: void _handleHighLatency2(mavlink_message_t& message); void _handleAttitude(mavlink_message_t& message); void _handleAttitudeTarget(mavlink_message_t& message); + void _handleDistanceSensor(mavlink_message_t& message); // ArduPilot dialect messages #if !defined(NO_ARDUPILOT_DIALECT) void _handleCameraFeedback(const mavlink_message_t& message); @@ -1168,13 +1199,14 @@ private: Fact _distanceToHomeFact; Fact _hobbsFact; - VehicleGPSFactGroup _gpsFactGroup; - VehicleBatteryFactGroup _batteryFactGroup; - VehicleWindFactGroup _windFactGroup; - VehicleVibrationFactGroup _vibrationFactGroup; - VehicleTemperatureFactGroup _temperatureFactGroup; - VehicleClockFactGroup _clockFactGroup; - VehicleSetpointFactGroup _setpointFactGroup; + VehicleGPSFactGroup _gpsFactGroup; + VehicleBatteryFactGroup _batteryFactGroup; + VehicleWindFactGroup _windFactGroup; + VehicleVibrationFactGroup _vibrationFactGroup; + VehicleTemperatureFactGroup _temperatureFactGroup; + VehicleClockFactGroup _clockFactGroup; + VehicleSetpointFactGroup _setpointFactGroup; + VehicleDistanceSensorFactGroup _distanceSensorFactGroup; static const char* _rollFactName; static const char* _pitchFactName; @@ -1198,6 +1230,7 @@ private: static const char* _vibrationFactGroupName; static const char* _temperatureFactGroupName; static const char* _clockFactGroupName; + static const char* _distanceSensorFactGroupName; static const int _vehicleUIUpdateRateMSecs = 100; -- 2.22.0