diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 2cd5bdb5869923d7b2b9d559caa85e65812af5ee..aba1506e15091a794ec3bcca275233035bed348c 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -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 \ diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 580598bc7af5fd2a5399b465fa2f87249337f2ed..dc56bd7afe64aa2ffb1ee70668f24990bdd75ff2 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -745,9 +745,6 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_SCALED_IMU3: emit mavlinkScaledImu3(message); break; - case MAVLINK_MSG_ID_VIBRATION: - _handleVibration(message); - break; case MAVLINK_MSG_ID_EXTENDED_SYS_STATE: _handleExtendedSysState(message); break; @@ -757,9 +754,6 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_COMMAND_LONG: _handleCommandLong(message); break; - case MAVLINK_MSG_ID_WIND_COV: - _handleWindCov(message); - break; case MAVLINK_MSG_ID_LOGGING_DATA: _handleMavlinkLoggingData(message); break; @@ -778,15 +772,6 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_VFR_HUD: _handleVfrHud(message); break; - case MAVLINK_MSG_ID_SCALED_PRESSURE: - _handleScaledPressure(message); - break; - case MAVLINK_MSG_ID_SCALED_PRESSURE2: - _handleScaledPressure2(message); - break; - case MAVLINK_MSG_ID_SCALED_PRESSURE3: - _handleScaledPressure3(message); - break; case MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED: _handleCameraImageCaptured(message); break; @@ -805,15 +790,6 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_ATTITUDE_QUATERNION: _handleAttitudeQuaternion(message); break; - case MAVLINK_MSG_ID_ATTITUDE_TARGET: - _handleAttitudeTarget(message); - break; - case MAVLINK_MSG_ID_DISTANCE_SENSOR: - _handleDistanceSensor(message); - break; - case MAVLINK_MSG_ID_ESTIMATOR_STATUS: - _handleEstimatorStatus(message); - break; case MAVLINK_MSG_ID_STATUSTEXT: _handleStatusText(message); break; @@ -846,9 +822,6 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_CAMERA_FEEDBACK: _handleCameraFeedback(message); break; - case MAVLINK_MSG_ID_WIND: - _handleWind(message); - break; #endif } @@ -1051,98 +1024,6 @@ void Vehicle::_handleVfrHud(mavlink_message_t& message) _throttlePctFact.setRawValue(static_cast(vfrHud.throttle)); } -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; - - mavlink_msg_distance_sensor_decode(&message, &distanceSensor); - - struct orientation2Fact_s { - MAV_SENSOR_ORIENTATION orientation; - Fact* fact; - }; - - orientation2Fact_s rgOrientation2Fact[] = - { - { MAV_SENSOR_ROTATION_NONE, _distanceSensorFactGroup.rotationNone() }, - { MAV_SENSOR_ROTATION_YAW_45, _distanceSensorFactGroup.rotationYaw45() }, - { MAV_SENSOR_ROTATION_YAW_90, _distanceSensorFactGroup.rotationYaw90() }, - { MAV_SENSOR_ROTATION_YAW_135, _distanceSensorFactGroup.rotationYaw135() }, - { MAV_SENSOR_ROTATION_YAW_180, _distanceSensorFactGroup.rotationYaw180() }, - { MAV_SENSOR_ROTATION_YAW_225, _distanceSensorFactGroup.rotationYaw225() }, - { MAV_SENSOR_ROTATION_YAW_270, _distanceSensorFactGroup.rotationYaw270() }, - { MAV_SENSOR_ROTATION_YAW_315, _distanceSensorFactGroup.rotationYaw315() }, - { MAV_SENSOR_ROTATION_PITCH_90, _distanceSensorFactGroup.rotationPitch90() }, - { MAV_SENSOR_ROTATION_PITCH_270, _distanceSensorFactGroup.rotationPitch270() }, - }; - - for (size_t i=0; isetRawValue(distanceSensor.current_distance / 100.0); // cm to meters - } - _distanceSensorFactGroup.maxDistance()->setRawValue(distanceSensor.max_distance/100.0); - } -} - // Ignore warnings from mavlink headers for both GCC/Clang and MSVC #ifdef __GNUC__ @@ -1161,24 +1042,6 @@ void Vehicle::_handleDistanceSensor(mavlink_message_t& message) #pragma warning(push, 0) #endif -void Vehicle::_handleAttitudeTarget(mavlink_message_t& message) -{ - mavlink_attitude_target_t attitudeTarget; - - mavlink_msg_attitude_target_decode(&message, &attitudeTarget); - - float roll, pitch, yaw; - mavlink_quaternion_to_euler(attitudeTarget.q, &roll, &pitch, &yaw); - - _setpointFactGroup.roll()->setRawValue(qRadiansToDegrees(roll)); - _setpointFactGroup.pitch()->setRawValue(qRadiansToDegrees(pitch)); - _setpointFactGroup.yaw()->setRawValue(qRadiansToDegrees(yaw)); - - _setpointFactGroup.rollRate()->setRawValue(qRadiansToDegrees(attitudeTarget.body_roll_rate)); - _setpointFactGroup.pitchRate()->setRawValue(qRadiansToDegrees(attitudeTarget.body_pitch_rate)); - _setpointFactGroup.yawRate()->setRawValue(qRadiansToDegrees(attitudeTarget.body_yaw_rate)); -} - void Vehicle::_handleAttitudeWorker(double rollRadians, double pitchRadians, double yawRadians) { double roll, pitch, yaw; @@ -1261,15 +1124,6 @@ void Vehicle::_handleGpsRawInt(mavlink_message_t& message) } } } - - _gpsFactGroup.lat()->setRawValue(gpsRawInt.lat * 1e-7); - _gpsFactGroup.lon()->setRawValue(gpsRawInt.lon * 1e-7); - _gpsFactGroup.mgrs()->setRawValue(convertGeoToMGRS(QGeoCoordinate(gpsRawInt.lat * 1e-7, gpsRawInt.lon * 1e-7))); - _gpsFactGroup.count()->setRawValue(gpsRawInt.satellites_visible == 255 ? 0 : gpsRawInt.satellites_visible); - _gpsFactGroup.hdop()->setRawValue(gpsRawInt.eph == UINT16_MAX ? qQNaN() : gpsRawInt.eph / 100.0); - _gpsFactGroup.vdop()->setRawValue(gpsRawInt.epv == UINT16_MAX ? qQNaN() : gpsRawInt.epv / 100.0); - _gpsFactGroup.courseOverGround()->setRawValue(gpsRawInt.cog == UINT16_MAX ? qQNaN() : gpsRawInt.cog / 100.0); - _gpsFactGroup.lock()->setRawValue(gpsRawInt.fix_type); } void Vehicle::_handleGlobalPositionInt(mavlink_message_t& message) @@ -1340,15 +1194,6 @@ void Vehicle::_handleHighLatency(mavlink_message_t& message) _headingFact.setRawValue((double)highLatency.heading * 2.0); _altitudeRelativeFact.setRawValue(qQNaN()); _altitudeAMSLFact.setRawValue(coordinate.altitude); - - _windFactGroup.speed()->setRawValue((double)highLatency.airspeed / 5.0); - - _temperatureFactGroup.temperature1()->setRawValue(highLatency.temperature_air); - - _gpsFactGroup.lat()->setRawValue(coordinate.latitude); - _gpsFactGroup.lon()->setRawValue(coordinate.longitude); - _gpsFactGroup.mgrs()->setRawValue(convertGeoToMGRS(QGeoCoordinate(coordinate.latitude, coordinate.longitude))); - _gpsFactGroup.count()->setRawValue(0); } void Vehicle::_handleHighLatency2(mavlink_message_t& message) @@ -1386,18 +1231,6 @@ void Vehicle::_handleHighLatency2(mavlink_message_t& message) _altitudeRelativeFact.setRawValue(qQNaN()); _altitudeAMSLFact.setRawValue(highLatency2.altitude); - _windFactGroup.direction()->setRawValue((double)highLatency2.wind_heading * 2.0); - _windFactGroup.speed()->setRawValue((double)highLatency2.windspeed / 5.0); - - _temperatureFactGroup.temperature1()->setRawValue(highLatency2.temperature_air); - - _gpsFactGroup.lat()->setRawValue(highLatency2.latitude * 1e-7); - _gpsFactGroup.lon()->setRawValue(highLatency2.longitude * 1e-7); - _gpsFactGroup.mgrs()->setRawValue(convertGeoToMGRS(QGeoCoordinate(highLatency2.latitude * 1e-7, highLatency2.longitude * 1e-7))); - _gpsFactGroup.count()->setRawValue(0); - _gpsFactGroup.hdop()->setRawValue(highLatency2.eph == UINT8_MAX ? qQNaN() : highLatency2.eph / 10.0); - _gpsFactGroup.vdop()->setRawValue(highLatency2.epv == UINT8_MAX ? qQNaN() : highLatency2.epv / 10.0); - struct failure2Sensor_s { HL_FAILURE_FLAG failureBit; MAV_SYS_STATUS_SENSOR sensorBit; @@ -1559,53 +1392,6 @@ void Vehicle::_handleExtendedSysState(mavlink_message_t& message) } } -void Vehicle::_handleVibration(mavlink_message_t& message) -{ - mavlink_vibration_t vibration; - mavlink_msg_vibration_decode(&message, &vibration); - - _vibrationFactGroup.xAxis()->setRawValue(vibration.vibration_x); - _vibrationFactGroup.yAxis()->setRawValue(vibration.vibration_y); - _vibrationFactGroup.zAxis()->setRawValue(vibration.vibration_z); - _vibrationFactGroup.clipCount1()->setRawValue(vibration.clipping_0); - _vibrationFactGroup.clipCount2()->setRawValue(vibration.clipping_1); - _vibrationFactGroup.clipCount3()->setRawValue(vibration.clipping_2); -} - -void Vehicle::_handleWindCov(mavlink_message_t& message) -{ - mavlink_wind_cov_t wind; - mavlink_msg_wind_cov_decode(&message, &wind); - - float direction = qRadiansToDegrees(qAtan2(wind.wind_y, wind.wind_x)); - float speed = qSqrt(qPow(wind.wind_x, 2) + qPow(wind.wind_y, 2)); - - if (direction < 0) { - direction += 360; - } - - _windFactGroup.direction()->setRawValue(direction); - _windFactGroup.speed()->setRawValue(speed); - _windFactGroup.verticalSpeed()->setRawValue(0); -} - -#if !defined(NO_ARDUPILOT_DIALECT) -void Vehicle::_handleWind(mavlink_message_t& message) -{ - mavlink_wind_t wind; - mavlink_msg_wind_decode(&message, &wind); - - // We don't want negative wind angles - float direction = wind.direction; - if (direction < 0) { - direction += 360; - } - _windFactGroup.direction()->setRawValue(direction); - _windFactGroup.speed()->setRawValue(wind.speed); - _windFactGroup.verticalSpeed()->setRawValue(wind.speed_z); -} -#endif - bool Vehicle::_apmArmingNotRequired() { QString armingRequireParam("ARMING_REQUIRE"); @@ -1941,24 +1727,6 @@ void Vehicle::_handleRCChannels(mavlink_message_t& message) #pragma warning(pop, 0) #endif -void Vehicle::_handleScaledPressure(mavlink_message_t& message) { - mavlink_scaled_pressure_t pressure; - mavlink_msg_scaled_pressure_decode(&message, &pressure); - _temperatureFactGroup.temperature1()->setRawValue(pressure.temperature / 100.0); -} - -void Vehicle::_handleScaledPressure2(mavlink_message_t& message) { - mavlink_scaled_pressure2_t pressure; - mavlink_msg_scaled_pressure2_decode(&message, &pressure); - _temperatureFactGroup.temperature2()->setRawValue(pressure.temperature / 100.0); -} - -void Vehicle::_handleScaledPressure3(mavlink_message_t& message) { - mavlink_scaled_pressure3_t pressure; - mavlink_msg_scaled_pressure3_decode(&message, &pressure); - _temperatureFactGroup.temperature3()->setRawValue(pressure.temperature / 100.0); -} - bool Vehicle::_containsLink(LinkInterface* link) { return _links.contains(link); @@ -3703,43 +3471,6 @@ void Vehicle::setVtolInFwdFlight(bool vtolInFwdFlight) } } -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::quiet_NaN()); - _lonFact.setRawValue(std::numeric_limits::quiet_NaN()); - _mgrsFact.setRawValue(""); - _hdopFact.setRawValue(std::numeric_limits::quiet_NaN()); - _vdopFact.setRawValue(std::numeric_limits::quiet_NaN()); - _courseOverGroundFact.setRawValue(std::numeric_limits::quiet_NaN()); -} - void Vehicle::startMavlinkLog() { sendMavCommand(_defaultComponentId, MAV_CMD_LOGGING_START, false /* showError */); @@ -4379,245 +4110,3 @@ void Vehicle::sendJoystickDataThreadSafe(float roll, float pitch, float yaw, flo buttons); sendMessageOnLinkThreadSafe(priorityLink(), message); } - -const char* VehicleWindFactGroup::_directionFactName = "direction"; -const char* VehicleWindFactGroup::_speedFactName = "speed"; -const char* VehicleWindFactGroup::_verticalSpeedFactName = "verticalSpeed"; - -VehicleWindFactGroup::VehicleWindFactGroup(QObject* parent) - : FactGroup(1000, ":/json/Vehicle/WindFact.json", parent) - , _directionFact (0, _directionFactName, FactMetaData::valueTypeDouble) - , _speedFact (0, _speedFactName, FactMetaData::valueTypeDouble) - , _verticalSpeedFact(0, _verticalSpeedFactName, FactMetaData::valueTypeDouble) -{ - _addFact(&_directionFact, _directionFactName); - _addFact(&_speedFact, _speedFactName); - _addFact(&_verticalSpeedFact, _verticalSpeedFactName); - - // Start out as not available "--.--" - _directionFact.setRawValue (qQNaN()); - _speedFact.setRawValue (qQNaN()); - _verticalSpeedFact.setRawValue (qQNaN()); -} - -const char* VehicleVibrationFactGroup::_xAxisFactName = "xAxis"; -const char* VehicleVibrationFactGroup::_yAxisFactName = "yAxis"; -const char* VehicleVibrationFactGroup::_zAxisFactName = "zAxis"; -const char* VehicleVibrationFactGroup::_clipCount1FactName = "clipCount1"; -const char* VehicleVibrationFactGroup::_clipCount2FactName = "clipCount2"; -const char* VehicleVibrationFactGroup::_clipCount3FactName = "clipCount3"; - -VehicleVibrationFactGroup::VehicleVibrationFactGroup(QObject* parent) - : FactGroup(1000, ":/json/Vehicle/VibrationFact.json", parent) - , _xAxisFact (0, _xAxisFactName, FactMetaData::valueTypeDouble) - , _yAxisFact (0, _yAxisFactName, FactMetaData::valueTypeDouble) - , _zAxisFact (0, _zAxisFactName, FactMetaData::valueTypeDouble) - , _clipCount1Fact (0, _clipCount1FactName, FactMetaData::valueTypeUint32) - , _clipCount2Fact (0, _clipCount2FactName, FactMetaData::valueTypeUint32) - , _clipCount3Fact (0, _clipCount3FactName, FactMetaData::valueTypeUint32) -{ - _addFact(&_xAxisFact, _xAxisFactName); - _addFact(&_yAxisFact, _yAxisFactName); - _addFact(&_zAxisFact, _zAxisFactName); - _addFact(&_clipCount1Fact, _clipCount1FactName); - _addFact(&_clipCount2Fact, _clipCount2FactName); - _addFact(&_clipCount3Fact, _clipCount3FactName); - - // Start out as not available "--.--" - _xAxisFact.setRawValue(qQNaN()); - _yAxisFact.setRawValue(qQNaN()); - _zAxisFact.setRawValue(qQNaN()); -} - -const char* VehicleTemperatureFactGroup::_temperature1FactName = "temperature1"; -const char* VehicleTemperatureFactGroup::_temperature2FactName = "temperature2"; -const char* VehicleTemperatureFactGroup::_temperature3FactName = "temperature3"; - -VehicleTemperatureFactGroup::VehicleTemperatureFactGroup(QObject* parent) - : FactGroup(1000, ":/json/Vehicle/TemperatureFact.json", parent) - , _temperature1Fact (0, _temperature1FactName, FactMetaData::valueTypeDouble) - , _temperature2Fact (0, _temperature2FactName, FactMetaData::valueTypeDouble) - , _temperature3Fact (0, _temperature3FactName, FactMetaData::valueTypeDouble) -{ - _addFact(&_temperature1Fact, _temperature1FactName); - _addFact(&_temperature2Fact, _temperature2FactName); - _addFact(&_temperature3Fact, _temperature3FactName); - - // Start out as not available "--.--" - _temperature1Fact.setRawValue (qQNaN()); - _temperature2Fact.setRawValue (qQNaN()); - _temperature3Fact.setRawValue (qQNaN()); -} - -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::quiet_NaN()); - _currentDateFact.setRawValue (std::numeric_limits::quiet_NaN()); -} - -void VehicleClockFactGroup::_updateAllValues() -{ - _currentTimeFact.setRawValue(QTime::currentTime().toString()); - _currentDateFact.setRawValue(QDateTime::currentDateTime().toString(QLocale::system().dateFormat(QLocale::ShortFormat))); - - FactGroup::_updateAllValues(); -} - -const char* VehicleSetpointFactGroup::_rollFactName = "roll"; -const char* VehicleSetpointFactGroup::_pitchFactName = "pitch"; -const char* VehicleSetpointFactGroup::_yawFactName = "yaw"; -const char* VehicleSetpointFactGroup::_rollRateFactName = "rollRate"; -const char* VehicleSetpointFactGroup::_pitchRateFactName = "pitchRate"; -const char* VehicleSetpointFactGroup::_yawRateFactName = "yawRate"; - -VehicleSetpointFactGroup::VehicleSetpointFactGroup(QObject* parent) - : FactGroup (1000, ":/json/Vehicle/SetpointFact.json", parent) - , _rollFact (0, _rollFactName, FactMetaData::valueTypeDouble) - , _pitchFact (0, _pitchFactName, FactMetaData::valueTypeDouble) - , _yawFact (0, _yawFactName, FactMetaData::valueTypeDouble) - , _rollRateFact (0, _rollRateFactName, FactMetaData::valueTypeDouble) - , _pitchRateFact(0, _pitchRateFactName, FactMetaData::valueTypeDouble) - , _yawRateFact (0, _yawRateFactName, FactMetaData::valueTypeDouble) -{ - _addFact(&_rollFact, _rollFactName); - _addFact(&_pitchFact, _pitchFactName); - _addFact(&_yawFact, _yawFactName); - _addFact(&_rollRateFact, _rollRateFactName); - _addFact(&_pitchRateFact, _pitchRateFactName); - _addFact(&_yawRateFact, _yawRateFactName); - - // Start out as not available "--.--" - _rollFact.setRawValue(qQNaN()); - _pitchFact.setRawValue(qQNaN()); - _yawFact.setRawValue(qQNaN()); - _rollRateFact.setRawValue(qQNaN()); - _pitchRateFact.setRawValue(qQNaN()); - _yawRateFact.setRawValue(qQNaN()); -} - -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()); -} - -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 add186dd9ac3c37fa4a64dcc500a6f165cc0ce12..2a95fa6bcc464d43b1f3fd6e43012f30d4c5c932 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -26,6 +26,14 @@ #include "QGCMapCircle.h" #include "TerrainFactGroup.h" #include "SysStatusSensorInfo.h" +#include "VehicleClockFactGroup.h" +#include "VehicleDistanceSensorFactGroup.h" +#include "VehicleWindFactGroup.h" +#include "VehicleGPSFactGroup.h" +#include "VehicleSetpointFactGroup.h" +#include "VehicleTemperatureFactGroup.h" +#include "VehicleVibrationFactGroup.h" +#include "VehicleEstimatorStatusFactGroup.h" class UAS; class UASInterface; @@ -57,386 +65,6 @@ Q_DECLARE_LOGGING_CATEGORY(VehicleLog) 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; } - - 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; -}; - -class VehicleSetpointFactGroup : public FactGroup -{ - Q_OBJECT - -public: - VehicleSetpointFactGroup(QObject* parent = nullptr); - - Q_PROPERTY(Fact* roll READ roll CONSTANT) - Q_PROPERTY(Fact* pitch READ pitch CONSTANT) - Q_PROPERTY(Fact* yaw READ yaw CONSTANT) - Q_PROPERTY(Fact* rollRate READ rollRate CONSTANT) - Q_PROPERTY(Fact* pitchRate READ pitchRate CONSTANT) - Q_PROPERTY(Fact* yawRate READ yawRate CONSTANT) - - Fact* roll () { return &_rollFact; } - Fact* pitch () { return &_pitchFact; } - Fact* yaw () { return &_yawFact; } - Fact* rollRate () { return &_rollRateFact; } - Fact* pitchRate () { return &_pitchRateFact; } - Fact* yawRate () { return &_yawRateFact; } - - static const char* _rollFactName; - static const char* _pitchFactName; - static const char* _yawFactName; - static const char* _rollRateFactName; - static const char* _pitchRateFactName; - static const char* _yawRateFactName; - -private: - Fact _rollFact; - Fact _pitchFact; - Fact _yawFact; - Fact _rollRateFact; - Fact _pitchRateFact; - Fact _yawRateFact; -}; - -class VehicleVibrationFactGroup : public FactGroup -{ - Q_OBJECT - -public: - VehicleVibrationFactGroup(QObject* parent = nullptr); - - Q_PROPERTY(Fact* xAxis READ xAxis CONSTANT) - Q_PROPERTY(Fact* yAxis READ yAxis CONSTANT) - Q_PROPERTY(Fact* zAxis READ zAxis CONSTANT) - Q_PROPERTY(Fact* clipCount1 READ clipCount1 CONSTANT) - Q_PROPERTY(Fact* clipCount2 READ clipCount2 CONSTANT) - Q_PROPERTY(Fact* clipCount3 READ clipCount3 CONSTANT) - - Fact* xAxis () { return &_xAxisFact; } - Fact* yAxis () { return &_yAxisFact; } - Fact* zAxis () { return &_zAxisFact; } - Fact* clipCount1 () { return &_clipCount1Fact; } - Fact* clipCount2 () { return &_clipCount2Fact; } - Fact* clipCount3 () { return &_clipCount3Fact; } - - static const char* _xAxisFactName; - static const char* _yAxisFactName; - static const char* _zAxisFactName; - static const char* _clipCount1FactName; - static const char* _clipCount2FactName; - static const char* _clipCount3FactName; - -private: - Fact _xAxisFact; - Fact _yAxisFact; - Fact _zAxisFact; - Fact _clipCount1Fact; - Fact _clipCount2Fact; - Fact _clipCount3Fact; -}; - -class VehicleWindFactGroup : public FactGroup -{ - Q_OBJECT - -public: - VehicleWindFactGroup(QObject* parent = nullptr); - - Q_PROPERTY(Fact* direction READ direction CONSTANT) - Q_PROPERTY(Fact* speed READ speed CONSTANT) - Q_PROPERTY(Fact* verticalSpeed READ verticalSpeed CONSTANT) - - Fact* direction () { return &_directionFact; } - Fact* speed () { return &_speedFact; } - Fact* verticalSpeed () { return &_verticalSpeedFact; } - - static const char* _directionFactName; - static const char* _speedFactName; - static const char* _verticalSpeedFactName; - -private: - Fact _directionFact; - Fact _speedFact; - Fact _verticalSpeedFact; -}; - -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; } - - 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: - Fact _latFact; - Fact _lonFact; - Fact _mgrsFact; - Fact _hdopFact; - Fact _vdopFact; - Fact _courseOverGroundFact; - Fact _countFact; - Fact _lockFact; -}; - -class VehicleTemperatureFactGroup : public FactGroup -{ - Q_OBJECT - -public: - VehicleTemperatureFactGroup(QObject* parent = nullptr); - - Q_PROPERTY(Fact* temperature1 READ temperature1 CONSTANT) - Q_PROPERTY(Fact* temperature2 READ temperature2 CONSTANT) - Q_PROPERTY(Fact* temperature3 READ temperature3 CONSTANT) - - Fact* temperature1 () { return &_temperature1Fact; } - Fact* temperature2 () { return &_temperature2Fact; } - Fact* temperature3 () { return &_temperature3Fact; } - - static const char* _temperature1FactName; - static const char* _temperature2FactName; - static const char* _temperature3FactName; - - static const char* _settingsGroup; - - static const double _temperatureUnavailable; - -private: - Fact _temperature1Fact; - Fact _temperature2Fact; - Fact _temperature3Fact; -}; - -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; -}; - -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; } - - 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 @@ -1303,8 +931,6 @@ private: void _handleRCChannels (mavlink_message_t& message); void _handleBatteryStatus (mavlink_message_t& message); void _handleSysStatus (mavlink_message_t& message); - void _handleWindCov (mavlink_message_t& message); - void _handleVibration (mavlink_message_t& message); void _handleExtendedSysState (mavlink_message_t& message); void _handleCommandAck (mavlink_message_t& message); void _handleCommandLong (mavlink_message_t& message); @@ -1312,17 +938,11 @@ private: void _handleGlobalPositionInt (mavlink_message_t& message); void _handleAltitude (mavlink_message_t& message); void _handleVfrHud (mavlink_message_t& message); - void _handleScaledPressure (mavlink_message_t& message); - void _handleScaledPressure2 (mavlink_message_t& message); - void _handleScaledPressure3 (mavlink_message_t& message); void _handleHighLatency (mavlink_message_t& message); void _handleHighLatency2 (mavlink_message_t& message); void _handleAttitudeWorker (double rollRadians, double pitchRadians, double yawRadians); void _handleAttitude (mavlink_message_t& message); void _handleAttitudeQuaternion (mavlink_message_t& message); - void _handleAttitudeTarget (mavlink_message_t& message); - void _handleDistanceSensor (mavlink_message_t& message); - void _handleEstimatorStatus (mavlink_message_t& message); void _handleStatusText (mavlink_message_t& message); void _handleOrbitExecutionStatus (const mavlink_message_t& message); void _handleMessageInterval (const mavlink_message_t& message); @@ -1331,7 +951,6 @@ private: // ArduPilot dialect messages #if !defined(NO_ARDUPILOT_DIALECT) void _handleCameraFeedback (const mavlink_message_t& message); - void _handleWind (mavlink_message_t& message); #endif void _handleCameraImageCaptured (const mavlink_message_t& message); void _handleADSBVehicle (const mavlink_message_t& message); diff --git a/src/Vehicle/VehicleClockFactGroup.cc b/src/Vehicle/VehicleClockFactGroup.cc new file mode 100644 index 0000000000000000000000000000000000000000..ab771f458175b345fa96042542fb95a6d4cfdd49 --- /dev/null +++ b/src/Vehicle/VehicleClockFactGroup.cc @@ -0,0 +1,35 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * 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::quiet_NaN()); + _currentDateFact.setRawValue(std::numeric_limits::quiet_NaN()); +} + +void VehicleClockFactGroup::_updateAllValues() +{ + _currentTimeFact.setRawValue(QTime::currentTime().toString()); + _currentDateFact.setRawValue(QDateTime::currentDateTime().toString(QLocale::system().dateFormat(QLocale::ShortFormat))); + + FactGroup::_updateAllValues(); +} diff --git a/src/Vehicle/VehicleClockFactGroup.h b/src/Vehicle/VehicleClockFactGroup.h new file mode 100644 index 0000000000000000000000000000000000000000..0b886b86d41c409a34103cab05b714982193e1d3 --- /dev/null +++ b/src/Vehicle/VehicleClockFactGroup.h @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * 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; +}; diff --git a/src/Vehicle/VehicleDistanceSensorFactGroup.cc b/src/Vehicle/VehicleDistanceSensorFactGroup.cc new file mode 100644 index 0000000000000000000000000000000000000000..c4c46f18fbec564d9b2ba6da02b240df045ae0f7 --- /dev/null +++ b/src/Vehicle/VehicleDistanceSensorFactGroup.cc @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * 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; isetRawValue(distanceSensor.current_distance / 100.0); // cm to meters + } + maxDistance()->setRawValue(distanceSensor.max_distance/100.0); + } +} diff --git a/src/Vehicle/VehicleDistanceSensorFactGroup.h b/src/Vehicle/VehicleDistanceSensorFactGroup.h new file mode 100644 index 0000000000000000000000000000000000000000..dfc219ed1adc690cee405d7d8a105da71eb8c05f --- /dev/null +++ b/src/Vehicle/VehicleDistanceSensorFactGroup.h @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * 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; +}; diff --git a/src/Vehicle/VehicleEstimatorStatusFactGroup.cc b/src/Vehicle/VehicleEstimatorStatusFactGroup.cc new file mode 100644 index 0000000000000000000000000000000000000000..516b085f79e59ae70743236e2c6b2d6801ee7fc9 --- /dev/null +++ b/src/Vehicle/VehicleEstimatorStatusFactGroup.cc @@ -0,0 +1,108 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * 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); +} diff --git a/src/Vehicle/VehicleEstimatorStatusFactGroup.h b/src/Vehicle/VehicleEstimatorStatusFactGroup.h new file mode 100644 index 0000000000000000000000000000000000000000..8ec15bc26c87ed5e8da07aa9925d04a2defa8dfd --- /dev/null +++ b/src/Vehicle/VehicleEstimatorStatusFactGroup.h @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * 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; +}; diff --git a/src/Vehicle/VehicleGPSFactGroup.cc b/src/Vehicle/VehicleGPSFactGroup.cc new file mode 100644 index 0000000000000000000000000000000000000000..e4762b6ad71bce3be94db53111acbbde403f4371 --- /dev/null +++ b/src/Vehicle/VehicleGPSFactGroup.cc @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * 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::quiet_NaN()); + _lonFact.setRawValue(std::numeric_limits::quiet_NaN()); + _mgrsFact.setRawValue(""); + _hdopFact.setRawValue(std::numeric_limits::quiet_NaN()); + _vdopFact.setRawValue(std::numeric_limits::quiet_NaN()); + _courseOverGroundFact.setRawValue(std::numeric_limits::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(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); +} diff --git a/src/Vehicle/VehicleGPSFactGroup.h b/src/Vehicle/VehicleGPSFactGroup.h new file mode 100644 index 0000000000000000000000000000000000000000..a08fd0927ab91fed70f24f82c0a85d36b756d6cb --- /dev/null +++ b/src/Vehicle/VehicleGPSFactGroup.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * 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; + Fact _lonFact; + Fact _mgrsFact; + Fact _hdopFact; + Fact _vdopFact; + Fact _courseOverGroundFact; + Fact _countFact; + Fact _lockFact; +}; diff --git a/src/Vehicle/VehicleSetpointFactGroup.cc b/src/Vehicle/VehicleSetpointFactGroup.cc new file mode 100644 index 0000000000000000000000000000000000000000..993f0e2d59ecc7531727d1d26cf0309023e7b7c4 --- /dev/null +++ b/src/Vehicle/VehicleSetpointFactGroup.cc @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "VehicleSetpointFactGroup.h" +#include "Vehicle.h" + +#include + +const char* VehicleSetpointFactGroup::_rollFactName = "roll"; +const char* VehicleSetpointFactGroup::_pitchFactName = "pitch"; +const char* VehicleSetpointFactGroup::_yawFactName = "yaw"; +const char* VehicleSetpointFactGroup::_rollRateFactName = "rollRate"; +const char* VehicleSetpointFactGroup::_pitchRateFactName = "pitchRate"; +const char* VehicleSetpointFactGroup::_yawRateFactName = "yawRate"; + +VehicleSetpointFactGroup::VehicleSetpointFactGroup(QObject* parent) + : FactGroup (1000, ":/json/Vehicle/SetpointFact.json", parent) + , _rollFact (0, _rollFactName, FactMetaData::valueTypeDouble) + , _pitchFact (0, _pitchFactName, FactMetaData::valueTypeDouble) + , _yawFact (0, _yawFactName, FactMetaData::valueTypeDouble) + , _rollRateFact (0, _rollRateFactName, FactMetaData::valueTypeDouble) + , _pitchRateFact(0, _pitchRateFactName, FactMetaData::valueTypeDouble) + , _yawRateFact (0, _yawRateFactName, FactMetaData::valueTypeDouble) +{ + _addFact(&_rollFact, _rollFactName); + _addFact(&_pitchFact, _pitchFactName); + _addFact(&_yawFact, _yawFactName); + _addFact(&_rollRateFact, _rollRateFactName); + _addFact(&_pitchRateFact, _pitchRateFactName); + _addFact(&_yawRateFact, _yawRateFactName); + + // Start out as not available "--.--" + _rollFact.setRawValue(qQNaN()); + _pitchFact.setRawValue(qQNaN()); + _yawFact.setRawValue(qQNaN()); + _rollRateFact.setRawValue(qQNaN()); + _pitchRateFact.setRawValue(qQNaN()); + _yawRateFact.setRawValue(qQNaN()); +} + +void VehicleSetpointFactGroup::handleMessage(Vehicle* /* vehicle */, mavlink_message_t& message) +{ + if (message.msgid != MAVLINK_MSG_ID_ATTITUDE_TARGET) { + return; + } + + mavlink_attitude_target_t attitudeTarget; + + mavlink_msg_attitude_target_decode(&message, &attitudeTarget); + + float roll, pitch, yaw; + mavlink_quaternion_to_euler(attitudeTarget.q, &roll, &pitch, &yaw); + + this->roll()->setRawValue (qRadiansToDegrees(roll)); + this->pitch()->setRawValue (qRadiansToDegrees(pitch)); + this->yaw()->setRawValue (qRadiansToDegrees(yaw)); + + rollRate()->setRawValue (qRadiansToDegrees(attitudeTarget.body_roll_rate)); + pitchRate()->setRawValue(qRadiansToDegrees(attitudeTarget.body_pitch_rate)); + yawRate()->setRawValue (qRadiansToDegrees(attitudeTarget.body_yaw_rate)); +} diff --git a/src/Vehicle/VehicleSetpointFactGroup.h b/src/Vehicle/VehicleSetpointFactGroup.h new file mode 100644 index 0000000000000000000000000000000000000000..4866f17659f77aaed5482ff3ff97105985109348 --- /dev/null +++ b/src/Vehicle/VehicleSetpointFactGroup.h @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * 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 VehicleSetpointFactGroup : public FactGroup +{ + Q_OBJECT + +public: + VehicleSetpointFactGroup(QObject* parent = nullptr); + + Q_PROPERTY(Fact* roll READ roll CONSTANT) + Q_PROPERTY(Fact* pitch READ pitch CONSTANT) + Q_PROPERTY(Fact* yaw READ yaw CONSTANT) + Q_PROPERTY(Fact* rollRate READ rollRate CONSTANT) + Q_PROPERTY(Fact* pitchRate READ pitchRate CONSTANT) + Q_PROPERTY(Fact* yawRate READ yawRate CONSTANT) + + Fact* roll () { return &_rollFact; } + Fact* pitch () { return &_pitchFact; } + Fact* yaw () { return &_yawFact; } + Fact* rollRate () { return &_rollRateFact; } + Fact* pitchRate () { return &_pitchRateFact; } + Fact* yawRate () { return &_yawRateFact; } + + // Overrides from FactGroup + void handleMessage(Vehicle* vehicle, mavlink_message_t& message) override; + + static const char* _rollFactName; + static const char* _pitchFactName; + static const char* _yawFactName; + static const char* _rollRateFactName; + static const char* _pitchRateFactName; + static const char* _yawRateFactName; + +private: + Fact _rollFact; + Fact _pitchFact; + Fact _yawFact; + Fact _rollRateFact; + Fact _pitchRateFact; + Fact _yawRateFact; +}; diff --git a/src/Vehicle/VehicleTemperatureFactGroup.cc b/src/Vehicle/VehicleTemperatureFactGroup.cc new file mode 100644 index 0000000000000000000000000000000000000000..96cef80261ea3037d8b1039421a093f34f5bb4f3 --- /dev/null +++ b/src/Vehicle/VehicleTemperatureFactGroup.cc @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "VehicleSetpointFactGroup.h" +#include "Vehicle.h" + +const char* VehicleTemperatureFactGroup::_temperature1FactName = "temperature1"; +const char* VehicleTemperatureFactGroup::_temperature2FactName = "temperature2"; +const char* VehicleTemperatureFactGroup::_temperature3FactName = "temperature3"; + +VehicleTemperatureFactGroup::VehicleTemperatureFactGroup(QObject* parent) + : FactGroup(1000, ":/json/Vehicle/TemperatureFact.json", parent) + , _temperature1Fact (0, _temperature1FactName, FactMetaData::valueTypeDouble) + , _temperature2Fact (0, _temperature2FactName, FactMetaData::valueTypeDouble) + , _temperature3Fact (0, _temperature3FactName, FactMetaData::valueTypeDouble) +{ + _addFact(&_temperature1Fact, _temperature1FactName); + _addFact(&_temperature2Fact, _temperature2FactName); + _addFact(&_temperature3Fact, _temperature3FactName); + + // Start out as not available "--.--" + _temperature1Fact.setRawValue (qQNaN()); + _temperature2Fact.setRawValue (qQNaN()); + _temperature3Fact.setRawValue (qQNaN()); +} + +void VehicleTemperatureFactGroup::handleMessage(Vehicle* /* vehicle */, mavlink_message_t& message) +{ + switch (message.msgid) { + case MAVLINK_MSG_ID_SCALED_PRESSURE: + _handleScaledPressure(message); + break; + case MAVLINK_MSG_ID_SCALED_PRESSURE2: + _handleScaledPressure2(message); + break; + case MAVLINK_MSG_ID_SCALED_PRESSURE3: + _handleScaledPressure3(message); + break; + case MAVLINK_MSG_ID_HIGH_LATENCY: + _handleHighLatency(message); + break; + case MAVLINK_MSG_ID_HIGH_LATENCY2: + _handleHighLatency2(message); + break; + default: + break; + } +} + +void VehicleTemperatureFactGroup::_handleHighLatency(mavlink_message_t& message) +{ + mavlink_high_latency_t highLatency; + mavlink_msg_high_latency_decode(&message, &highLatency); + temperature1()->setRawValue(highLatency.temperature_air); +} + +void VehicleTemperatureFactGroup::_handleHighLatency2(mavlink_message_t& message) +{ + mavlink_high_latency2_t highLatency2; + mavlink_msg_high_latency2_decode(&message, &highLatency2); + temperature1()->setRawValue(highLatency2.temperature_air); +} + +void VehicleTemperatureFactGroup::_handleScaledPressure(mavlink_message_t& message) +{ + mavlink_scaled_pressure_t pressure; + mavlink_msg_scaled_pressure_decode(&message, &pressure); + temperature1()->setRawValue(pressure.temperature / 100.0); +} + +void VehicleTemperatureFactGroup::_handleScaledPressure2(mavlink_message_t& message) +{ + mavlink_scaled_pressure2_t pressure; + mavlink_msg_scaled_pressure2_decode(&message, &pressure); + temperature2()->setRawValue(pressure.temperature / 100.0); +} + +void VehicleTemperatureFactGroup::_handleScaledPressure3(mavlink_message_t& message) +{ + mavlink_scaled_pressure3_t pressure; + mavlink_msg_scaled_pressure3_decode(&message, &pressure); + temperature3()->setRawValue(pressure.temperature / 100.0); +} diff --git a/src/Vehicle/VehicleTemperatureFactGroup.h b/src/Vehicle/VehicleTemperatureFactGroup.h new file mode 100644 index 0000000000000000000000000000000000000000..7fac1d83ab7515fae2ae7c2a2af0cefaa69c91d2 --- /dev/null +++ b/src/Vehicle/VehicleTemperatureFactGroup.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * 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 VehicleTemperatureFactGroup : public FactGroup +{ + Q_OBJECT + +public: + VehicleTemperatureFactGroup(QObject* parent = nullptr); + + Q_PROPERTY(Fact* temperature1 READ temperature1 CONSTANT) + Q_PROPERTY(Fact* temperature2 READ temperature2 CONSTANT) + Q_PROPERTY(Fact* temperature3 READ temperature3 CONSTANT) + + Fact* temperature1 () { return &_temperature1Fact; } + Fact* temperature2 () { return &_temperature2Fact; } + Fact* temperature3 () { return &_temperature3Fact; } + + // Overrides from FactGroup + void handleMessage(Vehicle* vehicle, mavlink_message_t& message) override; + + static const char* _temperature1FactName; + static const char* _temperature2FactName; + static const char* _temperature3FactName; + + static const char* _settingsGroup; + + static const double _temperatureUnavailable; + +private: + void _handleScaledPressure (mavlink_message_t& message); + void _handleScaledPressure2 (mavlink_message_t& message); + void _handleScaledPressure3 (mavlink_message_t& message); + void _handleHighLatency (mavlink_message_t& message); + void _handleHighLatency2 (mavlink_message_t& message); + + Fact _temperature1Fact; + Fact _temperature2Fact; + Fact _temperature3Fact; +}; diff --git a/src/Vehicle/VehicleVibrationFactGroup.cc b/src/Vehicle/VehicleVibrationFactGroup.cc new file mode 100644 index 0000000000000000000000000000000000000000..76cda6431eb8126db6eb427d520964f62a1a9658 --- /dev/null +++ b/src/Vehicle/VehicleVibrationFactGroup.cc @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "VehicleVibrationFactGroup.h" +#include "Vehicle.h" + +const char* VehicleVibrationFactGroup::_xAxisFactName = "xAxis"; +const char* VehicleVibrationFactGroup::_yAxisFactName = "yAxis"; +const char* VehicleVibrationFactGroup::_zAxisFactName = "zAxis"; +const char* VehicleVibrationFactGroup::_clipCount1FactName = "clipCount1"; +const char* VehicleVibrationFactGroup::_clipCount2FactName = "clipCount2"; +const char* VehicleVibrationFactGroup::_clipCount3FactName = "clipCount3"; + +VehicleVibrationFactGroup::VehicleVibrationFactGroup(QObject* parent) + : FactGroup (1000, ":/json/Vehicle/VibrationFact.json", parent) + , _xAxisFact (0, _xAxisFactName, FactMetaData::valueTypeDouble) + , _yAxisFact (0, _yAxisFactName, FactMetaData::valueTypeDouble) + , _zAxisFact (0, _zAxisFactName, FactMetaData::valueTypeDouble) + , _clipCount1Fact (0, _clipCount1FactName, FactMetaData::valueTypeUint32) + , _clipCount2Fact (0, _clipCount2FactName, FactMetaData::valueTypeUint32) + , _clipCount3Fact (0, _clipCount3FactName, FactMetaData::valueTypeUint32) +{ + _addFact(&_xAxisFact, _xAxisFactName); + _addFact(&_yAxisFact, _yAxisFactName); + _addFact(&_zAxisFact, _zAxisFactName); + _addFact(&_clipCount1Fact, _clipCount1FactName); + _addFact(&_clipCount2Fact, _clipCount2FactName); + _addFact(&_clipCount3Fact, _clipCount3FactName); + + // Start out as not available "--.--" + _xAxisFact.setRawValue(qQNaN()); + _yAxisFact.setRawValue(qQNaN()); + _zAxisFact.setRawValue(qQNaN()); +} + +void VehicleVibrationFactGroup::handleMessage(Vehicle* /* vehicle */, mavlink_message_t& message) +{ + if (message.msgid != MAVLINK_MSG_ID_VIBRATION) { + return; + } + + mavlink_vibration_t vibration; + mavlink_msg_vibration_decode(&message, &vibration); + + xAxis()->setRawValue(vibration.vibration_x); + yAxis()->setRawValue(vibration.vibration_y); + zAxis()->setRawValue(vibration.vibration_z); + clipCount1()->setRawValue(vibration.clipping_0); + clipCount2()->setRawValue(vibration.clipping_1); + clipCount3()->setRawValue(vibration.clipping_2); +} + diff --git a/src/Vehicle/VehicleVibrationFactGroup.h b/src/Vehicle/VehicleVibrationFactGroup.h new file mode 100644 index 0000000000000000000000000000000000000000..3cd7db1d1e9ea050aff6953dcc591610769a7105 --- /dev/null +++ b/src/Vehicle/VehicleVibrationFactGroup.h @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * 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 VehicleVibrationFactGroup : public FactGroup +{ + Q_OBJECT + +public: + VehicleVibrationFactGroup(QObject* parent = nullptr); + + Q_PROPERTY(Fact* xAxis READ xAxis CONSTANT) + Q_PROPERTY(Fact* yAxis READ yAxis CONSTANT) + Q_PROPERTY(Fact* zAxis READ zAxis CONSTANT) + Q_PROPERTY(Fact* clipCount1 READ clipCount1 CONSTANT) + Q_PROPERTY(Fact* clipCount2 READ clipCount2 CONSTANT) + Q_PROPERTY(Fact* clipCount3 READ clipCount3 CONSTANT) + + Fact* xAxis () { return &_xAxisFact; } + Fact* yAxis () { return &_yAxisFact; } + Fact* zAxis () { return &_zAxisFact; } + Fact* clipCount1 () { return &_clipCount1Fact; } + Fact* clipCount2 () { return &_clipCount2Fact; } + Fact* clipCount3 () { return &_clipCount3Fact; } + + // Overrides from FactGroup + void handleMessage(Vehicle* vehicle, mavlink_message_t& message) override; + + static const char* _xAxisFactName; + static const char* _yAxisFactName; + static const char* _zAxisFactName; + static const char* _clipCount1FactName; + static const char* _clipCount2FactName; + static const char* _clipCount3FactName; + +private: + Fact _xAxisFact; + Fact _yAxisFact; + Fact _zAxisFact; + Fact _clipCount1Fact; + Fact _clipCount2Fact; + Fact _clipCount3Fact; +}; diff --git a/src/Vehicle/VehicleWindFactGroup.cc b/src/Vehicle/VehicleWindFactGroup.cc new file mode 100644 index 0000000000000000000000000000000000000000..3c84c39a4b626c4c68325de8328d4805f49e90fe --- /dev/null +++ b/src/Vehicle/VehicleWindFactGroup.cc @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "VehicleWindFactGroup.h" +#include "Vehicle.h" + +#include + +const char* VehicleWindFactGroup::_directionFactName = "direction"; +const char* VehicleWindFactGroup::_speedFactName = "speed"; +const char* VehicleWindFactGroup::_verticalSpeedFactName = "verticalSpeed"; + +VehicleWindFactGroup::VehicleWindFactGroup(QObject* parent) + : FactGroup(1000, ":/json/Vehicle/WindFact.json", parent) + , _directionFact (0, _directionFactName, FactMetaData::valueTypeDouble) + , _speedFact (0, _speedFactName, FactMetaData::valueTypeDouble) + , _verticalSpeedFact(0, _verticalSpeedFactName, FactMetaData::valueTypeDouble) +{ + _addFact(&_directionFact, _directionFactName); + _addFact(&_speedFact, _speedFactName); + _addFact(&_verticalSpeedFact, _verticalSpeedFactName); + + // Start out as not available "--.--" + _directionFact.setRawValue (qQNaN()); + _speedFact.setRawValue (qQNaN()); + _verticalSpeedFact.setRawValue (qQNaN()); +} + +void VehicleWindFactGroup::handleMessage(Vehicle* /* vehicle */, mavlink_message_t& message) +{ + switch (message.msgid) { + case MAVLINK_MSG_ID_WIND_COV: + _handleWindCov(message); + break; +#if !defined(NO_ARDUPILOT_DIALECT) + case MAVLINK_MSG_ID_WIND: + _handleWind(message); + break; +#endif + case MAVLINK_MSG_ID_HIGH_LATENCY: + _handleHighLatency(message); + break; + case MAVLINK_MSG_ID_HIGH_LATENCY2: + _handleHighLatency2(message); + break; + default: + break; + } +} + +void VehicleWindFactGroup::_handleHighLatency(mavlink_message_t& message) +{ + mavlink_high_latency_t highLatency; + mavlink_msg_high_latency_decode(&message, &highLatency); + speed()->setRawValue((double)highLatency.airspeed / 5.0); +} + +void VehicleWindFactGroup::_handleHighLatency2(mavlink_message_t& message) +{ + mavlink_high_latency2_t highLatency2; + mavlink_msg_high_latency2_decode(&message, &highLatency2); + direction()->setRawValue((double)highLatency2.wind_heading * 2.0); + speed()->setRawValue((double)highLatency2.windspeed / 5.0); +} + +void VehicleWindFactGroup::_handleWindCov(mavlink_message_t& message) +{ + mavlink_wind_cov_t wind; + mavlink_msg_wind_cov_decode(&message, &wind); + + float direction = qRadiansToDegrees(qAtan2(wind.wind_y, wind.wind_x)); + float speed = qSqrt(qPow(wind.wind_x, 2) + qPow(wind.wind_y, 2)); + + if (direction < 0) { + direction += 360; + } + + this->direction()->setRawValue(direction); + this->speed()->setRawValue(speed); + verticalSpeed()->setRawValue(0); +} + +#if !defined(NO_ARDUPILOT_DIALECT) +void VehicleWindFactGroup::_handleWind(mavlink_message_t& message) +{ + mavlink_wind_t wind; + mavlink_msg_wind_decode(&message, &wind); + + // We don't want negative wind angles + float direction = wind.direction; + if (direction < 0) { + direction += 360; + } + this->direction()->setRawValue(direction); + speed()->setRawValue(wind.speed); + verticalSpeed()->setRawValue(wind.speed_z); +} +#endif diff --git a/src/Vehicle/VehicleWindFactGroup.h b/src/Vehicle/VehicleWindFactGroup.h new file mode 100644 index 0000000000000000000000000000000000000000..7972695b5656193f8fcde6cc5de3b17259ffc0a6 --- /dev/null +++ b/src/Vehicle/VehicleWindFactGroup.h @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * 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 VehicleWindFactGroup : public FactGroup +{ + Q_OBJECT + +public: + VehicleWindFactGroup(QObject* parent = nullptr); + + Q_PROPERTY(Fact* direction READ direction CONSTANT) + Q_PROPERTY(Fact* speed READ speed CONSTANT) + Q_PROPERTY(Fact* verticalSpeed READ verticalSpeed CONSTANT) + + Fact* direction () { return &_directionFact; } + Fact* speed () { return &_speedFact; } + Fact* verticalSpeed () { return &_verticalSpeedFact; } + + // Overrides from FactGroup + void handleMessage(Vehicle* vehicle, mavlink_message_t& message) override; + + static const char* _directionFactName; + static const char* _speedFactName; + static const char* _verticalSpeedFactName; + +private: + void _handleHighLatency (mavlink_message_t& message); + void _handleHighLatency2(mavlink_message_t& message); + void _handleWindCov (mavlink_message_t& message); +#if !defined(NO_ARDUPILOT_DIALECT) + void _handleWind (mavlink_message_t& message); +#endif + + Fact _directionFact; + Fact _speedFact; + Fact _verticalSpeedFact; +}; diff --git a/src/ui/toolbar/BatteryIndicator.qml b/src/ui/toolbar/BatteryIndicator.qml index f164b16c5c82de26c9657e84fdb512c80361d03d..f3e1aeb842d50e13d0c89ef46f534ad5a26128f1 100644 --- a/src/ui/toolbar/BatteryIndicator.qml +++ b/src/ui/toolbar/BatteryIndicator.qml @@ -25,8 +25,6 @@ Item { anchors.bottom: parent.bottom width: batteryIndicatorRow.width - Component.onCompleted: console.log("mavlink", MAVLink.MAV_BATTERY_CHARGE_STATE_CRITICAL, MAVLink.MAV_BATTERY_FUNCTION_ALL) - property bool showIndicator: true Row {