Skip to content

Q
qgroundcontrol
Unverified Commit e15f496a authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files
Options

Merge pull request #6312 from DonLakeFlyer/DistanceSensor 

Add DISTANCE_SENSOR support to Vehicle FactGroup [WIP]
parents b810c9ec 0b8c03cb
Hide whitespace changes
Inline Side-by-side
Showing with 264 additions and 28 deletions
qgroundcontrol.qrc
View file @ e15f496a
......@@ -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/FactSystem/FactGroup.cc
View file @ e15f496a
......@@ -105,6 +105,7 @@ void FactGroup::_addFact(Fact* fact, const QString& name)
fact->setMetaData(_nameToFactMetaDataMap[name]);
}
_nameToFactMap[name] = fact;
_factNames.append(name);
}
void FactGroup::_addFactGroup(FactGroup* factGroup, const QString& name)
......
src/FactSystem/FactGroup.h
View file @ e15f496a
......@@ -38,7 +38,7 @@ public:
/// @return FactGroup for specified name, NULL if not found
Q_INVOKABLE FactGroup* getFactGroup(const QString& name);
QStringList factNames(void) const { return _nameToFactMap.keys(); }
QStringList factNames(void) const { return _factNames; }
QStringList factGroupNames(void) const { return _nameToFactGroupMap.keys(); }
protected:
......@@ -59,7 +59,7 @@ protected:
QMap<QString, Fact*> _nameToFactMap;
QMap<QString, FactGroup*> _nameToFactGroupMap;
QMap<QString, FactMetaData*> _nameToFactMetaDataMap;
QStringList _factNames;
};
#endif
src/Vehicle/DistanceSensorFact.json 0 → 100644
View file @ e15f496a
[
{
"name": "rotationNone",
"shortDescription": "Forward",
"type": "double",
"decimalPlaces": 2,
"units": "m"
},
{
"name": "rotationYaw45",
"shortDescription": "Forward/Right",
"type": "double",
"decimalPlaces": 2,
"units": "m"
},
{
"name": "rotationYaw90",
"shortDescription": "Right",
"type": "double",
"decimalPlaces": 2,
"units": "m"
},
{
"name": "rotationYaw135",
"shortDescription": "Rear/Right",
"type": "double",
"decimalPlaces": 2,
"units": "m"
},
{
"name": "rotationYaw180",
"shortDescription": "Rear",
"type": "double",
"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
View file @ e15f496a
......@@ -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);
......@@ -441,12 +444,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();
......@@ -718,6 +722,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:
{
......@@ -782,6 +789,49 @@ 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);\
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;
Fact* fact;
};
orientation2Fact_s rgOrientation2Fact[] =
{
{ 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 / 100.0); // cm to meters
}
}
}
void Vehicle::_handleAttitudeTarget(mavlink_message_t& message)
{
mavlink_attitude_target_t attitudeTarget;
......@@ -3360,3 +3410,52 @@ 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::_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)
, _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
View file @ e15f496a
......@@ -40,6 +40,65 @@ 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* 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
{
Q_OBJECT
......@@ -700,13 +759,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);
......@@ -964,6 +1024,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);
......@@ -1169,13 +1230,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;
......@@ -1199,6 +1261,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