diff --git a/ChangeLog.md b/ChangeLog.md
index d5ad9e2310ed9986a321f23b471c60cff010f6f4..df99792ec298717246f80bbecc194003ccdebf4b 100644
--- a/ChangeLog.md
+++ b/ChangeLog.md
@@ -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
diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc
index 786860205bddaa69740aa13509d1b2de6b178114..ab82893d4ad6b5fdfbaa9e88123a8656b7413055 100644
--- a/qgroundcontrol.qrc
+++ b/qgroundcontrol.qrc
@@ -237,6 +237,7 @@
src/Vehicle/BatteryFact.json
src/Vehicle/ClockFact.json
src/Vehicle/DistanceSensorFact.json
+ src/Vehicle/EstimatorStatusFactGroup.json
src/Vehicle/GPSFact.json
src/Vehicle/GPSRTKFact.json
src/Vehicle/SetpointFact.json
diff --git a/src/Vehicle/EstimatorStatusFactGroup.json b/src/Vehicle/EstimatorStatusFactGroup.json
new file mode 100644
index 0000000000000000000000000000000000000000..6a1d3cea067d1dd3e0d60aeded756b1e37edf4c6
--- /dev/null
+++ b/src/Vehicle/EstimatorStatusFactGroup.json
@@ -0,0 +1,130 @@
+[
+{
+ "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
+}
+]
diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index d30332de3cf385af6523437852d0b465293d9d9c..f9ea0b742fea548fe6072fb3071083f25be34119 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@@ -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* 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::quiet_NaN());
_rotationPitch270Fact.setRawValue(std::numeric_limits::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);
+}
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
index 252e230366a632f7b9c9570091ee89f8a584b6f2..d261171f367e4d84f392a77dc0a7c1c1449e7cb1 100644
--- a/src/Vehicle/Vehicle.h
+++ b/src/Vehicle/Vehicle.h
@@ -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;