Skip to content

Q
qgroundcontrol
Commit 70f899c6 authored by Don Gagne's avatar Don Gagne Committed by Lorenz Meier
Browse files
Options

Add FactGroup for ESTIMATOR_STATUS values

parent 71f74f3f
Hide whitespace changes
Inline Side-by-side
Showing with 447 additions and 56 deletions
ChangeLog.md
View file @ 70f899c6
......@@ -5,10 +5,10 @@ Note: This file only contains high level features or important fixes.
## 3.5
### 3.5.0 - Not yet released
*
* Add ESTIMATOR_STATUS values to new estimatorStatus Vehicle FactGroup. These are now available to display in instrument panel.
## 3.4
### 3.4.1 - Not yet released
* Fix crash when Survery with terrain follow is moved quickly
* Fix terrain follow climb/descent rate fields swapped in ui
\ No newline at end of file
* Fix terrain follow climb/descent rate fields swapped in ui
qgroundcontrol.qrc
View file @ 70f899c6
......@@ -237,6 +237,7 @@
<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/EstimatorStatusFactGroup.json">src/Vehicle/EstimatorStatusFactGroup.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/EstimatorStatusFactGroup.json 0 → 100644
View file @ 70f899c6
[
{
"name": "goodAttitudeEsimate",
"shortDescription": "Good Attitude Esimate",
"type": "bool",
"default": false
},
{
"name": "goodHorizVelEstimate",
"shortDescription": "Good Horiz Vel Estimate",
"type": "bool",
"default": false
},
{
"name": "goodVertVelEstimate",
"shortDescription": "Good Vert Vel Estimate",
"type": "bool",
"default": false
},
{
"name": "goodHorizPosRelEstimate",
"shortDescription": "Good Horiz Pos Rel Estimate",
"type": "bool",
"default": false
},
{
"name": "goodHorizPosAbsEstimate",
"shortDescription": "Good Horiz Pos Abs Estimate",
"type": "bool",
"default": false
},
{
"name": "goodVertPosAbsEstimate",
"shortDescription": "Good Vert Pos Abs Estimate",
"type": "bool",
"default": false
},
{
"name": "goodVertPosAGLEstimate",
"shortDescription": "Good Vert Pos AGL Estimate",
"type": "bool",
"default": false
},
{
"name": "goodConstPosModeEstimate",
"shortDescription": "Good Const Pos Mode Estimate",
"type": "bool",
"default": false
},
{
"name": "goodPredHorizPosRelEstimate",
"shortDescription": "Good Pred Horiz Pos Rel Estimate",
"type": "bool",
"default": false
},
{
"name": "goodPredHorizPosAbsEstimate",
"shortDescription": "Good Pred Horiz Pos Abs Estimate",
"type": "bool",
"default": false
},
{
"name": "gpsGlitch",
"shortDescription": "Gps Glitch",
"type": "bool",
"default": false
},
{
"name": "accelError",
"shortDescription": "Accel Error",
"type": "bool",
"default": false
},
{
"name": "velRatio",
"shortDescription": "Vel Ratio",
"type": "float",
"decimalPlaces": 2,
"default": null
},
{
"name": "horizPosRatio",
"shortDescription": "Horiz Pos Ratio",
"type": "float",
"decimalPlaces": 2,
"default": null
},
{
"name": "vertPosRatio",
"shortDescription": "Vert Pos Ratio",
"type": "float",
"decimalPlaces": 2,
"default": null
},
{
"name": "magRatio",
"shortDescription": "Mag Ratio",
"type": "float",
"decimalPlaces": 2,
"default": null
},
{
"name": "haglRatio",
"shortDescription": "HAGL Ratio",
"type": "float",
"decimalPlaces": 2,
"default": null
},
{
"name": "tasRatio",
"shortDescription": "TAS Ratio",
"type": "float",
"decimalPlaces": 2,
"default": null
},
{
"name": "horizPosAccuracy",
"shortDescription": "Horiz Pos Accuracy",
"type": "float",
"decimalPlaces": 2,
"default": null
},
{
"name": "vertPosAccuracy",
"shortDescription": "Vert Pos Accuracy",
"type": "float",
"decimalPlaces": 2,
"default": null
}
]
src/Vehicle/Vehicle.cc
View file @ 70f899c6
......@@ -68,14 +68,15 @@ const char* Vehicle::_flightTimeFactName = "flightTime";
const char* Vehicle::_distanceToHomeFactName = "distanceToHome";
const char* Vehicle::_hobbsFactName = "hobbs";
const char* Vehicle::_gpsFactGroupName = "gps";
const char* Vehicle::_battery1FactGroupName = "battery";
const char* Vehicle::_battery2FactGroupName = "battery2";
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";
const char* Vehicle::_gpsFactGroupName = "gps";
const char* Vehicle::_battery1FactGroupName = "battery";
const char* Vehicle::_battery2FactGroupName = "battery2";
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";
const char* Vehicle::_estimatorStatusFactGroupName = "estimatorStatus";
Vehicle::Vehicle(LinkInterface* link,
int vehicleId,
......@@ -91,17 +92,17 @@ Vehicle::Vehicle(LinkInterface* link,
, _offlineEditingVehicle(false)
, _firmwareType(firmwareType)
, _vehicleType(vehicleType)
, _firmwarePlugin(NULL)
, _firmwarePluginInstanceData(NULL)
, _autopilotPlugin(NULL)
, _mavlink(NULL)
, _firmwarePlugin(nullptr)
, _firmwarePluginInstanceData(nullptr)
, _autopilotPlugin(nullptr)
, _mavlink(nullptr)
, _soloFirmware(false)
, _toolbox(qgcApp()->toolbox())
, _settingsManager(_toolbox->settingsManager())
, _joystickMode(JoystickModeRC)
, _joystickEnabled(false)
, _uas(NULL)
, _mav(NULL)
, _uas(nullptr)
, _mav(nullptr)
, _currentMessageCount(0)
, _messageCount(0)
, _currentErrorCount(0)
......@@ -135,17 +136,17 @@ Vehicle::Vehicle(LinkInterface* link,
, _capabilityBits(0)
, _highLatencyLink(false)
, _receivingAttitudeQuaternion(false)
, _cameras(NULL)
, _cameras(nullptr)
, _connectionLost(false)
, _connectionLostEnabled(true)
, _initialPlanRequestComplete(false)
, _missionManager(NULL)
, _missionManager(nullptr)
, _missionManagerInitialRequestSent(false)
, _geoFenceManager(NULL)
, _geoFenceManager(nullptr)
, _geoFenceManagerInitialRequestSent(false)
, _rallyPointManager(NULL)
, _rallyPointManager(nullptr)
, _rallyPointManagerInitialRequestSent(false)
, _parameterManager(NULL)
, _parameterManager(nullptr)
, _armed(false)
, _base_mode(0)
, _custom_mode(0)
......@@ -194,6 +195,7 @@ Vehicle::Vehicle(LinkInterface* link,
, _temperatureFactGroup(this)
, _clockFactGroup(this)
, _distanceSensorFactGroup(this)
, _estimatorStatusFactGroup(this)
{
connect(_joystickManager, &JoystickManager::activeJoystickChanged, this, &Vehicle::_loadSettings);
connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::activeVehicleAvailableChanged, this, &Vehicle::_loadSettings);
......@@ -284,17 +286,17 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType,
, _offlineEditingVehicle(true)
, _firmwareType(firmwareType)
, _vehicleType(vehicleType)
, _firmwarePlugin(NULL)
, _firmwarePluginInstanceData(NULL)
, _autopilotPlugin(NULL)
, _mavlink(NULL)
, _firmwarePlugin(nullptr)
, _firmwarePluginInstanceData(nullptr)
, _autopilotPlugin(nullptr)
, _mavlink(nullptr)
, _soloFirmware(false)
, _toolbox(qgcApp()->toolbox())
, _settingsManager(_toolbox->settingsManager())
, _joystickMode(JoystickModeRC)
, _joystickEnabled(false)
, _uas(NULL)
, _mav(NULL)
, _uas(nullptr)
, _mav(nullptr)
, _currentMessageCount(0)
, _messageCount(0)
, _currentErrorCount(0)
......@@ -320,23 +322,23 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType,
, _capabilityBits(_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA ? 0 : MAV_PROTOCOL_CAPABILITY_MISSION_FENCE | MAV_PROTOCOL_CAPABILITY_MISSION_RALLY)
, _highLatencyLink(false)
, _receivingAttitudeQuaternion(false)
, _cameras(NULL)
, _cameras(nullptr)
, _connectionLost(false)
, _connectionLostEnabled(true)
, _initialPlanRequestComplete(false)
, _missionManager(NULL)
, _missionManager(nullptr)
, _missionManagerInitialRequestSent(false)
, _geoFenceManager(NULL)
, _geoFenceManager(nullptr)
, _geoFenceManagerInitialRequestSent(false)
, _rallyPointManager(NULL)
, _rallyPointManager(nullptr)
, _rallyPointManagerInitialRequestSent(false)
, _parameterManager(NULL)
, _parameterManager(nullptr)
, _armed(false)
, _base_mode(0)
, _custom_mode(0)
, _nextSendMessageMultipleIndex(0)
, _firmwarePluginManager(firmwarePluginManager)
, _joystickManager(NULL)
, _joystickManager(nullptr)
, _flowImageIndex(0)
, _allLinksInactiveSent(false)
, _messagesReceived(0)
......@@ -449,6 +451,7 @@ void Vehicle::_commonInit(void)
_addFactGroup(&_temperatureFactGroup, _temperatureFactGroupName);
_addFactGroup(&_clockFactGroup, _clockFactGroupName);
_addFactGroup(&_distanceSensorFactGroup, _distanceSensorFactGroupName);
_addFactGroup(&_estimatorStatusFactGroup, _estimatorStatusFactGroupName);
// Add firmware-specific fact groups, if provided
QMap<QString, FactGroup*>* fwFactGroups = _firmwarePlugin->factGroups();
......@@ -469,13 +472,13 @@ Vehicle::~Vehicle()
qCDebug(VehicleLog) << "~Vehicle" << this;
delete _missionManager;
_missionManager = NULL;
_missionManager = nullptr;
delete _autopilotPlugin;
_autopilotPlugin = NULL;
_autopilotPlugin = nullptr;
delete _mav;
_mav = NULL;
_mav = nullptr;
}
......@@ -483,7 +486,7 @@ void Vehicle::prepareDelete()
{
if(_cameras) {
delete _cameras;
_cameras = NULL;
_cameras = nullptr;
emit dynamicCamerasChanged();
qApp->processEvents();
}
......@@ -720,6 +723,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
case MAVLINK_MSG_ID_DISTANCE_SENSOR:
_handleDistanceSensor(message);
break;
case MAVLINK_MSG_ID_ESTIMATOR_STATUS:
_handleEstimatorStatus(message);
break;
case MAVLINK_MSG_ID_PING:
_handlePing(link, message);
break;
......@@ -787,6 +793,64 @@ void Vehicle::_handleVfrHud(mavlink_message_t& message)
_climbRateFact.setRawValue(qIsNaN(vfrHud.climb) ? 0 : vfrHud.climb);
}
void Vehicle::_handleEstimatorStatus(mavlink_message_t& message)
{
mavlink_estimator_status_t estimatorStatus;
mavlink_msg_estimator_status_decode(&message, &estimatorStatus);
_estimatorStatusFactGroup.goodAttitudeEstimate()->setRawValue(!!(estimatorStatus.flags & ESTIMATOR_ATTITUDE));
_estimatorStatusFactGroup.goodHorizVelEstimate()->setRawValue(!!(estimatorStatus.flags & ESTIMATOR_VELOCITY_HORIZ));
_estimatorStatusFactGroup.goodVertVelEstimate()->setRawValue(!!(estimatorStatus.flags & ESTIMATOR_VELOCITY_VERT));
_estimatorStatusFactGroup.goodHorizPosRelEstimate()->setRawValue(!!(estimatorStatus.flags & ESTIMATOR_POS_HORIZ_REL));
_estimatorStatusFactGroup.goodHorizPosAbsEstimate()->setRawValue(!!(estimatorStatus.flags & ESTIMATOR_POS_HORIZ_ABS));
_estimatorStatusFactGroup.goodVertPosAbsEstimate()->setRawValue(!!(estimatorStatus.flags & ESTIMATOR_POS_VERT_ABS));
_estimatorStatusFactGroup.goodVertPosAGLEstimate()->setRawValue(!!(estimatorStatus.flags & ESTIMATOR_POS_VERT_AGL));
_estimatorStatusFactGroup.goodConstPosModeEstimate()->setRawValue(!!(estimatorStatus.flags & ESTIMATOR_CONST_POS_MODE));
_estimatorStatusFactGroup.goodPredHorizPosRelEstimate()->setRawValue(!!(estimatorStatus.flags & ESTIMATOR_PRED_POS_HORIZ_REL));
_estimatorStatusFactGroup.goodPredHorizPosAbsEstimate()->setRawValue(!!(estimatorStatus.flags & ESTIMATOR_PRED_POS_HORIZ_ABS));
_estimatorStatusFactGroup.gpsGlitch()->setRawValue(estimatorStatus.flags & ESTIMATOR_GPS_GLITCH ? true : false);
_estimatorStatusFactGroup.accelError()->setRawValue(!!(estimatorStatus.flags & ESTIMATOR_ACCEL_ERROR));
_estimatorStatusFactGroup.velRatio()->setRawValue(estimatorStatus.vel_ratio);
_estimatorStatusFactGroup.horizPosRatio()->setRawValue(estimatorStatus.pos_horiz_ratio);
_estimatorStatusFactGroup.vertPosRatio()->setRawValue(estimatorStatus.pos_vert_ratio);
_estimatorStatusFactGroup.magRatio()->setRawValue(estimatorStatus.mag_ratio);
_estimatorStatusFactGroup.haglRatio()->setRawValue(estimatorStatus.hagl_ratio);
_estimatorStatusFactGroup.tasRatio()->setRawValue(estimatorStatus.tas_ratio);
_estimatorStatusFactGroup.horizPosAccuracy()->setRawValue(estimatorStatus.pos_horiz_accuracy);
_estimatorStatusFactGroup.vertPosAccuracy()->setRawValue(estimatorStatus.pos_vert_accuracy);
#if 0
typedef enum ESTIMATOR_STATUS_FLAGS
{
ESTIMATOR_ATTITUDE=1, /* True if the attitude estimate is good | */
ESTIMATOR_VELOCITY_HORIZ=2, /* True if the horizontal velocity estimate is good | */
ESTIMATOR_VELOCITY_VERT=4, /* True if the vertical velocity estimate is good | */
ESTIMATOR_POS_HORIZ_REL=8, /* True if the horizontal position (relative) estimate is good | */
ESTIMATOR_POS_HORIZ_ABS=16, /* True if the horizontal position (absolute) estimate is good | */
ESTIMATOR_POS_VERT_ABS=32, /* True if the vertical position (absolute) estimate is good | */
ESTIMATOR_POS_VERT_AGL=64, /* True if the vertical position (above ground) estimate is good | */
ESTIMATOR_CONST_POS_MODE=128, /* True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow) | */
ESTIMATOR_PRED_POS_HORIZ_REL=256, /* True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate | */
ESTIMATOR_PRED_POS_HORIZ_ABS=512, /* True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate | */
ESTIMATOR_GPS_GLITCH=1024, /* True if the EKF has detected a GPS glitch | */
ESTIMATOR_ACCEL_ERROR=2048, /* True if the EKF has detected bad accelerometer data | */
typedef struct __mavlink_estimator_status_t {
uint64_t time_usec; /*< Timestamp (micros since boot or Unix epoch)*/
float vel_ratio; /*< Velocity innovation test ratio*/
float pos_horiz_ratio; /*< Horizontal position innovation test ratio*/
float pos_vert_ratio; /*< Vertical position innovation test ratio*/
float mag_ratio; /*< Magnetometer innovation test ratio*/
float hagl_ratio; /*< Height above terrain innovation test ratio*/
float tas_ratio; /*< True airspeed innovation test ratio*/
float pos_horiz_accuracy; /*< Horizontal position 1-STD accuracy relative to the EKF local origin (m)*/
float pos_vert_accuracy; /*< Vertical position 1-STD accuracy relative to the EKF local origin (m)*/
uint16_t flags; /*< Integer bitmask indicating which EKF outputs are valid. See definition for ESTIMATOR_STATUS_FLAGS.*/
}) mavlink_estimator_status_t;
#endif
}
void Vehicle::_handleDistanceSensor(mavlink_message_t& message)
{
mavlink_distance_sensor_t distanceSensor;
......@@ -1748,11 +1812,11 @@ void Vehicle::_updatePriorityLink(bool updateActive, bool sendCommand)
}
}
LinkInterface* newPriorityLink = NULL;
LinkInterface* newPriorityLink = nullptr;
// This routine specifically does not clear _priorityLink when there are no links remaining.
// By doing this we hold a reference on the last link as the Vehicle shuts down. Thus preventing shutdown
// ordering NULL pointer crashes where priorityLink() is still called during shutdown sequence.
// ordering nullptr pointer crashes where priorityLink() is still called during shutdown sequence.
if (_links.count() == 0) {
return;
}
......@@ -2164,7 +2228,7 @@ void Vehicle::setPriorityLinkByName(const QString& priorityLinkName)
return;
}
LinkInterface* newPriorityLink = NULL;
LinkInterface* newPriorityLink = nullptr;
for (int i=0; i<_links.count(); i++) {
......@@ -3758,3 +3822,69 @@ VehicleDistanceSensorFactGroup::VehicleDistanceSensorFactGroup(QObject* parent)
_rotationPitch90Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
_rotationPitch270Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
}
const char* VehicleEstimatorStatusFactGroup::_goodAttitudeEstimateFactName = "goodAttitudeEsimate";
const char* VehicleEstimatorStatusFactGroup::_goodHorizVelEstimateFactName = "goodHorizVelEstimate";
const char* VehicleEstimatorStatusFactGroup::_goodVertVelEstimateFactName = "goodVertVelEstimate";
const char* VehicleEstimatorStatusFactGroup::_goodHorizPosRelEstimateFactName = "goodHorizPosRelEstimate";
const char* VehicleEstimatorStatusFactGroup::_goodHorizPosAbsEstimateFactName = "goodHorizPosAbsEstimate";
const char* VehicleEstimatorStatusFactGroup::_goodVertPosAbsEstimateFactName = "goodVertPosAbsEstimate";
const char* VehicleEstimatorStatusFactGroup::_goodVertPosAGLEstimateFactName = "goodVertPosAGLEstimate";
const char* VehicleEstimatorStatusFactGroup::_goodConstPosModeEstimateFactName = "goodConstPosModeEstimate";
const char* VehicleEstimatorStatusFactGroup::_goodPredHorizPosRelEstimateFactName = "goodPredHorizPosRelEstimate";
const char* VehicleEstimatorStatusFactGroup::_goodPredHorizPosAbsEstimateFactName = "goodPredHorizPosAbsEstimate";
const char* VehicleEstimatorStatusFactGroup::_gpsGlitchFactName = "gpsGlitch";
const char* VehicleEstimatorStatusFactGroup::_accelErrorFactName = "accelError";
const char* VehicleEstimatorStatusFactGroup::_velRatioFactName = "velRatio";
const char* VehicleEstimatorStatusFactGroup::_horizPosRatioFactName = "horizPosRatio";
const char* VehicleEstimatorStatusFactGroup::_vertPosRatioFactName = "vertPosRatio";
const char* VehicleEstimatorStatusFactGroup::_magRatioFactName = "magRatio";
const char* VehicleEstimatorStatusFactGroup::_haglRatioFactName = "haglRatio";
const char* VehicleEstimatorStatusFactGroup::_tasRatioFactName = "tasRatio";
const char* VehicleEstimatorStatusFactGroup::_horizPosAccuracyFactName = "horizPosAccuracy";
const char* VehicleEstimatorStatusFactGroup::_vertPosAccuracyFactName = "vertPosAccuracy";
VehicleEstimatorStatusFactGroup::VehicleEstimatorStatusFactGroup(QObject* parent)
: FactGroup (500, ":/json/Vehicle/EstimatorStatusFactGroup.json", parent)
, _goodAttitudeEstimateFact (0, _goodAttitudeEstimateFactName, FactMetaData::valueTypeBool)
, _goodHorizVelEstimateFact (0, _goodHorizVelEstimateFactName, FactMetaData::valueTypeBool)
, _goodVertVelEstimateFact (0, _goodVertVelEstimateFactName, FactMetaData::valueTypeBool)
, _goodHorizPosRelEstimateFact (0, _goodHorizPosRelEstimateFactName, FactMetaData::valueTypeBool)
, _goodHorizPosAbsEstimateFact (0, _goodHorizPosAbsEstimateFactName, FactMetaData::valueTypeBool)
, _goodVertPosAbsEstimateFact (0, _goodVertPosAbsEstimateFactName, FactMetaData::valueTypeBool)
, _goodVertPosAGLEstimateFact (0, _goodVertPosAGLEstimateFactName, FactMetaData::valueTypeBool)
, _goodConstPosModeEstimateFact (0, _goodConstPosModeEstimateFactName, FactMetaData::valueTypeBool)
, _goodPredHorizPosRelEstimateFact (0, _goodPredHorizPosRelEstimateFactName, FactMetaData::valueTypeBool)
, _goodPredHorizPosAbsEstimateFact (0, _goodPredHorizPosAbsEstimateFactName, FactMetaData::valueTypeBool)
, _gpsGlitchFact (0, _gpsGlitchFactName, FactMetaData::valueTypeBool)
, _accelErrorFact (0, _accelErrorFactName, FactMetaData::valueTypeBool)
, _velRatioFact (0, _velRatioFactName, FactMetaData::valueTypeFloat)
, _horizPosRatioFact (0, _horizPosRatioFactName, FactMetaData::valueTypeFloat)
, _vertPosRatioFact (0, _vertPosRatioFactName, FactMetaData::valueTypeFloat)
, _magRatioFact (0, _magRatioFactName, FactMetaData::valueTypeFloat)
, _haglRatioFact (0, _haglRatioFactName, FactMetaData::valueTypeFloat)
, _tasRatioFact (0, _tasRatioFactName, FactMetaData::valueTypeFloat)
, _horizPosAccuracyFact (0, _horizPosAccuracyFactName, FactMetaData::valueTypeFloat)
, _vertPosAccuracyFact (0, _vertPosAccuracyFactName, FactMetaData::valueTypeFloat)
{
_addFact(&_goodAttitudeEstimateFact, _goodAttitudeEstimateFactName);
_addFact(&_goodHorizVelEstimateFact, _goodHorizVelEstimateFactName);
_addFact(&_goodVertVelEstimateFact, _goodVertVelEstimateFactName);
_addFact(&_goodHorizPosRelEstimateFact, _goodHorizPosRelEstimateFactName);
_addFact(&_goodHorizPosAbsEstimateFact, _goodHorizPosAbsEstimateFactName);
_addFact(&_goodVertPosAbsEstimateFact, _goodVertPosAbsEstimateFactName);
_addFact(&_goodVertPosAGLEstimateFact, _goodVertPosAGLEstimateFactName);
_addFact(&_goodConstPosModeEstimateFact, _goodConstPosModeEstimateFactName);
_addFact(&_goodPredHorizPosRelEstimateFact, _goodPredHorizPosRelEstimateFactName);
_addFact(&_goodPredHorizPosAbsEstimateFact, _goodPredHorizPosAbsEstimateFactName);
_addFact(&_gpsGlitchFact, _gpsGlitchFactName);
_addFact(&_accelErrorFact, _accelErrorFactName);
_addFact(&_velRatioFact, _velRatioFactName);
_addFact(&_horizPosRatioFact, _horizPosRatioFactName);
_addFact(&_vertPosRatioFact, _vertPosRatioFactName);
_addFact(&_magRatioFact, _magRatioFactName);
_addFact(&_haglRatioFact, _haglRatioFactName);
_addFact(&_tasRatioFact, _tasRatioFactName);
_addFact(&_horizPosAccuracyFact, _horizPosAccuracyFactName);
_addFact(&_vertPosAccuracyFact, _vertPosAccuracyFactName);
}
src/Vehicle/Vehicle.h
View file @ 70f899c6
......@@ -45,7 +45,7 @@ class VehicleDistanceSensorFactGroup : public FactGroup
Q_OBJECT
public:
VehicleDistanceSensorFactGroup(QObject* parent = NULL);
VehicleDistanceSensorFactGroup(QObject* parent = nullptr);
Q_PROPERTY(Fact* rotationNone READ rotationNone CONSTANT)
Q_PROPERTY(Fact* rotationYaw45 READ rotationYaw45 CONSTANT)
......@@ -106,7 +106,7 @@ class VehicleSetpointFactGroup : public FactGroup
Q_OBJECT
public:
VehicleSetpointFactGroup(QObject* parent = NULL);
VehicleSetpointFactGroup(QObject* parent = nullptr);
Q_PROPERTY(Fact* roll READ roll CONSTANT)
Q_PROPERTY(Fact* pitch READ pitch CONSTANT)
......@@ -143,7 +143,7 @@ class VehicleVibrationFactGroup : public FactGroup
Q_OBJECT
public:
VehicleVibrationFactGroup(QObject* parent = NULL);
VehicleVibrationFactGroup(QObject* parent = nullptr);
Q_PROPERTY(Fact* xAxis READ xAxis CONSTANT)
Q_PROPERTY(Fact* yAxis READ yAxis CONSTANT)
......@@ -180,7 +180,7 @@ class VehicleWindFactGroup : public FactGroup
Q_OBJECT
public:
VehicleWindFactGroup(QObject* parent = NULL);
VehicleWindFactGroup(QObject* parent = nullptr);
Q_PROPERTY(Fact* direction READ direction CONSTANT)
Q_PROPERTY(Fact* speed READ speed CONSTANT)
......@@ -205,7 +205,7 @@ class VehicleGPSFactGroup : public FactGroup
Q_OBJECT
public:
VehicleGPSFactGroup(QObject* parent = NULL);
VehicleGPSFactGroup(QObject* parent = nullptr);
Q_PROPERTY(Fact* lat READ lat CONSTANT)
Q_PROPERTY(Fact* lon READ lon CONSTANT)
......@@ -246,7 +246,7 @@ class VehicleBatteryFactGroup : public FactGroup
Q_OBJECT
public:
VehicleBatteryFactGroup(QObject* parent = NULL);
VehicleBatteryFactGroup(QObject* parent = nullptr);
Q_PROPERTY(Fact* voltage READ voltage CONSTANT)
Q_PROPERTY(Fact* percentRemaining READ percentRemaining CONSTANT)
......@@ -305,7 +305,7 @@ class VehicleTemperatureFactGroup : public FactGroup
Q_OBJECT
public:
VehicleTemperatureFactGroup(QObject* parent = NULL);
VehicleTemperatureFactGroup(QObject* parent = nullptr);
Q_PROPERTY(Fact* temperature1 READ temperature1 CONSTANT)
Q_PROPERTY(Fact* temperature2 READ temperature2 CONSTANT)
......@@ -334,7 +334,7 @@ class VehicleClockFactGroup : public FactGroup
Q_OBJECT
public:
VehicleClockFactGroup(QObject* parent = NULL);
VehicleClockFactGroup(QObject* parent = nullptr);
Q_PROPERTY(Fact* currentTime READ currentTime CONSTANT)
Q_PROPERTY(Fact* currentDate READ currentDate CONSTANT)
......@@ -355,6 +355,131 @@ private:
Fact _currentDateFact;
};
class VehicleEstimatorStatusFactGroup : public FactGroup
{
Q_OBJECT
public:
VehicleEstimatorStatusFactGroup(QObject* parent = nullptr);
Q_PROPERTY(Fact* goodAttitudeEstimate READ goodAttitudeEstimate CONSTANT)
Q_PROPERTY(Fact* goodHorizVelEstimate READ goodHorizVelEstimate CONSTANT)
Q_PROPERTY(Fact* goodVertVelEstimate READ goodVertVelEstimate CONSTANT)
Q_PROPERTY(Fact* goodHorizPosRelEstimate READ goodHorizPosRelEstimate CONSTANT)
Q_PROPERTY(Fact* goodHorizPosAbsEstimate READ goodHorizPosAbsEstimate CONSTANT)
Q_PROPERTY(Fact* goodVertPosAbsEstimate READ goodVertPosAbsEstimate CONSTANT)
Q_PROPERTY(Fact* goodVertPosAGLEstimate READ goodVertPosAGLEstimate CONSTANT)
Q_PROPERTY(Fact* goodConstPosModeEstimate READ goodConstPosModeEstimate CONSTANT)
Q_PROPERTY(Fact* goodPredHorizPosRelEstimate READ goodPredHorizPosRelEstimate CONSTANT)
Q_PROPERTY(Fact* goodPredHorizPosAbsEstimate READ goodPredHorizPosAbsEstimate CONSTANT)
Q_PROPERTY(Fact* gpsGlitch READ gpsGlitch CONSTANT)
Q_PROPERTY(Fact* accelError READ accelError CONSTANT)
Q_PROPERTY(Fact* velRatio READ velRatio CONSTANT)
Q_PROPERTY(Fact* horizPosRatio READ horizPosRatio CONSTANT)
Q_PROPERTY(Fact* vertPosRatio READ vertPosRatio CONSTANT)
Q_PROPERTY(Fact* magRatio READ magRatio CONSTANT)
Q_PROPERTY(Fact* haglRatio READ haglRatio CONSTANT)
Q_PROPERTY(Fact* tasRatio READ tasRatio CONSTANT)
Q_PROPERTY(Fact* horizPosAccuracy READ horizPosAccuracy CONSTANT)
Q_PROPERTY(Fact* vertPosAccuracy READ vertPosAccuracy CONSTANT)
Fact* goodAttitudeEstimate (void) { return &_goodAttitudeEstimateFact; }
Fact* goodHorizVelEstimate (void) { return &_goodHorizVelEstimateFact; }
Fact* goodVertVelEstimate (void) { return &_goodVertVelEstimateFact; }
Fact* goodHorizPosRelEstimate (void) { return &_goodHorizPosRelEstimateFact; }
Fact* goodHorizPosAbsEstimate (void) { return &_goodHorizPosAbsEstimateFact; }
Fact* goodVertPosAbsEstimate (void) { return &_goodVertPosAbsEstimateFact; }
Fact* goodVertPosAGLEstimate (void) { return &_goodVertPosAGLEstimateFact; }
Fact* goodConstPosModeEstimate (void) { return &_goodConstPosModeEstimateFact; }
Fact* goodPredHorizPosRelEstimate (void) { return &_goodPredHorizPosRelEstimateFact; }
Fact* goodPredHorizPosAbsEstimate (void) { return &_goodPredHorizPosAbsEstimateFact; }
Fact* gpsGlitch (void) { return &_gpsGlitchFact; }
Fact* accelError (void) { return &_accelErrorFact; }
Fact* velRatio (void) { return &_velRatioFact; }
Fact* horizPosRatio (void) { return &_horizPosRatioFact; }
Fact* vertPosRatio (void) { return &_vertPosRatioFact; }
Fact* magRatio (void) { return &_magRatioFact; }
Fact* haglRatio (void) { return &_haglRatioFact; }
Fact* tasRatio (void) { return &_tasRatioFact; }
Fact* horizPosAccuracy (void) { return &_horizPosAccuracyFact; }
Fact* vertPosAccuracy (void) { return &_vertPosAccuracyFact; }
static const char* _goodAttitudeEstimateFactName;
static const char* _goodHorizVelEstimateFactName;
static const char* _goodVertVelEstimateFactName;
static const char* _goodHorizPosRelEstimateFactName;
static const char* _goodHorizPosAbsEstimateFactName;
static const char* _goodVertPosAbsEstimateFactName;
static const char* _goodVertPosAGLEstimateFactName;
static const char* _goodConstPosModeEstimateFactName;
static const char* _goodPredHorizPosRelEstimateFactName;
static const char* _goodPredHorizPosAbsEstimateFactName;
static const char* _gpsGlitchFactName;
static const char* _accelErrorFactName;
static const char* _velRatioFactName;
static const char* _horizPosRatioFactName;
static const char* _vertPosRatioFactName;
static const char* _magRatioFactName;
static const char* _haglRatioFactName;
static const char* _tasRatioFactName;
static const char* _horizPosAccuracyFactName;
static const char* _vertPosAccuracyFactName;
private:
Fact _goodAttitudeEstimateFact;
Fact _goodHorizVelEstimateFact;
Fact _goodVertVelEstimateFact;
Fact _goodHorizPosRelEstimateFact;
Fact _goodHorizPosAbsEstimateFact;
Fact _goodVertPosAbsEstimateFact;
Fact _goodVertPosAGLEstimateFact;
Fact _goodConstPosModeEstimateFact;
Fact _goodPredHorizPosRelEstimateFact;
Fact _goodPredHorizPosAbsEstimateFact;
Fact _gpsGlitchFact;
Fact _accelErrorFact;
Fact _velRatioFact;
Fact _horizPosRatioFact;
Fact _vertPosRatioFact;
Fact _magRatioFact;
Fact _haglRatioFact;
Fact _tasRatioFact;
Fact _horizPosAccuracyFact;
Fact _vertPosAccuracyFact;
#if 0
typedef enum ESTIMATOR_STATUS_FLAGS
{
ESTIMATOR_ATTITUDE=1, /* True if the attitude estimate is good | */
ESTIMATOR_VELOCITY_HORIZ=2, /* True if the horizontal velocity estimate is good | */
ESTIMATOR_VELOCITY_VERT=4, /* True if the vertical velocity estimate is good | */
ESTIMATOR_POS_HORIZ_REL=8, /* True if the horizontal position (relative) estimate is good | */
ESTIMATOR_POS_HORIZ_ABS=16, /* True if the horizontal position (absolute) estimate is good | */
ESTIMATOR_POS_VERT_ABS=32, /* True if the vertical position (absolute) estimate is good | */
ESTIMATOR_POS_VERT_AGL=64, /* True if the vertical position (above ground) estimate is good | */
ESTIMATOR_CONST_POS_MODE=128, /* True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow) | */
ESTIMATOR_PRED_POS_HORIZ_REL=256, /* True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate | */
ESTIMATOR_PRED_POS_HORIZ_ABS=512, /* True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate | */
ESTIMATOR_GPS_GLITCH=1024, /* True if the EKF has detected a GPS glitch | */
ESTIMATOR_ACCEL_ERROR=2048, /* True if the EKF has detected bad accelerometer data | */
typedef struct __mavlink_estimator_status_t {
uint64_t time_usec; /*< Timestamp (micros since boot or Unix epoch)*/
float vel_ratio; /*< Velocity innovation test ratio*/
float pos_horiz_ratio; /*< Horizontal position innovation test ratio*/
float pos_vert_ratio; /*< Vertical position innovation test ratio*/
float mag_ratio; /*< Magnetometer innovation test ratio*/
float hagl_ratio; /*< Height above terrain innovation test ratio*/
float tas_ratio; /*< True airspeed innovation test ratio*/
float pos_horiz_accuracy; /*< Horizontal position 1-STD accuracy relative to the EKF local origin (m)*/
float pos_vert_accuracy; /*< Vertical position 1-STD accuracy relative to the EKF local origin (m)*/
uint16_t flags; /*< Integer bitmask indicating which EKF outputs are valid. See definition for ESTIMATOR_STATUS_FLAGS.*/
}) mavlink_estimator_status_t;
#endif
};
class Vehicle : public FactGroup
{
Q_OBJECT
......@@ -526,14 +651,15 @@ public:
Q_PROPERTY(Fact* distanceToHome READ distanceToHome CONSTANT)
Q_PROPERTY(Fact* hobbs READ hobbs CONSTANT)
Q_PROPERTY(FactGroup* gps READ gpsFactGroup CONSTANT)
Q_PROPERTY(FactGroup* battery READ battery1FactGroup CONSTANT)
Q_PROPERTY(FactGroup* battery2 READ battery2FactGroup CONSTANT)
Q_PROPERTY(FactGroup* wind READ windFactGroup CONSTANT)
Q_PROPERTY(FactGroup* vibration READ vibrationFactGroup CONSTANT)
Q_PROPERTY(FactGroup* temperature READ temperatureFactGroup CONSTANT)
Q_PROPERTY(FactGroup* clock READ clockFactGroup CONSTANT)
Q_PROPERTY(FactGroup* setpoint READ setpointFactGroup CONSTANT)
Q_PROPERTY(FactGroup* gps READ gpsFactGroup CONSTANT)
Q_PROPERTY(FactGroup* battery READ battery1FactGroup CONSTANT)
Q_PROPERTY(FactGroup* battery2 READ battery2FactGroup CONSTANT)
Q_PROPERTY(FactGroup* wind READ windFactGroup CONSTANT)
Q_PROPERTY(FactGroup* vibration READ vibrationFactGroup CONSTANT)
Q_PROPERTY(FactGroup* temperature READ temperatureFactGroup CONSTANT)
Q_PROPERTY(FactGroup* clock READ clockFactGroup CONSTANT)
Q_PROPERTY(FactGroup* setpoint READ setpointFactGroup CONSTANT)
Q_PROPERTY(FactGroup* estimatorStatus READ estimatorStatusFactGroup CONSTANT)
Q_PROPERTY(int firmwareMajorVersion READ firmwareMajorVersion NOTIFY firmwareVersionChanged)
Q_PROPERTY(int firmwareMinorVersion READ firmwareMinorVersion NOTIFY firmwareVersionChanged)
......@@ -823,6 +949,7 @@ public:
FactGroup* clockFactGroup (void) { return &_clockFactGroup; }
FactGroup* setpointFactGroup (void) { return &_setpointFactGroup; }
FactGroup* distanceSensorFactGroup (void) { return &_distanceSensorFactGroup; }
FactGroup* estimatorStatusFactGroup (void) { return &_estimatorStatusFactGroup; }
void setConnectionLostEnabled(bool connectionLostEnabled);
......@@ -1088,6 +1215,7 @@ private:
void _handleAttitudeQuaternion(mavlink_message_t& message);
void _handleAttitudeTarget(mavlink_message_t& message);
void _handleDistanceSensor(mavlink_message_t& message);
void _handleEstimatorStatus(mavlink_message_t& message);
// ArduPilot dialect messages
#if !defined(NO_ARDUPILOT_DIALECT)
void _handleCameraFeedback(const mavlink_message_t& message);
......@@ -1304,6 +1432,7 @@ private:
VehicleClockFactGroup _clockFactGroup;
VehicleSetpointFactGroup _setpointFactGroup;
VehicleDistanceSensorFactGroup _distanceSensorFactGroup;
VehicleEstimatorStatusFactGroup _estimatorStatusFactGroup;
static const char* _rollFactName;
static const char* _pitchFactName;
......@@ -1329,6 +1458,7 @@ private:
static const char* _temperatureFactGroupName;
static const char* _clockFactGroupName;
static const char* _distanceSensorFactGroupName;
static const char* _estimatorStatusFactGroupName;
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