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

Support for mavlink GPS2_RAW message

QGC4.4
david sastre 4 years ago committed by Lorenz Meier
parent
cc95825594
commit
b3f8b40d8a
7 changed files with 109 additions and 2 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. 2
      qgroundcontrol.pro
  2. 2
      src/Vehicle/CMakeLists.txt
  3. 28
      src/Vehicle/Vehicle.cc
  4. 7
      src/Vehicle/Vehicle.h
  5. 41
      src/Vehicle/VehicleGPS2FactGroup.cc
  6. 27
      src/Vehicle/VehicleGPS2FactGroup.h
  7. 4
      src/Vehicle/VehicleGPSFactGroup.h

2
qgroundcontrol.pro
Unescape View File

@ -700,6 +700,7 @@ HEADERS += \ @@ -700,6 +700,7 @@ HEADERS += \
src/Vehicle/VehicleDistanceSensorFactGroup.h \
src/Vehicle/VehicleEstimatorStatusFactGroup.h \
src/Vehicle/VehicleGPSFactGroup.h \
src/Vehicle/VehicleGPS2FactGroup.h \
src/Vehicle/VehicleLinkManager.h \
src/Vehicle/VehicleSetpointFactGroup.h \
src/Vehicle/VehicleTemperatureFactGroup.h \
@ -930,6 +931,7 @@ SOURCES += \ @@ -930,6 +931,7 @@ SOURCES += \
src/Vehicle/VehicleDistanceSensorFactGroup.cc \
src/Vehicle/VehicleEstimatorStatusFactGroup.cc \
src/Vehicle/VehicleGPSFactGroup.cc \
src/Vehicle/VehicleGPS2FactGroup.cc \
src/Vehicle/VehicleLinkManager.cc \
src/Vehicle/VehicleSetpointFactGroup.cc \
src/Vehicle/VehicleTemperatureFactGroup.cc \

2
src/Vehicle/CMakeLists.txt
Unescape View File

