Add remaining orientations, drop multi-sensor

src/Vehicle/DistanceSensorFact.json

@@ -6,6 +6,13 @@
     "decimalPlaces":    2,
     "units":            "m"
 },
+{
+    "name":             "rotationYaw45",
+    "shortDescription": "Forward/Right",
+    "type":             "double",
+    "decimalPlaces":    2,
+    "units":            "m"
+},
 {
     "name":             "rotationYaw90",
     "shortDescription": "Right",
@@ -13,6 +20,13 @@
     "decimalPlaces":    2,
     "units":            "m"
 },
+{
+    "name":             "rotationYaw135",
+    "shortDescription": "Rear/Right",
+    "type":             "double",
+    "decimalPlaces":    2,
+    "units":            "m"
+},
 {
     "name":             "rotationYaw180",
     "shortDescription": "Rear",
@@ -20,11 +34,39 @@
     "decimalPlaces":    2,
     "units":            "m"
 },
+{
+    "name":             "rotationYaw225",
+    "shortDescription": "Rear/Left",
+    "type":             "double",
+    "decimalPlaces":    2,
+    "units":            "m"
+},
 {
     "name":             "rotationYaw270",
     "shortDescription": "Left",
     "type":             "double",
     "decimalPlaces":    2,
     "units":            "m"
+},
+{
+    "name":             "rotationYaw315",
+    "shortDescription": "Forward/Left",
+    "type":             "double",
+    "decimalPlaces":    2,
+    "units":            "m"
+},
+{
+    "name":             "rotationPitch90",
+    "shortDescription": "Up",
+    "type":             "double",
+    "decimalPlaces":    2,
+    "units":            "m"
+},
+{
+    "name":             "rotationPitch270",
+    "shortDescription": "Down",
+    "type":             "double",
+    "decimalPlaces":    2,
+    "units":            "m"
 }
 ]

src/Vehicle/Vehicle.cc

@@ -790,7 +790,17 @@ void Vehicle::_handleDistanceSensor(mavlink_message_t& message)
 {
     mavlink_distance_sensor_t distanceSensor;
 
-    mavlink_msg_distance_sensor_decode(&message, &distanceSensor);
+    mavlink_msg_distance_sensor_decode(&message, &distanceSensor);\
+
+    if (!_distanceSensorFactGroup.idSet()) {
+        _distanceSensorFactGroup.setIdSet(true);
+        _id = distanceSensor.id;
+    }
+
+    if (_id != distanceSensor.id) {
+        // We can only handle a single sensor reporting
+        return;
+    }
 
     struct orientation2Fact_s {
         MAV_SENSOR_ORIENTATION  orientation;
@@ -799,16 +809,22 @@ void Vehicle::_handleDistanceSensor(mavlink_message_t& message)
 
     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() },
+        { MAV_SENSOR_ROTATION_NONE,         _distanceSensorFactGroup.rotationNone() },
+        { MAV_SENSOR_ROTATION_YAW_45,       _distanceSensorFactGroup.rotationYaw45() },
+        { MAV_SENSOR_ROTATION_YAW_90,       _distanceSensorFactGroup.rotationYaw90() },
+        { MAV_SENSOR_ROTATION_YAW_135,      _distanceSensorFactGroup.rotationYaw135() },
+        { MAV_SENSOR_ROTATION_YAW_180,      _distanceSensorFactGroup.rotationYaw180() },
+        { MAV_SENSOR_ROTATION_YAW_225,      _distanceSensorFactGroup.rotationYaw225() },
+        { MAV_SENSOR_ROTATION_YAW_270,      _distanceSensorFactGroup.rotationYaw270() },
+        { MAV_SENSOR_ROTATION_YAW_315,      _distanceSensorFactGroup.rotationYaw315() },
+        { MAV_SENSOR_ROTATION_PITCH_90,     _distanceSensorFactGroup.rotationPitch90() },
+        { MAV_SENSOR_ROTATION_PITCH_270,    _distanceSensorFactGroup.rotationPitch270() },
     };
 
     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);
