zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merge pull request #6312 from DonLakeFlyer/DistanceSensor 

Add DISTANCE_SENSOR support to Vehicle FactGroup [WIP]
QGC4.4
Don Gagne 7 years ago committed by GitHub
parent
b810c9ecb0 0b8c03cb8e
commit
e15f496abb
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 264 additions and 28 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      qgroundcontrol.qrc
  2. 1
      src/FactSystem/FactGroup.cc
  3. 4
      src/FactSystem/FactGroup.h
  4. 72
      src/Vehicle/DistanceSensorFact.json
  5. 123
      src/Vehicle/Vehicle.cc
  6. 91
      src/Vehicle/Vehicle.h

1
qgroundcontrol.qrc
Unescape View File

@ -222,6 +222,7 @@ @@ -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>

1
src/FactSystem/FactGroup.cc
Unescape View File

@ -105,6 +105,7 @@ void FactGroup::_addFact(Fact* fact, const QString& name) @@ -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)

4
src/FactSystem/FactGroup.h
Unescape View File

@ -38,7 +38,7 @@ public: @@ -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: @@ -59,7 +59,7 @@ protected:
QMap<QString, Fact*> _nameToFactMap;
QMap<QString, FactGroup*> _nameToFactGroupMap;
QMap<QString, FactMetaData*> _nameToFactMetaDataMap;
QStringList _factNames;
};
#endif

72
src/Vehicle/DistanceSensorFact.json
Unescape View File

@ -0,0 +1,72 @@ @@ -0,0 +1,72 @@
[
{
"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"
}
]

123
src/Vehicle/Vehicle.cc
Unescape View File

@ -68,12 +68,13 @@ const char* Vehicle::_flightTimeFactName = "flightTime"; @@ -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, @@ -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, @@ -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) @@ -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 @@ -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) @@ -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) @@ -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());
}

91
src/Vehicle/Vehicle.h
Unescape View File

@ -40,6 +40,65 @@ Q_DECLARE_LOGGING_CATEGORY(VehicleLog) @@ -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: @@ -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: @@ -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: @@ -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: @@ -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;

Write Preview
Loading…
Cancel
Save