Unverified Commit c7696bc8 authored by Don Gagne's avatar Don Gagne Committed by GitHub

Merge pull request #9020 from DonLakeFlyer/VehicleFactGroupRefactor

Vehicle fact group refactor
parents e5de3fdb 3c13e0ec
......@@ -706,6 +706,14 @@ HEADERS += \
src/Vehicle/Vehicle.h \
src/Vehicle/VehicleObjectAvoidance.h \
src/Vehicle/VehicleBatteryFactGroup.h \
src/Vehicle/VehicleClockFactGroup.h \
src/Vehicle/VehicleDistanceSensorFactGroup.h \
src/Vehicle/VehicleEstimatorStatusFactGroup.h \
src/Vehicle/VehicleGPSFactGroup.h \
src/Vehicle/VehicleSetpointFactGroup.h \
src/Vehicle/VehicleTemperatureFactGroup.h \
src/Vehicle/VehicleVibrationFactGroup.h \
src/Vehicle/VehicleWindFactGroup.h \
src/VehicleSetup/JoystickConfigController.h \
src/comm/LinkConfiguration.h \
src/comm/LinkInterface.h \
......@@ -926,6 +934,14 @@ SOURCES += \
src/Vehicle/Vehicle.cc \
src/Vehicle/VehicleObjectAvoidance.cc \
src/Vehicle/VehicleBatteryFactGroup.cc \
src/Vehicle/VehicleClockFactGroup.cc \
src/Vehicle/VehicleDistanceSensorFactGroup.cc \
src/Vehicle/VehicleEstimatorStatusFactGroup.cc \
src/Vehicle/VehicleGPSFactGroup.cc \
src/Vehicle/VehicleSetpointFactGroup.cc \
src/Vehicle/VehicleTemperatureFactGroup.cc \
src/Vehicle/VehicleVibrationFactGroup.cc \
src/Vehicle/VehicleWindFactGroup.cc \
src/VehicleSetup/JoystickConfigController.cc \
src/comm/LinkConfiguration.cc \
src/comm/LinkInterface.cc \
......
This diff is collapsed.
This diff is collapsed.
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#include "VehicleClockFactGroup.h"
#include "Vehicle.h"
const char* VehicleClockFactGroup::_currentTimeFactName = "currentTime";
const char* VehicleClockFactGroup::_currentDateFactName = "currentDate";
VehicleClockFactGroup::VehicleClockFactGroup(QObject* parent)
: FactGroup(1000, ":/json/Vehicle/ClockFact.json", parent)
, _currentTimeFact (0, _currentTimeFactName, FactMetaData::valueTypeString)
, _currentDateFact (0, _currentDateFactName, FactMetaData::valueTypeString)
{
_addFact(&_currentTimeFact, _currentTimeFactName);
_addFact(&_currentDateFact, _currentDateFactName);
// Start out as not available "--.--"
_currentTimeFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
_currentDateFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
}
void VehicleClockFactGroup::_updateAllValues()
{
_currentTimeFact.setRawValue(QTime::currentTime().toString());
_currentDateFact.setRawValue(QDateTime::currentDateTime().toString(QLocale::system().dateFormat(QLocale::ShortFormat)));
FactGroup::_updateAllValues();
}
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#pragma once
#include "FactGroup.h"
#include "QGCMAVLink.h"
class Vehicle;
class VehicleClockFactGroup : public FactGroup
{
Q_OBJECT
public:
VehicleClockFactGroup(QObject* parent = nullptr);
Q_PROPERTY(Fact* currentTime READ currentTime CONSTANT)
Q_PROPERTY(Fact* currentDate READ currentDate CONSTANT)
Fact* currentTime () { return &_currentTimeFact; }
Fact* currentDate () { return &_currentDateFact; }
static const char* _currentTimeFactName;
static const char* _currentDateFactName;
static const char* _settingsGroup;
private slots:
void _updateAllValues() override;
private:
Fact _currentTimeFact;
Fact _currentDateFact;
};
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#include "VehicleDistanceSensorFactGroup.h"
#include "Vehicle.h"
const char* VehicleDistanceSensorFactGroup::_rotationNoneFactName = "rotationNone";
const char* VehicleDistanceSensorFactGroup::_rotationYaw45FactName = "rotationYaw45";
const char* VehicleDistanceSensorFactGroup::_rotationYaw90FactName = "rotationYaw90";
const char* VehicleDistanceSensorFactGroup::_rotationYaw135FactName = "rotationYaw135";
const char* VehicleDistanceSensorFactGroup::_rotationYaw180FactName = "rotationYaw180";
const char* VehicleDistanceSensorFactGroup::_rotationYaw225FactName = "rotationYaw225";
const char* VehicleDistanceSensorFactGroup::_rotationYaw270FactName = "rotationYaw270";
const char* VehicleDistanceSensorFactGroup::_rotationYaw315FactName = "rotationYaw315";
const char* VehicleDistanceSensorFactGroup::_rotationPitch90FactName = "rotationPitch90";
const char* VehicleDistanceSensorFactGroup::_rotationPitch270FactName = "rotationPitch270";
const char* VehicleDistanceSensorFactGroup::_maxDistanceFactName = "maxDistance";
VehicleDistanceSensorFactGroup::VehicleDistanceSensorFactGroup(QObject* parent)
: FactGroup (1000, ":/json/Vehicle/DistanceSensorFact.json", parent)
, _rotationNoneFact (0, _rotationNoneFactName, FactMetaData::valueTypeDouble)
, _rotationYaw45Fact (0, _rotationYaw45FactName, FactMetaData::valueTypeDouble)
, _rotationYaw90Fact (0, _rotationYaw90FactName, FactMetaData::valueTypeDouble)
, _rotationYaw135Fact (0, _rotationYaw135FactName, FactMetaData::valueTypeDouble)
, _rotationYaw180Fact (0, _rotationYaw180FactName, FactMetaData::valueTypeDouble)
, _rotationYaw225Fact (0, _rotationYaw225FactName, FactMetaData::valueTypeDouble)
, _rotationYaw270Fact (0, _rotationYaw270FactName, FactMetaData::valueTypeDouble)
, _rotationYaw315Fact (0, _rotationYaw315FactName, FactMetaData::valueTypeDouble)
, _rotationPitch90Fact (0, _rotationPitch90FactName, FactMetaData::valueTypeDouble)
, _rotationPitch270Fact (0, _rotationPitch270FactName, FactMetaData::valueTypeDouble)
, _maxDistanceFact (0, _maxDistanceFactName, FactMetaData::valueTypeDouble)
{
_addFact(&_rotationNoneFact, _rotationNoneFactName);
_addFact(&_rotationYaw45Fact, _rotationYaw45FactName);
_addFact(&_rotationYaw90Fact, _rotationYaw90FactName);
_addFact(&_rotationYaw135Fact, _rotationYaw135FactName);
_addFact(&_rotationYaw180Fact, _rotationYaw180FactName);
_addFact(&_rotationYaw225Fact, _rotationYaw225FactName);
_addFact(&_rotationYaw270Fact, _rotationYaw270FactName);
_addFact(&_rotationYaw315Fact, _rotationYaw315FactName);
_addFact(&_rotationPitch90Fact, _rotationPitch90FactName);
_addFact(&_rotationPitch270Fact, _rotationPitch270FactName);
_addFact(&_maxDistanceFact, _maxDistanceFactName);
// Start out as not available "--.--"
_rotationNoneFact.setRawValue(qQNaN());
_rotationYaw45Fact.setRawValue(qQNaN());
_rotationYaw135Fact.setRawValue(qQNaN());
_rotationYaw90Fact.setRawValue(qQNaN());
_rotationYaw180Fact.setRawValue(qQNaN());
_rotationYaw225Fact.setRawValue(qQNaN());
_rotationYaw270Fact.setRawValue(qQNaN());
_rotationPitch90Fact.setRawValue(qQNaN());
_rotationPitch270Fact.setRawValue(qQNaN());
_maxDistanceFact.setRawValue(qQNaN());
}
void VehicleDistanceSensorFactGroup::handleMessage(Vehicle* /* vehicle */, mavlink_message_t& message)
{
if (message.msgid != MAVLINK_MSG_ID_DISTANCE_SENSOR) {
return;
}
mavlink_distance_sensor_t distanceSensor;
mavlink_msg_distance_sensor_decode(&message, &distanceSensor);
struct orientation2Fact_s {
MAV_SENSOR_ORIENTATION orientation;
Fact* fact;
};
orientation2Fact_s rgOrientation2Fact[] =
{
{ MAV_SENSOR_ROTATION_NONE, rotationNone() },
{ MAV_SENSOR_ROTATION_YAW_45, rotationYaw45() },
{ MAV_SENSOR_ROTATION_YAW_90, rotationYaw90() },
{ MAV_SENSOR_ROTATION_YAW_135, rotationYaw135() },
{ MAV_SENSOR_ROTATION_YAW_180, rotationYaw180() },
{ MAV_SENSOR_ROTATION_YAW_225, rotationYaw225() },
{ MAV_SENSOR_ROTATION_YAW_270, rotationYaw270() },
{ MAV_SENSOR_ROTATION_YAW_315, rotationYaw315() },
{ MAV_SENSOR_ROTATION_PITCH_90, rotationPitch90() },
{ MAV_SENSOR_ROTATION_PITCH_270, rotationPitch270() },
};
for (size_t i=0; i<sizeof(rgOrientation2Fact)/sizeof(rgOrientation2Fact[0]); i++) {
const orientation2Fact_s& orientation2Fact = rgOrientation2Fact[i];
if (orientation2Fact.orientation == distanceSensor.orientation) {
orientation2Fact.fact->setRawValue(distanceSensor.current_distance / 100.0); // cm to meters
}
maxDistance()->setRawValue(distanceSensor.max_distance/100.0);
}
}
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#pragma once
#include "FactGroup.h"
#include "QGCMAVLink.h"
class Vehicle;
class VehicleDistanceSensorFactGroup : public FactGroup
{
Q_OBJECT
public:
VehicleDistanceSensorFactGroup(QObject* parent = nullptr);
Q_PROPERTY(Fact* rotationNone READ rotationNone CONSTANT)
Q_PROPERTY(Fact* rotationYaw45 READ rotationYaw45 CONSTANT)
Q_PROPERTY(Fact* rotationYaw90 READ rotationYaw90 CONSTANT)
Q_PROPERTY(Fact* rotationYaw135 READ rotationYaw135 CONSTANT)
Q_PROPERTY(Fact* rotationYaw180 READ rotationYaw180 CONSTANT)
Q_PROPERTY(Fact* rotationYaw225 READ rotationYaw225 CONSTANT)
Q_PROPERTY(Fact* rotationYaw270 READ rotationYaw270 CONSTANT)
Q_PROPERTY(Fact* rotationYaw315 READ rotationYaw315 CONSTANT)
Q_PROPERTY(Fact* rotationPitch90 READ rotationPitch90 CONSTANT)
Q_PROPERTY(Fact* rotationPitch270 READ rotationPitch270 CONSTANT)
Q_PROPERTY(Fact* maxDistance READ maxDistance CONSTANT)
Fact* rotationNone () { return &_rotationNoneFact; }
Fact* rotationYaw45 () { return &_rotationYaw45Fact; }
Fact* rotationYaw90 () { return &_rotationYaw90Fact; }
Fact* rotationYaw135 () { return &_rotationYaw135Fact; }
Fact* rotationYaw180 () { return &_rotationYaw180Fact; }
Fact* rotationYaw225 () { return &_rotationYaw225Fact; }
Fact* rotationYaw270 () { return &_rotationYaw270Fact; }
Fact* rotationYaw315 () { return &_rotationYaw315Fact; }
Fact* rotationPitch90 () { return &_rotationPitch90Fact; }
Fact* rotationPitch270 () { return &_rotationPitch270Fact; }
Fact* maxDistance () { return &_maxDistanceFact; }
// Overrides from FactGroup
void handleMessage(Vehicle* vehicle, mavlink_message_t& message) override;
static const char* _rotationNoneFactName;
static const char* _rotationYaw45FactName;
static const char* _rotationYaw90FactName;
static const char* _rotationYaw135FactName;
static const char* _rotationYaw180FactName;
static const char* _rotationYaw225FactName;
static const char* _rotationYaw270FactName;
static const char* _rotationYaw315FactName;
static const char* _rotationPitch90FactName;
static const char* _rotationPitch270FactName;
static const char* _maxDistanceFactName;
private:
Fact _rotationNoneFact;
Fact _rotationYaw45Fact;
Fact _rotationYaw90Fact;
Fact _rotationYaw135Fact;
Fact _rotationYaw180Fact;
Fact _rotationYaw225Fact;
Fact _rotationYaw270Fact;
Fact _rotationYaw315Fact;
Fact _rotationPitch90Fact;
Fact _rotationPitch270Fact;
Fact _maxDistanceFact;
};
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#include "VehicleEstimatorStatusFactGroup.h"
#include "Vehicle.h"
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);
}
void VehicleEstimatorStatusFactGroup::handleMessage(Vehicle* /* vehicle */, mavlink_message_t& message)
{
if (message.msgid != MAVLINK_MSG_ID_ESTIMATOR_STATUS) {
return;
}
mavlink_estimator_status_t estimatorStatus;
mavlink_msg_estimator_status_decode(&message, &estimatorStatus);
goodAttitudeEstimate()->setRawValue (!!(estimatorStatus.flags & ESTIMATOR_ATTITUDE));
goodHorizVelEstimate()->setRawValue (!!(estimatorStatus.flags & ESTIMATOR_VELOCITY_HORIZ));
goodVertVelEstimate()->setRawValue (!!(estimatorStatus.flags & ESTIMATOR_VELOCITY_VERT));
goodHorizPosRelEstimate()->setRawValue (!!(estimatorStatus.flags & ESTIMATOR_POS_HORIZ_REL));
goodHorizPosAbsEstimate()->setRawValue (!!(estimatorStatus.flags & ESTIMATOR_POS_HORIZ_ABS));
goodVertPosAbsEstimate()->setRawValue (!!(estimatorStatus.flags & ESTIMATOR_POS_VERT_ABS));
goodVertPosAGLEstimate()->setRawValue (!!(estimatorStatus.flags & ESTIMATOR_POS_VERT_AGL));
goodConstPosModeEstimate()->setRawValue (!!(estimatorStatus.flags & ESTIMATOR_CONST_POS_MODE));
goodPredHorizPosRelEstimate()->setRawValue (!!(estimatorStatus.flags & ESTIMATOR_PRED_POS_HORIZ_REL));
goodPredHorizPosAbsEstimate()->setRawValue (!!(estimatorStatus.flags & ESTIMATOR_PRED_POS_HORIZ_ABS));
gpsGlitch()->setRawValue (estimatorStatus.flags & ESTIMATOR_GPS_GLITCH ? true : false);
accelError()->setRawValue (!!(estimatorStatus.flags & ESTIMATOR_ACCEL_ERROR));
velRatio()->setRawValue (estimatorStatus.vel_ratio);
horizPosRatio()->setRawValue (estimatorStatus.pos_horiz_ratio);
vertPosRatio()->setRawValue (estimatorStatus.pos_vert_ratio);
magRatio()->setRawValue (estimatorStatus.mag_ratio);
haglRatio()->setRawValue (estimatorStatus.hagl_ratio);
tasRatio()->setRawValue (estimatorStatus.tas_ratio);
horizPosAccuracy()->setRawValue (estimatorStatus.pos_horiz_accuracy);
vertPosAccuracy()->setRawValue (estimatorStatus.pos_vert_accuracy);
}
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#pragma once
#include "FactGroup.h"
#include "QGCMAVLink.h"
class Vehicle;
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 () { return &_goodAttitudeEstimateFact; }
Fact* goodHorizVelEstimate () { return &_goodHorizVelEstimateFact; }
Fact* goodVertVelEstimate () { return &_goodVertVelEstimateFact; }
Fact* goodHorizPosRelEstimate () { return &_goodHorizPosRelEstimateFact; }
Fact* goodHorizPosAbsEstimate () { return &_goodHorizPosAbsEstimateFact; }
Fact* goodVertPosAbsEstimate () { return &_goodVertPosAbsEstimateFact; }
Fact* goodVertPosAGLEstimate () { return &_goodVertPosAGLEstimateFact; }
Fact* goodConstPosModeEstimate () { return &_goodConstPosModeEstimateFact; }
Fact* goodPredHorizPosRelEstimate () { return &_goodPredHorizPosRelEstimateFact; }
Fact* goodPredHorizPosAbsEstimate () { return &_goodPredHorizPosAbsEstimateFact; }
Fact* gpsGlitch () { return &_gpsGlitchFact; }
Fact* accelError () { return &_accelErrorFact; }
Fact* velRatio () { return &_velRatioFact; }
Fact* horizPosRatio () { return &_horizPosRatioFact; }
Fact* vertPosRatio () { return &_vertPosRatioFact; }
Fact* magRatio () { return &_magRatioFact; }
Fact* haglRatio () { return &_haglRatioFact; }
Fact* tasRatio () { return &_tasRatioFact; }
Fact* horizPosAccuracy () { return &_horizPosAccuracyFact; }
Fact* vertPosAccuracy () { return &_vertPosAccuracyFact; }
// Overrides from FactGroup
void handleMessage(Vehicle* vehicle, mavlink_message_t& message) override;
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;
};
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#include "VehicleGPSFactGroup.h"
#include "Vehicle.h"
#include "QGCGeo.h"
const char* VehicleGPSFactGroup::_latFactName = "lat";
const char* VehicleGPSFactGroup::_lonFactName = "lon";
const char* VehicleGPSFactGroup::_mgrsFactName = "mgrs";
const char* VehicleGPSFactGroup::_hdopFactName = "hdop";
const char* VehicleGPSFactGroup::_vdopFactName = "vdop";
const char* VehicleGPSFactGroup::_courseOverGroundFactName = "courseOverGround";
const char* VehicleGPSFactGroup::_countFactName = "count";
const char* VehicleGPSFactGroup::_lockFactName = "lock";
VehicleGPSFactGroup::VehicleGPSFactGroup(QObject* parent)
: FactGroup(1000, ":/json/Vehicle/GPSFact.json", parent)
, _latFact (0, _latFactName, FactMetaData::valueTypeDouble)
, _lonFact (0, _lonFactName, FactMetaData::valueTypeDouble)
, _mgrsFact (0, _mgrsFactName, FactMetaData::valueTypeString)
, _hdopFact (0, _hdopFactName, FactMetaData::valueTypeDouble)
, _vdopFact (0, _vdopFactName, FactMetaData::valueTypeDouble)
, _courseOverGroundFact (0, _courseOverGroundFactName, FactMetaData::valueTypeDouble)
, _countFact (0, _countFactName, FactMetaData::valueTypeInt32)
, _lockFact (0, _lockFactName, FactMetaData::valueTypeInt32)
{
_addFact(&_latFact, _latFactName);
_addFact(&_lonFact, _lonFactName);
_addFact(&_mgrsFact, _mgrsFactName);
_addFact(&_hdopFact, _hdopFactName);
_addFact(&_vdopFact, _vdopFactName);
_addFact(&_courseOverGroundFact, _courseOverGroundFactName);
_addFact(&_lockFact, _lockFactName);
_addFact(&_countFact, _countFactName);
_latFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
_lonFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
_mgrsFact.setRawValue("");
_hdopFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
_vdopFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
_courseOverGroundFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
}
void VehicleGPSFactGroup::handleMessage(Vehicle* /* vehicle */, mavlink_message_t& message)
{
switch (message.msgid) {
case MAVLINK_MSG_ID_GPS_RAW_INT:
_handleGpsRawInt(message);
break;
case MAVLINK_MSG_ID_HIGH_LATENCY:
_handleHighLatency(message);
break;
case MAVLINK_MSG_ID_HIGH_LATENCY2:
_handleHighLatency2(message);
break;
default:
break;
}
}
void VehicleGPSFactGroup::_handleGpsRawInt(mavlink_message_t& message)
{
mavlink_gps_raw_int_t gpsRawInt;
mavlink_msg_gps_raw_int_decode(&message, &gpsRawInt);
lat()->setRawValue (gpsRawInt.lat * 1e-7);
lon()->setRawValue (gpsRawInt.lon * 1e-7);
mgrs()->setRawValue (convertGeoToMGRS(QGeoCoordinate(gpsRawInt.lat * 1e-7, gpsRawInt.lon * 1e-7)));
count()->setRawValue (gpsRawInt.satellites_visible == 255 ? 0 : gpsRawInt.satellites_visible);
hdop()->setRawValue (gpsRawInt.eph == UINT16_MAX ? qQNaN() : gpsRawInt.eph / 100.0);
vdop()->setRawValue (gpsRawInt.epv == UINT16_MAX ? qQNaN() : gpsRawInt.epv / 100.0);
courseOverGround()->setRawValue (gpsRawInt.cog == UINT16_MAX ? qQNaN() : gpsRawInt.cog / 100.0);
lock()->setRawValue (gpsRawInt.fix_type);
}
void VehicleGPSFactGroup::_handleHighLatency(mavlink_message_t& message)
{
mavlink_high_latency_t highLatency;
mavlink_msg_high_latency_decode(&message, &highLatency);
struct {
const double latitude;
const double longitude;
const double altitude;
} coordinate {
highLatency.latitude / (double)1E7,
highLatency.longitude / (double)1E7,
static_cast<double>(highLatency.altitude_amsl)
};
lat()->setRawValue (coordinate.latitude);
lon()->setRawValue (coordinate.longitude);
mgrs()->setRawValue (convertGeoToMGRS(QGeoCoordinate(coordinate.latitude, coordinate.longitude)));
count()->setRawValue(0);
}
void VehicleGPSFactGroup::_handleHighLatency2(mavlink_message_t& message)
{
mavlink_high_latency2_t highLatency2;
mavlink_msg_high_latency2_decode(&message, &highLatency2);
lat()->setRawValue (highLatency2.latitude * 1e-7);
lon()->setRawValue (highLatency2.longitude * 1e-7);
mgrs()->setRawValue (convertGeoToMGRS(QGeoCoordinate(highLatency2.latitude * 1e-7, highLatency2.longitude * 1e-7)));
count()->setRawValue(0);
hdop()->setRawValue (highLatency2.eph == UINT8_MAX ? qQNaN() : highLatency2.eph / 10.0);
vdop()->setRawValue (highLatency2.epv == UINT8_MAX ? qQNaN() : highLatency2.epv / 10.0);
}
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#pragma once
#include "FactGroup.h"
#include "QGCMAVLink.h"
class VehicleGPSFactGroup : public FactGroup
{
Q_OBJECT
public:
VehicleGPSFactGroup(QObject* parent = nullptr);
Q_PROPERTY(Fact* lat READ lat CONSTANT)
Q_PROPERTY(Fact* lon READ lon CONSTANT)
Q_PROPERTY(Fact* mgrs READ mgrs CONSTANT)
Q_PROPERTY(Fact* hdop READ hdop CONSTANT)
Q_PROPERTY(Fact* vdop READ vdop CONSTANT)
Q_PROPERTY(Fact* courseOverGround READ courseOverGround CONSTANT)
Q_PROPERTY(Fact* count READ count CONSTANT)
Q_PROPERTY(Fact* lock READ lock CONSTANT)
Fact* lat () { return &_latFact; }
Fact* lon () { return &_lonFact; }
Fact* mgrs () { return &_mgrsFact; }
Fact* hdop () { return &_hdopFact; }
Fact* vdop () { return &_vdopFact; }
Fact* courseOverGround () { return &_courseOverGroundFact; }
Fact* count () { return &_countFact; }
Fact* lock () { return &_lockFact; }
// Overrides from FactGroup
void handleMessage(Vehicle* vehicle, mavlink_message_t& message) override;
static const char* _latFactName;
static const char* _lonFactName;
static const char* _mgrsFactName;
static const char* _hdopFactName;
static const char* _vdopFactName;
static const char* _courseOverGroundFactName;
static const char* _countFactName;
static const char* _lockFactName;
private:
void _handleGpsRawInt (mavlink_message_t& message);
void _handleHighLatency (mavlink_message_t& message);
void _handleHighLatency2(mavlink_message_t& message);
Fact _latFact;