+            orientation2Fact.fact->setRawValue(distanceSensor.current_distance / 100.0); // cm to meters
         }
     }
 }
@@ -3393,25 +3409,50 @@ VehicleSetpointFactGroup::VehicleSetpointFactGroup(QObject* parent)
 }
 
 const char* VehicleDistanceSensorFactGroup::_rotationNoneFactName =     "rotationNone";
+const char* VehicleDistanceSensorFactGroup::_rotationYaw45FactName =    "rotationYaw45";
 const char* VehicleDistanceSensorFactGroup::_rotationYaw90FactName =    "rotationYaw90";
+const char* VehicleDistanceSensorFactGroup::_rotationYaw135FactName =   "rotationYaw135";
 const char* VehicleDistanceSensorFactGroup::_rotationYaw180FactName =   "rotationYaw180";
+const char* VehicleDistanceSensorFactGroup::_rotationYaw225FactName =   "rotationYaw225";
 const char* VehicleDistanceSensorFactGroup::_rotationYaw270FactName =   "rotationYaw270";
+const char* VehicleDistanceSensorFactGroup::_rotationYaw315FactName =   "rotationYaw315";
+const char* VehicleDistanceSensorFactGroup::_rotationPitch90FactName =  "rotationPitch90";
+const char* VehicleDistanceSensorFactGroup::_rotationPitch270FactName = "rotationPitch270";
 
 VehicleDistanceSensorFactGroup::VehicleDistanceSensorFactGroup(QObject* parent)
     : FactGroup             (1000, ":/json/Vehicle/DistanceSensorFact.json", parent)
     , _rotationNoneFact     (0, _rotationNoneFactName,      FactMetaData::valueTypeDouble)
+    , _rotationYaw45Fact    (0, _rotationYaw45FactName,     FactMetaData::valueTypeDouble)
     , _rotationYaw90Fact    (0, _rotationYaw90FactName,     FactMetaData::valueTypeDouble)
+    , _rotationYaw135Fact   (0, _rotationYaw135FactName,    FactMetaData::valueTypeDouble)
     , _rotationYaw180Fact   (0, _rotationYaw180FactName,    FactMetaData::valueTypeDouble)
+    , _rotationYaw225Fact   (0, _rotationYaw225FactName,    FactMetaData::valueTypeDouble)
     , _rotationYaw270Fact   (0, _rotationYaw270FactName,    FactMetaData::valueTypeDouble)
-{
-    _addFact(&_rotationNoneFact,    _rotationNoneFactName);
-    _addFact(&_rotationYaw90Fact,   _rotationYaw90FactName);
-    _addFact(&_rotationYaw180Fact,  _rotationYaw180FactName);
-    _addFact(&_rotationYaw270Fact,  _rotationYaw270FactName);
+    , _rotationYaw315Fact   (0, _rotationYaw315FactName,    FactMetaData::valueTypeDouble)
+    , _rotationPitch90Fact  (0, _rotationPitch90FactName,   FactMetaData::valueTypeDouble)
+    , _rotationPitch270Fact (0, _rotationPitch270FactName,  FactMetaData::valueTypeDouble)
+    , _idSet                (false)
+    , _id                   (0)
+{
+    _addFact(&_rotationNoneFact,        _rotationNoneFactName);
+    _addFact(&_rotationYaw45Fact,       _rotationYaw45FactName);
+    _addFact(&_rotationYaw90Fact,       _rotationYaw90FactName);
+    _addFact(&_rotationYaw135Fact,      _rotationYaw135FactName);
+    _addFact(&_rotationYaw180Fact,      _rotationYaw180FactName);
+    _addFact(&_rotationYaw225Fact,      _rotationYaw225FactName);
+    _addFact(&_rotationYaw270Fact,      _rotationYaw270FactName);
+    _addFact(&_rotationYaw315Fact,      _rotationYaw315FactName);
+    _addFact(&_rotationPitch90Fact,     _rotationPitch90FactName);
+    _addFact(&_rotationPitch270Fact,    _rotationPitch270FactName);
 
     // Start out as not available "--.--"
     _rotationNoneFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
+    _rotationYaw45Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
+    _rotationYaw135Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
     _rotationYaw90Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
     _rotationYaw180Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
+    _rotationYaw225Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
     _rotationYaw270Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
+    _rotationPitch90Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
+    _rotationPitch270Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
 }

src/Vehicle/Vehicle.h

@@ -47,26 +47,56 @@ class VehicleDistanceSensorFactGroup : public FactGroup
 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)
+    Q_PROPERTY(Fact* rotationNone       READ rotationNone       CONSTANT)
+    Q_PROPERTY(Fact* rotationYaw45      READ rotationYaw45      CONSTANT)
+    Q_PROPERTY(Fact* rotationYaw90      READ rotationYaw90      CONSTANT)
+    Q_PROPERTY(Fact* rotationYaw135     READ rotationYaw135     CONSTANT)
+    Q_PROPERTY(Fact* rotationYaw180     READ rotationYaw180     CONSTANT)
+    Q_PROPERTY(Fact* rotationYaw225     READ rotationYaw225     CONSTANT)
+    Q_PROPERTY(Fact* rotationYaw270     READ rotationYaw270     CONSTANT)
+    Q_PROPERTY(Fact* rotationYaw315     READ rotationYaw315     CONSTANT)
+    Q_PROPERTY(Fact* rotationPitch90    READ rotationPitch90    CONSTANT)
+    Q_PROPERTY(Fact* rotationPitch270   READ rotationPitch270   CONSTANT)
 
     Fact* rotationNone      (void) { return &_rotationNoneFact; }
+    Fact* rotationYaw45     (void) { return &_rotationYaw45Fact; }
     Fact* rotationYaw90     (void) { return &_rotationYaw90Fact; }
+    Fact* rotationYaw135    (void) { return &_rotationYaw90Fact; }
     Fact* rotationYaw180    (void) { return &_rotationYaw180Fact; }
+    Fact* rotationYaw225    (void) { return &_rotationYaw180Fact; }
     Fact* rotationYaw270    (void) { return &_rotationYaw270Fact; }
+    Fact* rotationYaw315    (void) { return &_rotationYaw315Fact; }
+    Fact* rotationPitch90   (void) { return &_rotationPitch90Fact; }
+    Fact* rotationPitch270  (void) { return &_rotationPitch270Fact; }
+
+    bool idSet(void) { return _idSet; }
+    void setIdSet(bool idSet) { _idSet = idSet; }
 
     static const char* _rotationNoneFactName;
+    static const char* _rotationYaw45FactName;
     static const char* _rotationYaw90FactName;
+    static const char* _rotationYaw135FactName;
     static const char* _rotationYaw180FactName;
+    static const char* _rotationYaw225FactName;
     static const char* _rotationYaw270FactName;
+    static const char* _rotationYaw315FactName;
+    static const char* _rotationPitch90FactName;
+    static const char* _rotationPitch270FactName;
 
 private:
     Fact _rotationNoneFact;
+    Fact _rotationYaw45Fact;
     Fact _rotationYaw90Fact;
+    Fact _rotationYaw135Fact;
     Fact _rotationYaw180Fact;
+    Fact _rotationYaw225Fact;
     Fact _rotationYaw270Fact;
+    Fact _rotationYaw315Fact;
+    Fact _rotationPitch90Fact;
+    Fact _rotationPitch270Fact;
+
+    bool    _idSet; // true: _id is set to seen sensor id
+    uint8_t _id;    // The id for the sensor being tracked. Current support for only a single sensor.
 };
 
 class VehicleSetpointFactGroup : public FactGroup