zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Correct handling of GPS_RAW_INT, GLOBAL_POSITION_INT, VFR_HUD 

* Moved from UAS
* Remove unused code
QGC4.4
Don Gagne 8 years ago
parent
23d3eb50ea
commit
12e5dd7145
2 changed files with 89 additions and 127 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 192
      src/Vehicle/Vehicle.cc
  2. 24
      src/Vehicle/Vehicle.h

192
src/Vehicle/Vehicle.cc
Unescape View File

@ -85,11 +85,6 @@ Vehicle::Vehicle(LinkInterface* link, @@ -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, @@ -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, @@ -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, @@ -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, @@ -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, @@ -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, @@ -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 @@ -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 @@ -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, @@ -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) @@ -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) @@ -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<UAS*>(_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";

24
src/Vehicle/Vehicle.h
Unescape View File

@ -137,13 +137,6 @@ public: @@ -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: @@ -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: @@ -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: @@ -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: @@ -779,6 +766,9 @@ private:
uint32_t _onboardControlSensorsEnabled;
uint32_t _onboardControlSensorsHealth;
uint32_t _onboardControlSensorsUnhealthy;
bool _useGpsRawIntForPosition;
bool _useGpsRawIntForAltitude;
bool _useAltitudeForAltitude;
typedef struct {
int component;

Write Preview
Loading…
Cancel
Save