diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 6c0740bc7cd6aeb6441610defab489cc8a182ffa..df7b8afa370686d9b91435b17cac4b2ebe698e41 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -85,11 +85,6 @@ Vehicle::Vehicle(LinkInterface* link, , _currentWarningCount(0) , _currentNormalCount(0) , _currentMessageType(MessageNone) - , _navigationAltitudeError(0.0f) - , _navigationSpeedError(0.0f) - , _navigationCrosstrackError(0.0f) - , _navigationTargetBearing(0.0f) - , _refreshTimer(new QTimer(this)) , _updateCount(0) , _rcRSSI(255) , _rcRSSIstore(255) @@ -99,6 +94,9 @@ Vehicle::Vehicle(LinkInterface* link, , _onboardControlSensorsEnabled(0) , _onboardControlSensorsHealth(0) , _onboardControlSensorsUnhealthy(0) + , _useGpsRawIntForPosition(true) + , _useGpsRawIntForAltitude(true) + , _useAltitudeForAltitude(false) , _connectionLost(false) , _connectionLostEnabled(true) , _missionManager(NULL) @@ -150,11 +148,6 @@ Vehicle::Vehicle(LinkInterface* link, connect(this, &Vehicle::armedChanged, this, &Vehicle::_announceArmedChanged); _uas = new UAS(_mavlink, this, _firmwarePluginManager); - setLatitude(_uas->getLatitude()); - setLongitude(_uas->getLongitude()); - - connect(_uas, &UAS::latitudeChanged, this, &Vehicle::setLatitude); - connect(_uas, &UAS::longitudeChanged, this, &Vehicle::setLongitude); connect(_uas, &UAS::imageReady, this, &Vehicle::_imageReady); connect(this, &Vehicle::remoteControlRSSIChanged, this, &Vehicle::_remoteControlRSSIChanged); @@ -164,11 +157,6 @@ Vehicle::Vehicle(LinkInterface* link, // connect this vehicle to the follow me handle manager connect(this, &Vehicle::flightModeChanged,qgcApp()->toolbox()->followMe(), &FollowMe::followMeHandleManager); - // Refresh timer - connect(_refreshTimer, &QTimer::timeout, this, &Vehicle::_checkUpdate); - _refreshTimer->setInterval(UPDATE_TIMER); - _refreshTimer->start(UPDATE_TIMER); - // PreArm Error self-destruct timer connect(&_prearmErrorTimer, &QTimer::timeout, this, &Vehicle::_prearmErrorTimeout); _prearmErrorTimer.setInterval(_prearmErrorTimeoutMSecs); @@ -196,11 +184,6 @@ Vehicle::Vehicle(LinkInterface* link, connect(_mav, SIGNAL(attitudeChanged (UASInterface*,int,double,double,double,quint64)), this, SLOT(_updateAttitude(UASInterface*,int,double, double, double, quint64))); connect(_mav, SIGNAL(statusChanged (UASInterface*,QString,QString)), this, SLOT(_updateState(UASInterface*, QString,QString))); - connect(_mav, &UASInterface::speedChanged, this, &Vehicle::_updateSpeed); - connect(_mav, &UASInterface::altitudeChanged, this, &Vehicle::_updateAltitude); - connect(_mav, &UASInterface::navigationControllerErrorsChanged,this, &Vehicle::_updateNavigationControllerErrors); - connect(_mav, &UASInterface::NavigationControllerDataChanged, this, &Vehicle::_updateNavigationControllerData); - _loadSettings(); _missionManager = new MissionManager(this); @@ -284,11 +267,6 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _currentWarningCount(0) , _currentNormalCount(0) , _currentMessageType(MessageNone) - , _navigationAltitudeError(0.0f) - , _navigationSpeedError(0.0f) - , _navigationCrosstrackError(0.0f) - , _navigationTargetBearing(0.0f) - , _refreshTimer(new QTimer(this)) , _updateCount(0) , _rcRSSI(255) , _rcRSSIstore(255) @@ -298,6 +276,9 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _onboardControlSensorsEnabled(0) , _onboardControlSensorsHealth(0) , _onboardControlSensorsUnhealthy(0) + , _useGpsRawIntForPosition(true) + , _useGpsRawIntForAltitude(true) + , _useAltitudeForAltitude(false) , _connectionLost(false) , _connectionLostEnabled(true) , _missionManager(NULL) @@ -496,6 +477,18 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_LOGGING_DATA_ACKED: _handleMavlinkLoggingDataAcked(message); break; + case MAVLINK_MSG_ID_GPS_RAW_INT: + _handleGpsRawInt(message); + break; + case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: + _handleGlobalPositionInt(message); + break; + case MAVLINK_MSG_ID_ALTITUDE: + _handleAltitude(message); + break; + case MAVLINK_MSG_ID_VFR_HUD: + _handleVfrHud(message); + break; // Following are ArduPilot dialect messages @@ -509,6 +502,70 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes _uas->receiveMessage(message); } +void Vehicle::_handleVfrHud(mavlink_message_t& message) +{ + mavlink_vfr_hud_t vfrHud; + mavlink_msg_vfr_hud_decode(&message, &vfrHud); + + _airSpeedFact.setRawValue(qIsNaN(vfrHud.airspeed) ? 0 : vfrHud.airspeed); + _groundSpeedFact.setRawValue(qIsNaN(vfrHud.groundspeed) ? 0 : vfrHud.groundspeed); + _climbRateFact.setRawValue(qIsNaN(vfrHud.climb) ? 0 : vfrHud.climb); +} + +void Vehicle::_handleGpsRawInt(mavlink_message_t& message) +{ + mavlink_gps_raw_int_t gpsRawInt; + mavlink_msg_gps_raw_int_decode(&message, &gpsRawInt); + + if (gpsRawInt.fix_type >= GPS_FIX_TYPE_3D_FIX) { + if (_useGpsRawIntForPosition) { + setLatitude(gpsRawInt.lat / (double)1E7); + setLongitude(gpsRawInt.lon / (double)1E7); + } + if (_useGpsRawIntForAltitude) { + _altitudeRelativeFact.setRawValue(gpsRawInt.alt / 1000.0); + } + } + + _gpsFactGroup.count()->setRawValue(gpsRawInt.satellites_visible == 255 ? 0 : gpsRawInt.satellites_visible); + _gpsFactGroup.hdop()->setRawValue(gpsRawInt.eph == UINT16_MAX ? 1e10f : gpsRawInt.eph / 100.0); + _gpsFactGroup.vdop()->setRawValue(gpsRawInt.epv == UINT16_MAX ? 1e10f : gpsRawInt.epv / 100.0); + _gpsFactGroup.courseOverGround()->setRawValue(gpsRawInt.cog == UINT16_MAX ? 0.0 : gpsRawInt.cog / 100.0); + _gpsFactGroup.lock()->setRawValue(gpsRawInt.fix_type); + + if (gpsRawInt.fix_type >= GPS_FIX_TYPE_3D_FIX) { + _setCoordinateValid(true); + } +} + +void Vehicle::_handleGlobalPositionInt(mavlink_message_t& message) +{ + mavlink_global_position_int_t globalPositionInt; + mavlink_msg_global_position_int_decode(&message, &globalPositionInt); + + _useGpsRawIntForPosition = false; + _useGpsRawIntForAltitude = false; + + setLatitude(globalPositionInt.lat / (double)1E7); + setLongitude(globalPositionInt.lon / (double)1E7); + if (!_useAltitudeForAltitude) { + _altitudeRelativeFact.setRawValue(globalPositionInt.relative_alt / 1000.0); + _altitudeAMSLFact.setRawValue(globalPositionInt.alt / 1000.0); + } +} + +void Vehicle::_handleAltitude(mavlink_message_t& message) +{ + mavlink_altitude_t altitude; + mavlink_msg_altitude_decode(&message, &altitude); + + _useAltitudeForAltitude = true; + _useGpsRawIntForAltitude = false; + _altitudeRelativeFact.setRawValue(altitude.altitude_relative / 1000.0); + _altitudeAMSLFact.setRawValue(altitude.altitude_amsl / 1000.0); + +} + void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& message) { mavlink_autopilot_version_t autopilotVersion; @@ -986,31 +1043,6 @@ void Vehicle::_updateAttitude(UASInterface* uas, int, double roll, double pitch, _updateAttitude(uas, roll, pitch, yaw, timestamp); } -void Vehicle::_updateSpeed(UASInterface*, double groundSpeed, double airSpeed, quint64) -{ - _groundSpeedFact.setRawValue(groundSpeed); - _airSpeedFact.setRawValue(airSpeed); -} - -void Vehicle::_updateAltitude(UASInterface*, double altitudeAMSL, double altitudeRelative, double climbRate, quint64) -{ - _altitudeAMSLFact.setRawValue(altitudeAMSL); - _altitudeRelativeFact.setRawValue(altitudeRelative); - _climbRateFact.setRawValue(climbRate); -} - -void Vehicle::_updateNavigationControllerErrors(UASInterface*, double altitudeError, double speedError, double xtrackError) { - _navigationAltitudeError = altitudeError; - _navigationSpeedError = speedError; - _navigationCrosstrackError = xtrackError; -} - -void Vehicle::_updateNavigationControllerData(UASInterface *uas, float, float, float, float targetBearing, float) { - if (_mav == uas) { - _navigationTargetBearing = targetBearing; - } -} - int Vehicle::motorCount(void) { switch (_vehicleType) { @@ -1046,19 +1078,6 @@ bool Vehicle::xConfigMotors(void) * Internal */ -void Vehicle::_checkUpdate() -{ - // Update current location - if(_mav) { - if(latitude() != _mav->getLatitude()) { - setLatitude(_mav->getLatitude()); - } - if(longitude() != _mav->getLongitude()) { - setLongitude(_mav->getLongitude()); - } - } -} - QString Vehicle::getMavIconColor() { // TODO: Not using because not only the colors are ghastly, it doesn't respect dark/light palette @@ -2115,53 +2134,6 @@ void Vehicle::setFirmwarePluginInstanceData(QObject* firmwarePluginInstanceData) void VehicleGPSFactGroup::setVehicle(Vehicle* vehicle) { _vehicle = vehicle; - - if (!vehicle) { - // Disconnected Vehicle - return; - } - - connect(_vehicle->uas(), &UASInterface::localizationChanged, this, &VehicleGPSFactGroup::_setSatLoc); - - UAS* pUas = dynamic_cast(_vehicle->uas()); - connect(pUas, &UAS::satelliteCountChanged, this, &VehicleGPSFactGroup::_setSatelliteCount); - connect(pUas, &UAS::satRawHDOPChanged, this, &VehicleGPSFactGroup::_setSatRawHDOP); - connect(pUas, &UAS::satRawVDOPChanged, this, &VehicleGPSFactGroup::_setSatRawVDOP); - connect(pUas, &UAS::satRawCOGChanged, this, &VehicleGPSFactGroup::_setSatRawCOG); -} - -void VehicleGPSFactGroup::_setSatelliteCount(double val, QString) -{ - // I'm assuming that a negative value or over 99 means there is no GPS - if(val < 0.0) val = -1.0; - if(val > 99.0) val = -1.0; - - _countFact.setRawValue(val); -} - -void VehicleGPSFactGroup::_setSatRawHDOP(double val) -{ - _hdopFact.setRawValue(val); -} - -void VehicleGPSFactGroup::_setSatRawVDOP(double val) -{ - _vdopFact.setRawValue(val); -} - -void VehicleGPSFactGroup::_setSatRawCOG(double val) -{ - _courseOverGroundFact.setRawValue(val); -} - -void VehicleGPSFactGroup::_setSatLoc(UASInterface*, int fix) -{ - _lockFact.setRawValue(fix); - - // fix 0: lost, 1: at least one satellite, but no GPS fix, 2: 2D lock, 3: 3D lock - if (fix > 2) { - _vehicle->_setCoordinateValid(true); - } } const char* VehicleBatteryFactGroup::_voltageFactName = "voltage"; diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 1f8eb3cb39fc25e690526bffe07d6d8c55e98779..2d6bdcd4e64fb1e2e49f5fcc81c038bf6c478bff 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -137,13 +137,6 @@ public: static const char* _countFactName; static const char* _lockFactName; -private slots: - void _setSatelliteCount(double val, QString); - void _setSatRawHDOP(double val); - void _setSatRawVDOP(double val); - void _setSatRawCOG(double val); - void _setSatLoc(UASInterface*, int fix); - private: Vehicle* _vehicle; Fact _hdopFact; @@ -683,11 +676,6 @@ private slots: void _updateAttitude (UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp); /** @brief Attitude from one specific component / redundant autopilot */ void _updateAttitude (UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 timestamp); - void _updateSpeed (UASInterface* uas, double _groundSpeed, double _airSpeed, quint64 timestamp); - void _updateAltitude (UASInterface* uas, double _altitudeAMSL, double _altitudeRelative, double _climbRate, quint64 timestamp); - void _updateNavigationControllerErrors (UASInterface* uas, double altitudeError, double speedError, double xtrackError); - void _updateNavigationControllerData (UASInterface *uas, float navRoll, float navPitch, float navBearing, float targetBearing, float targetDistance); - void _checkUpdate (); void _updateState (UASInterface* system, QString name, QString description); /** @brief A new camera image has arrived */ void _imageReady (UASInterface* uas); @@ -716,6 +704,10 @@ private: void _handleCommandAck(mavlink_message_t& message); void _handleAutopilotVersion(LinkInterface* link, mavlink_message_t& message); void _handleHilActuatorControls(mavlink_message_t& message); + void _handleGpsRawInt(mavlink_message_t& message); + void _handleGlobalPositionInt(mavlink_message_t& message); + void _handleAltitude(mavlink_message_t& message); + void _handleVfrHud(mavlink_message_t& message); void _missionManagerError(int errorCode, const QString& errorMsg); void _geoFenceManagerError(int errorCode, const QString& errorMsg); void _rallyPointManagerError(int errorCode, const QString& errorMsg); @@ -763,11 +755,6 @@ private: int _currentNormalCount; MessageType_t _currentMessageType; QString _latestError; - float _navigationAltitudeError; - float _navigationSpeedError; - float _navigationCrosstrackError; - float _navigationTargetBearing; - QTimer* _refreshTimer; QString _currentState; int _updateCount; QString _formatedMessage; @@ -779,6 +766,9 @@ private: uint32_t _onboardControlSensorsEnabled; uint32_t _onboardControlSensorsHealth; uint32_t _onboardControlSensorsUnhealthy; + bool _useGpsRawIntForPosition; + bool _useGpsRawIntForAltitude; + bool _useAltitudeForAltitude; typedef struct { int component;