Skip to content
GitLab
Unverified Commit 8962f261 authored by Gus Grubba's avatar Gus Grubba Committed by GitHub
Browse files

Merge pull request #6380 from mavlink/sendGCSLocation 

Allow manual control of when to send FOLLOW_TARGET messages.
parents c0580acf 9b97ac30
Hide whitespace changes
Inline Side-by-side
src/FollowMe/FollowMe.cc
View file @ 8962f261
......@@ -16,30 +16,69 @@
#include "FollowMe.h"
#include "Vehicle.h"
#include "PositionManager.h"
#include "SettingsManager.h"
#include "AppSettings.h"
FollowMe::FollowMe(QGCApplication* app, QGCToolbox* toolbox)
: QGCTool(app, toolbox), estimatation_capabilities(0)
{
memset(&_motionReport, 0, sizeof(motionReport_s));
runTime.start();
_gcsMotionReportTimer.setSingleShot(false);
}
void FollowMe::setToolbox(QGCToolbox* toolbox)
{
QGCTool::setToolbox(toolbox);
connect(&_gcsMotionReportTimer, &QTimer::timeout, this, &FollowMe::_sendGCSMotionReport);
connect(toolbox->settingsManager()->appSettings()->followTarget(), &Fact::rawValueChanged, this, &FollowMe::_settingsChanged);
_currentMode = _toolbox->settingsManager()->appSettings()->followTarget()->rawValue().toUInt();
if(_currentMode == MODE_ALWAYS) {
_enable();
}
}
void FollowMe::followMeHandleManager(const QString&)
{
QmlObjectListModel & vehicles = *_toolbox->multiVehicleManager()->vehicles();
for (int i=0; i< vehicles.count(); i++) {
Vehicle* vehicle = qobject_cast<Vehicle*>(vehicles[i]);
if (vehicle->px4Firmware() && vehicle->flightMode().compare(FirmwarePlugin::px4FollowMeFlightMode, Qt::CaseInsensitive) == 0) {
_enable();
return;
//-- If we are to change based on current flight mode
if(_currentMode == MODE_FOLLOWME) {
QmlObjectListModel & vehicles = *_toolbox->multiVehicleManager()->vehicles();
for (int i=0; i< vehicles.count(); i++) {
Vehicle* vehicle = qobject_cast<Vehicle*>(vehicles[i]);
if (vehicle->px4Firmware() && vehicle->flightMode().compare(FirmwarePlugin::px4FollowMeFlightMode, Qt::CaseInsensitive) == 0) {
_enable();
return;
}
}
_disable();
}
}
_disable();
void FollowMe::_settingsChanged()
{
uint32_t mode = _toolbox->settingsManager()->appSettings()->followTarget()->rawValue().toUInt();
if(_currentMode != mode) {
_currentMode = mode;
switch (mode) {
case MODE_NEVER:
if(_gcsMotionReportTimer.isActive()) {
_disable();
}
break;
case MODE_ALWAYS:
if(!_gcsMotionReportTimer.isActive()) {
_enable();
}
break;
case MODE_FOLLOWME:
if(!_gcsMotionReportTimer.isActive()) {
followMeHandleManager(QString());
}
break;
default:
break;
}
}
}
void FollowMe::_enable()
......@@ -48,7 +87,6 @@ void FollowMe::_enable()
SIGNAL(positionInfoUpdated(QGeoPositionInfo)),
this,
SLOT(_setGPSLocation(QGeoPositionInfo)));
_gcsMotionReportTimer.setInterval(_toolbox->qgcPositionManager()->updateInterval());
_gcsMotionReportTimer.start();
}
......@@ -59,21 +97,22 @@ void FollowMe::_disable()
SIGNAL(positionInfoUpdated(QGeoPositionInfo)),
this,
SLOT(_setGPSLocation(QGeoPositionInfo)));
_gcsMotionReportTimer.stop();
}
void FollowMe::_setGPSLocation(QGeoPositionInfo geoPositionInfo)
{
if (geoPositionInfo.isValid())
{
if (!geoPositionInfo.isValid()) {
//-- Invalidate cached coordinates
memset(&_motionReport, 0, sizeof(motionReport_s));
} else {
// get the current location coordinates
QGeoCoordinate geoCoordinate = geoPositionInfo.coordinate();
_motionReport.lat_int = geoCoordinate.latitude()*1e7;
_motionReport.lon_int = geoCoordinate.longitude()*1e7;
_motionReport.alt = geoCoordinate.altitude();
_motionReport.lat_int = geoCoordinate.latitude() * 1e7;
_motionReport.lon_int = geoCoordinate.longitude() * 1e7;
_motionReport.alt = geoCoordinate.altitude();
estimatation_capabilities |= (1 << POS);
......@@ -101,7 +140,7 @@ void FollowMe::_setGPSLocation(QGeoPositionInfo geoPositionInfo)
estimatation_capabilities |= (1 << VEL);
qreal direction = _degreesToRadian(geoPositionInfo.attribute(QGeoPositionInfo::Direction));
qreal velocity = geoPositionInfo.attribute(QGeoPositionInfo::GroundSpeed);
qreal velocity = geoPositionInfo.attribute(QGeoPositionInfo::GroundSpeed);
_motionReport.vx = cos(direction)*velocity;
_motionReport.vy = sin(direction)*velocity;
......@@ -113,15 +152,20 @@ void FollowMe::_setGPSLocation(QGeoPositionInfo geoPositionInfo)
}
}
void FollowMe::_sendGCSMotionReport(void)
void FollowMe::_sendGCSMotionReport()
{
QmlObjectListModel & vehicles = *_toolbox->multiVehicleManager()->vehicles();
//-- Do we have any real data?
if(_motionReport.lat_int == 0 && _motionReport.lon_int == 0 && _motionReport.alt == 0) {
return;
}
QmlObjectListModel & vehicles = *_toolbox->multiVehicleManager()->vehicles();
MAVLinkProtocol* mavlinkProtocol = _toolbox->mavlinkProtocol();
mavlink_follow_target_t follow_target;
memset(&follow_target, 0, sizeof(follow_target));
follow_target.timestamp = runTime.nsecsElapsed()*1e-6;
follow_target.timestamp = runTime.nsecsElapsed() * 1e-6;
follow_target.est_capabilities = estimatation_capabilities;
follow_target.position_cov[0] = _motionReport.pos_std_dev[0];
follow_target.position_cov[2] = _motionReport.pos_std_dev[2];
......@@ -133,7 +177,7 @@ void FollowMe::_sendGCSMotionReport(void)
for (int i=0; i< vehicles.count(); i++) {
Vehicle* vehicle = qobject_cast<Vehicle*>(vehicles[i]);
if(vehicle->flightMode().compare(FirmwarePlugin::px4FollowMeFlightMode, Qt::CaseInsensitive) == 0) {
if(_currentMode || vehicle->flightMode().compare(FirmwarePlugin::px4FollowMeFlightMode, Qt::CaseInsensitive) == 0) {
mavlink_message_t message;
mavlink_msg_follow_target_encode_chan(mavlinkProtocol->getSystemId(),
mavlinkProtocol->getComponentId(),
......
src/FollowMe/FollowMe.h
View file @ 8962f261
......@@ -29,12 +29,15 @@ class FollowMe : public QGCTool
public:
FollowMe(QGCApplication* app, QGCToolbox* toolbox);
void setToolbox(QGCToolbox* toolbox) override;
public slots:
void followMeHandleManager(const QString&);
void followMeHandleManager (const QString&);
private slots:
void _setGPSLocation(QGeoPositionInfo geoPositionInfo);
void _sendGCSMotionReport(void);
void _setGPSLocation (QGeoPositionInfo geoPositionInfo);
void _sendGCSMotionReport ();
void _settingsChanged ();
private:
QElapsedTimer runTime;
......@@ -65,8 +68,16 @@ private:
uint8_t estimatation_capabilities;
void _disable();
void _enable();
void _disable ();
void _enable ();
double _degreesToRadian(double deg);
enum {
MODE_NEVER,
MODE_ALWAYS,
MODE_FOLLOWME
};
double _degreesToRadian(double deg);
uint32_t _currentMode;
};
src/Settings/App.SettingsGroup.json
View file @ 8962f261
......@@ -183,5 +183,13 @@
"shortDescription": "Default firmware type for flashing",
"type": "uint32",
"defaultValue": 12
},
{
"name": "FollowTarget",
"shortDescription": "Stream GCS' coordinates to Autopilot",
"type": "uint32",
"enumStrings": "Never,Always,When in Follow Me Flight Mode",
"enumValues": "0,1,2",
"defaultValue": 0
}
]
src/Settings/AppSettings.cc
View file @ 8962f261
......@@ -37,6 +37,7 @@ const char* AppSettings::mapboxTokenName = "MapboxT
const char* AppSettings::esriTokenName = "EsriToken";
const char* AppSettings::defaultFirmwareTypeName = "DefaultFirmwareType";
const char* AppSettings::gstDebugName = "GstreamerDebugLevel";
const char* AppSettings::followTargetName = "FollowTarget";
const char* AppSettings::parameterFileExtension = "params";
const char* AppSettings::planFileExtension = "plan";
......@@ -78,6 +79,7 @@ AppSettings::AppSettings(QObject* parent)
, _esriTokenFact (NULL)
, _defaultFirmwareTypeFact (NULL)
, _gstDebugFact (NULL)
, _followTargetFact (NULL)
{
QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
qmlRegisterUncreatableType<AppSettings>("QGroundControl.SettingsManager", 1, 0, "AppSettings", "Reference only");
......@@ -403,3 +405,13 @@ Fact* AppSettings::defaultFirmwareType(void)
return _defaultFirmwareTypeFact;
}
Fact* AppSettings::followTarget(void)
{
if (!_followTargetFact) {
_followTargetFact = _createSettingsFact(followTargetName);
}
return _followTargetFact;
}
src/Settings/AppSettings.h
View file @ 8962f261
......@@ -41,6 +41,7 @@ public:
Q_PROPERTY(Fact* esriToken READ esriToken CONSTANT)
Q_PROPERTY(Fact* defaultFirmwareType READ defaultFirmwareType CONSTANT)
Q_PROPERTY(Fact* gstDebug READ gstDebug CONSTANT)
Q_PROPERTY(Fact* followTarget READ followTarget CONSTANT)
Q_PROPERTY(QString missionSavePath READ missionSavePath NOTIFY savePathsChanged)
Q_PROPERTY(QString parameterSavePath READ parameterSavePath NOTIFY savePathsChanged)
......@@ -78,6 +79,7 @@ public:
Fact* esriToken (void);
Fact* defaultFirmwareType (void);
Fact* gstDebug (void);
Fact* followTarget (void);
QString missionSavePath (void);
QString parameterSavePath (void);
......@@ -112,6 +114,7 @@ public:
static const char* esriTokenName;
static const char* defaultFirmwareTypeName;
static const char* gstDebugName;
static const char* followTargetName;
// Application wide file extensions
static const char* parameterFileExtension;
......@@ -161,6 +164,7 @@ private:
SettingsFact* _esriTokenFact;
SettingsFact* _defaultFirmwareTypeFact;
SettingsFact* _gstDebugFact;
SettingsFact* _followTargetFact;
};
#endif
src/ui/preferences/GeneralSettings.qml
View file @ 8962f261
......@@ -41,6 +41,7 @@ QGCView {
property real _editFieldWidth: ScreenTools.defaultFontPixelWidth * 30
property Fact _mapProvider: QGroundControl.settingsManager.flightMapSettings.mapProvider
property Fact _mapType: QGroundControl.settingsManager.flightMapSettings.mapType
property Fact _followTarget: QGroundControl.settingsManager.appSettings.followTarget
property real _panelWidth: _qgcView.width * _internalWidthRatio
readonly property real _internalWidthRatio: 0.8
......@@ -245,6 +246,23 @@ QGCView {
}
}
//-----------------------------------------------------------------
//-- Follow Target
Row {
spacing: ScreenTools.defaultFontPixelWidth
visible: _followTarget.visible
QGCLabel {
text: qsTr("Stream GCS Position:")
width: _labelWidth
anchors.verticalCenter: parent.verticalCenter
}
FactComboBox {
width: _editFieldWidth
fact: _followTarget
indexModel: false
anchors.verticalCenter: parent.verticalCenter
}
}
//-----------------------------------------------------------------
//-- Audio preferences
FactCheckBox {
text: qsTr("Mute all audio output")
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment