From 70f899c6ca6c0dd1ff7c8c8d83ba1e5a48e54c9c Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 23 Aug 2018 20:46:57 -0700 Subject: [PATCH] Add FactGroup for ESTIMATOR_STATUS values --- ChangeLog.md | 4 +- qgroundcontrol.qrc | 1 + src/Vehicle/EstimatorStatusFactGroup.json | 130 ++++++++++++++ src/Vehicle/Vehicle.cc | 206 ++++++++++++++++++---- src/Vehicle/Vehicle.h | 162 +++++++++++++++-- 5 files changed, 447 insertions(+), 56 deletions(-) create mode 100644 src/Vehicle/EstimatorStatusFactGroup.json diff --git a/ChangeLog.md b/ChangeLog.md index d5ad9e231..df99792ec 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 786860205..ab82893d4 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 000000000..6a1d3cea0 --- /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 d30332de3..f9ea0b742 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 252e23036..d261171f3 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; -- 2.22.0