Skip to content

Q
qgroundcontrol
Commit f00988e1 authored by DonLakeFlyer's avatar DonLakeFlyer
Browse files
Options

Add DISTANCE_SENSOR support to Vehicle FactGroup

parent 9b7cfd25
Hide whitespace changes
Inline Side-by-side
Showing with 148 additions and 26 deletions
qgroundcontrol.qrc
View file @ f00988e1
......@@ -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 0 → 100644
View file @ f00988e1
[
{
"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
View file @ f00988e1
......@@ -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<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
View file @ f00988e1
......@@ -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;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment