diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc
index 40f0871f666064b7b223e552cf6e8c5acd05b2ae..e3e0b17f03ec774a1b7620674f2b9a65e967f25f 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 0000000000000000000000000000000000000000..cbada4aa22a46f99c094d4aae4b85ed0713e4e68
--- /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 fbbb2890aa5e3e667dcff22fce29150367c7b68e..eb2111f758d53907459eb5972438e85841b80d9c 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 827444ac6e83d91111effc8c2d81d8298a58391b..2c7310b2ec886b767d8fca559bb13fa919fc05aa 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;