@ -57,6 +57,8 @@ add_library(Vehicle @@ -57,6 +57,8 @@ add_library(Vehicle
VehicleEstimatorStatusFactGroup.h
VehicleGPSFactGroup.cc
VehicleGPSFactGroup.h
VehicleGPS2FactGroup.cc
VehicleGPS2FactGroup.h
Vehicle.h
VehicleLinkManager.cc
VehicleLinkManager.h

28
src/Vehicle/Vehicle.cc
Unescape View File

@ -90,6 +90,7 @@ const char* Vehicle::_hobbsFactName = "hobbs"; @@ -90,6 +90,7 @@ const char* Vehicle::_hobbsFactName = "hobbs";
const char* Vehicle::_throttlePctFactName = "throttlePct";
const char* Vehicle::_gpsFactGroupName = "gps";
const char* Vehicle::_gps2FactGroupName = "gps2";
const char* Vehicle::_windFactGroupName = "wind";
const char* Vehicle::_vibrationFactGroupName = "vibration";
const char* Vehicle::_temperatureFactGroupName = "temperature";
@ -143,6 +144,7 @@ Vehicle::Vehicle(LinkInterface* link, @@ -143,6 +144,7 @@ Vehicle::Vehicle(LinkInterface* link,
, _hobbsFact (0, _hobbsFactName, FactMetaData::valueTypeString)
, _throttlePctFact (0, _throttlePctFactName, FactMetaData::valueTypeUint16)
, _gpsFactGroup (this)
, _gps2FactGroup (this)
, _windFactGroup (this)
, _vibrationFactGroup (this)
, _temperatureFactGroup (this)
@ -289,6 +291,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, @@ -289,6 +291,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType,
, _hobbsFact (0, _hobbsFactName, FactMetaData::valueTypeString)
, _throttlePctFact (0, _throttlePctFactName, FactMetaData::valueTypeUint16)
, _gpsFactGroup (this)
, _gps2FactGroup (this)
, _windFactGroup (this)
, _vibrationFactGroup (this)
, _clockFactGroup (this)
@ -398,6 +401,7 @@ void Vehicle::_commonInit() @@ -398,6 +401,7 @@ void Vehicle::_commonInit()
_addFact(&_hobbsFact, _hobbsFactName);
_addFactGroup(&_gpsFactGroup, _gpsFactGroupName);
_addFactGroup(&_gps2FactGroup, _gps2FactGroupName);
_addFactGroup(&_windFactGroup, _windFactGroupName);
_addFactGroup(&_vibrationFactGroup, _vibrationFactGroupName);
_addFactGroup(&_temperatureFactGroup, _temperatureFactGroupName);
@ -662,6 +666,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes @@ -662,6 +666,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
case MAVLINK_MSG_ID_GPS_RAW_INT:
_handleGpsRawInt(message);
break;
case MAVLINK_MSG_ID_GPS2_RAW:
_handleGps2Raw(message);
break;
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
_handleGlobalPositionInt(message);
break;
@ -1025,6 +1032,27 @@ void Vehicle::_handleGpsRawInt(mavlink_message_t& message) @@ -1025,6 +1032,27 @@ void Vehicle::_handleGpsRawInt(mavlink_message_t& message)
}
}
void Vehicle::_handleGps2Raw(mavlink_message_t& message)
{
mavlink_gps2_raw_t gpsRaw;
mavlink_msg_gps2_raw_decode(&message, &gpsRaw);
_gps2RawMessageAvailable = true;
if (gpsRaw.fix_type >= GPS_FIX_TYPE_3D_FIX) {
if (!_globalPositionIntMessageAvailable) {
QGeoCoordinate newPosition(gpsRaw.lat / (double)1E7, gpsRaw.lon / (double)1E7, gpsRaw.alt / 1000.0);
if (newPosition != _coordinate) {
_coordinate = newPosition;
emit coordinateChanged(_coordinate);
}
if (!_altitudeMessageAvailable) {
_altitudeAMSLFact.setRawValue(gpsRaw.alt / 1000.0);
}
}
}
}
void Vehicle::_handleGlobalPositionInt(mavlink_message_t& message)
{
mavlink_global_position_int_t globalPositionInt;

7
src/Vehicle/Vehicle.h
Unescape View File

@ -29,6 +29,7 @@ @@ -29,6 +29,7 @@
#include "VehicleDistanceSensorFactGroup.h"
#include "VehicleWindFactGroup.h"
#include "VehicleGPSFactGroup.h"
#include "VehicleGPS2FactGroup.h"
#include "VehicleSetpointFactGroup.h"
#include "VehicleTemperatureFactGroup.h"
#include "VehicleVibrationFactGroup.h"
@ -280,6 +281,7 @@ public: @@ -280,6 +281,7 @@ public:
Q_PROPERTY(Fact* throttlePct READ throttlePct CONSTANT)
Q_PROPERTY(FactGroup* gps READ gpsFactGroup CONSTANT)
Q_PROPERTY(FactGroup* gps2 READ gps2FactGroup CONSTANT)
Q_PROPERTY(FactGroup* wind READ windFactGroup CONSTANT)
Q_PROPERTY(FactGroup* vibration READ vibrationFactGroup CONSTANT)
Q_PROPERTY(FactGroup* temperature READ temperatureFactGroup CONSTANT)
@ -601,6 +603,7 @@ public: @@ -601,6 +603,7 @@ public:
Fact* throttlePct () { return &_throttlePctFact; }
FactGroup* gpsFactGroup () { return &_gpsFactGroup; }
FactGroup* gps2FactGroup () { return &_gps2FactGroup; }
FactGroup* windFactGroup () { return &_windFactGroup; }
FactGroup* vibrationFactGroup () { return &_vibrationFactGroup; }
FactGroup* temperatureFactGroup () { return &_temperatureFactGroup; }
@ -907,6 +910,7 @@ private: @@ -907,6 +910,7 @@ private:
void _handleExtendedSysState (mavlink_message_t& message);
void _handleCommandAck (mavlink_message_t& message);
void _handleGpsRawInt (mavlink_message_t& message);
void _handleGps2Raw (mavlink_message_t& message);
void _handleGlobalPositionInt (mavlink_message_t& message);
void _handleAltitude (mavlink_message_t& message);
void _handleVfrHud (mavlink_message_t& message);
@ -993,6 +997,7 @@ private: @@ -993,6 +997,7 @@ private:
uint32_t _onboardControlSensorsHealth = 0;
uint32_t _onboardControlSensorsUnhealthy = 0;
bool _gpsRawIntMessageAvailable = false;
bool _gps2RawMessageAvailable = false;
bool _globalPositionIntMessageAvailable = false;
bool _altitudeMessageAvailable = false;
double _defaultCruiseSpeed = qQNaN();
@ -1206,6 +1211,7 @@ private: @@ -1206,6 +1211,7 @@ private:
Fact _throttlePctFact;
VehicleGPSFactGroup _gpsFactGroup;
VehicleGPS2FactGroup _gps2FactGroup;
VehicleWindFactGroup _windFactGroup;
VehicleVibrationFactGroup _vibrationFactGroup;
VehicleTemperatureFactGroup _temperatureFactGroup;
@ -1248,6 +1254,7 @@ private: @@ -1248,6 +1254,7 @@ private:
static const char* _throttlePctFactName;
static const char* _gpsFactGroupName;
static const char* _gps2FactGroupName;
static const char* _windFactGroupName;
static const char* _vibrationFactGroupName;
static const char* _temperatureFactGroupName;

41
src/Vehicle/VehicleGPS2FactGroup.cc
Unescape View File

@ -0,0 +1,41 @@ @@ -0,0 +1,41 @@
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#include "VehicleGPS2FactGroup.h"
#include "Vehicle.h"
#include "QGCGeo.h"
VehicleGPS2FactGroup::VehicleGPS2FactGroup(QObject* parent)
: VehicleGPSFactGroup(parent) {}
void VehicleGPS2FactGroup::handleMessage(Vehicle* /* vehicle */, mavlink_message_t& message)
{
switch (message.msgid) {
case MAVLINK_MSG_ID_GPS2_RAW:
_handleGps2Raw(message);
break;
default:
break;
}
}
void VehicleGPS2FactGroup::_handleGps2Raw(mavlink_message_t& message)
{
mavlink_gps2_raw_t gps2Raw;
mavlink_msg_gps2_raw_decode(&message, &gps2Raw);
lat()->setRawValue (gps2Raw.lat * 1e-7);
lon()->setRawValue (gps2Raw.lon * 1e-7);
mgrs()->setRawValue (convertGeoToMGRS(QGeoCoordinate(gps2Raw.lat * 1e-7, gps2Raw.lon * 1e-7)));
count()->setRawValue (gps2Raw.satellites_visible == 255 ? 0 : gps2Raw.satellites_visible);
hdop()->setRawValue (gps2Raw.eph == UINT16_MAX ? qQNaN() : gps2Raw.eph / 100.0);
vdop()->setRawValue (gps2Raw.epv == UINT16_MAX ? qQNaN() : gps2Raw.epv / 100.0);
courseOverGround()->setRawValue (gps2Raw.cog == UINT16_MAX ? qQNaN() : gps2Raw.cog / 100.0);
lock()->setRawValue (gps2Raw.fix_type);
}

27
src/Vehicle/VehicleGPS2FactGroup.h
Unescape View File

@ -0,0 +1,27 @@ @@ -0,0 +1,27 @@
/****************************************************************************
*
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#pragma once
#include "VehicleGPSFactGroup.h"
#include "QGCMAVLink.h"
class VehicleGPS2FactGroup : public VehicleGPSFactGroup
{
Q_OBJECT
public:
VehicleGPS2FactGroup(QObject* parent = nullptr);
// Overrides from VehicleGPSFactGroup
void handleMessage(Vehicle* vehicle, mavlink_message_t& message) override;
private:
void _handleGps2Raw(mavlink_message_t& message);
};

4
src/Vehicle/VehicleGPSFactGroup.h
Unescape View File

@ -38,7 +38,7 @@ public: @@ -38,7 +38,7 @@ public:
Fact* lock () { return &_lockFact; }
// Overrides from FactGroup
void handleMessage(Vehicle* vehicle, mavlink_message_t& message) override;
virtual void handleMessage(Vehicle* vehicle, mavlink_message_t& message) override;
static const char* _latFactName;
static const char* _lonFactName;
@ -49,7 +49,7 @@ public: @@ -49,7 +49,7 @@ public:
static const char* _countFactName;
static const char* _lockFactName;
private:
protected:
void _handleGpsRawInt (mavlink_message_t& message);
void _handleHighLatency (mavlink_message_t& message);
void _handleHighLatency2(mavlink_message_t& message);

Write Preview
Loading…
Cancel
Save