Add DISTANCE_SENSOR support to Vehicle FactGroup

qgroundcontrol.qrc

@@ -222,6 +222,7 @@
         <file alias="USBBoardInfo.json">src/comm/USBBoardInfo.json</file>
         <file alias="Vehicle/BatteryFact.json">src/Vehicle/BatteryFact.json</file>
         <file alias="Vehicle/ClockFact.json">src/Vehicle/ClockFact.json</file>
+		<file alias="Vehicle/DistanceSensorFact.json">src/Vehicle/DistanceSensorFact.json</file>
         <file alias="Vehicle/GPSFact.json">src/Vehicle/GPSFact.json</file>
         <file alias="Vehicle/GPSRTKFact.json">src/Vehicle/GPSRTKFact.json</file>
 		<file alias="Vehicle/SetpointFact.json">src/Vehicle/SetpointFact.json</file>

src/Vehicle/DistanceSensorFact.json

+[
+{
+    "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"
+}
+]

src/Vehicle/Vehicle.cc

@@ -74,6 +74,7 @@ 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);
@@ -444,6 +447,7 @@ void Vehicle::_commonInit(void)
     _addFactGroup(&_vibrationFactGroup,         _vibrationFactGroupName);
     _addFactGroup(&_temperatureFactGroup,       _temperatureFactGroupName);
     _addFactGroup(&_clockFactGroup,             _clockFactGroupName);
+    _addFactGroup(&_distanceSensorFactGroup,    _distanceSensorFactGroupName);
 
     // Add firmware-specific fact groups, if provided
     QMap<QString, FactGroup*>* 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; i<sizeof(rgOrientation2Fact)/sizeof(rgOrientation2Fact[0]); i++) {
+        const orientation2Fact_s& orientation2Fact = rgOrientation2Fact[i];
+        if (orientation2Fact.orientation == distanceSensor.orientation) {
+            orientation2Fact.fact->setRawValue(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<float>::quiet_NaN());
     _yawRateFact.setRawValue(std::numeric_limits<float>::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<float>::quiet_NaN());
+    _rotationYaw90Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
+    _rotationYaw180Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
+    _rotationYaw270Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
+}

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
@@ -707,6 +736,7 @@ public:
     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);
@@ -1175,6 +1206,7 @@ private:
     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;