From 94ab6fc92fdd88471b7b736e3301535c4d4b19eb Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Sun, 22 Sep 2019 11:23:11 -0700 Subject: [PATCH] Finish up follow me support --- src/FirmwarePlugin/APM/APMFirmwarePlugin.cc | 2 +- src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc | 6 +++++- src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h | 1 + src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc | 5 +++++ src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h | 1 + 5 files changed, 13 insertions(+), 2 deletions(-) diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 1ef62b3..2f2f7a8 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -1088,7 +1088,7 @@ void APMFirmwarePlugin::_handleRCChannelsRaw(Vehicle* vehicle, mavlink_message_t void APMFirmwarePlugin::_sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimationCapabilities) { - if (vehicle->homePosition().isValid()) { + if (!vehicle->homePosition().isValid()) { static bool sentOnce = false; if (!sentOnce) { sentOnce = true; diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc index 10e5abb..378bae1 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc @@ -43,7 +43,7 @@ APMCopterMode::APMCopterMode(uint32_t mode, bool settable) : { GUIDED_NOGPS, "Guided No GPS"}, { SMART_RTL, "Smart RTL"}, { FLOWHOLD, "Flow Hold" }, - { FOLLOW, "Follow Vehicle" }, + { FOLLOW, "Follow" }, { ZIGZAG, "ZigZag" }, }); } @@ -136,3 +136,7 @@ bool ArduCopterFirmwarePlugin::vehicleYawsToNextWaypointInMission(const Vehicle* return true; } +void ArduCopterFirmwarePlugin::sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimatationCapabilities) +{ + _sendGCSMotionReport(vehicle, motionReport, estimatationCapabilities); +} diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h index f6dc886..08be16f 100644 --- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.h @@ -71,6 +71,7 @@ public: bool vehicleYawsToNextWaypointInMission (const Vehicle* vehicle) const override; QString autoDisarmParameter (Vehicle* vehicle) override { Q_UNUSED(vehicle); return QStringLiteral("DISARM_DELAY"); } bool supportsSmartRTL (void) const override { return true; } + void sendGCSMotionReport (Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimatationCapabilities) override; private: static bool _remapParamNameIntialized; diff --git a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc index ddf64d7..3383987 100644 --- a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.cc @@ -77,3 +77,8 @@ bool ArduRoverFirmwarePlugin::supportsNegativeThrust(Vehicle* /*vehicle*/) { return true; } + +void ArduRoverFirmwarePlugin::sendGCSMotionReport(Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimatationCapabilities) +{ + _sendGCSMotionReport(vehicle, motionReport, estimatationCapabilities); +} diff --git a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h index c71b395..dea59ff 100644 --- a/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduRoverFirmwarePlugin.h @@ -53,6 +53,7 @@ public: bool supportsNegativeThrust (Vehicle *) final; bool supportsSmartRTL (void) const override { return true; } QString offlineEditingParamFile (Vehicle* vehicle) override { Q_UNUSED(vehicle); return QStringLiteral(":/FirmwarePlugin/APM/Rover.OfflineEditing.params"); } + void sendGCSMotionReport (Vehicle* vehicle, FollowMe::GCSMotionReport& motionReport, uint8_t estimatationCapabilities) override; private: static bool _remapParamNameIntialized;