From 0b8c03cb8e5b9afae8a3738ad6655d5a7651035c Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Thu, 5 Apr 2018 17:29:25 -0700 Subject: [PATCH] Add remaining orientations, drop multi-sensor --- src/Vehicle/DistanceSensorFact.json | 42 +++++++++++++++++++ src/Vehicle/Vehicle.cc | 63 ++++++++++++++++++++++++----- src/Vehicle/Vehicle.h | 38 +++++++++++++++-- 3 files changed, 128 insertions(+), 15 deletions(-) diff --git a/src/Vehicle/DistanceSensorFact.json b/src/Vehicle/DistanceSensorFact.json index cbada4aa2..97f2837e4 100644 --- a/src/Vehicle/DistanceSensorFact.json +++ b/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" } ] diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index eb2111f75..c246d7bd7 100644 --- a/src/Vehicle/Vehicle.cc +++ b/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; isetRawValue(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::quiet_NaN()); + _rotationYaw45Fact.setRawValue(std::numeric_limits::quiet_NaN()); + _rotationYaw135Fact.setRawValue(std::numeric_limits::quiet_NaN()); _rotationYaw90Fact.setRawValue(std::numeric_limits::quiet_NaN()); _rotationYaw180Fact.setRawValue(std::numeric_limits::quiet_NaN()); + _rotationYaw225Fact.setRawValue(std::numeric_limits::quiet_NaN()); _rotationYaw270Fact.setRawValue(std::numeric_limits::quiet_NaN()); + _rotationPitch90Fact.setRawValue(std::numeric_limits::quiet_NaN()); + _rotationPitch270Fact.setRawValue(std::numeric_limits::quiet_NaN()); } diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 2c7310b2e..ac9bff6ea 100644 --- a/src/Vehicle/Vehicle.h +++ b/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 -- 2.22.0