Rover: add support for rally points

APMrover2/AP_Rally.cpp

@@ -0,0 +1,29 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_Common/Location.h>
+
+#include "Rover.h"
+
+#include "AP_Rally.h"
+
+bool AP_Rally_Rover::is_valid(const Location &rally_point) const
+{
+    Location_Class rally_loc(rally_point);
+    if (!rover.g2.fence.check_destination_within_fence(rally_loc)) {
+        return false;
+    }
+    return true;
+}

APMrover2/AP_Rally.h

@@ -0,0 +1,31 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <AP_Rally/AP_Rally.h>
+#include <AP_AHRS/AP_AHRS.h>
+
+class AP_Rally_Rover : public AP_Rally
+{
+public:
+    AP_Rally_Rover(AP_AHRS &ahrs) : AP_Rally(ahrs) { }
+
+    /* Do not allow copies */
+    AP_Rally_Rover(const AP_Rally_Rover &other) = delete;
+    AP_Rally_Rover &operator=(const AP_Rally_Rover&) = delete;
+
+private:
+    bool is_valid(const Location &rally_point) const override;
+};

APMrover2/GCS_Mavlink.cpp

@@ -1305,6 +1305,15 @@ AP_Mission *GCS_MAVLINK_Rover::get_mission()
     return &rover.mission;
 }
 
+AP_Rally *GCS_MAVLINK_Rover::get_rally() const
+{
+#if AC_RALLY == ENABLED
+    return &rover.rally;
+#else
+    return nullptr;
+#endif
+}
+
 bool GCS_MAVLINK_Rover::set_mode(const uint8_t mode)
 {
     Mode *new_mode = rover.mode_from_mode_num((enum Mode::Number)mode);

APMrover2/GCS_Mavlink.h

@@ -16,7 +16,7 @@ protected:
 
     Compass *get_compass() const override;
     AP_Mission *get_mission() override;
-    AP_Rally *get_rally() const override { return nullptr; };
+    AP_Rally *get_rally() const override;
     AP_Camera *get_camera() const override;
     AP_AdvancedFailsafe *get_advanced_failsafe() const override;
     AP_VisualOdom *get_visual_odom() const override;

APMrover2/Parameters.cpp

@@ -356,6 +356,12 @@ const AP_Param::Info Rover::var_info[] = {
     GOBJECT(camera_mount,           "MNT",  AP_Mount),
 #endif
 
+#if AC_RALLY == ENABLED
+    // @Group: RALLY_
+    // @Path: AP_Rally.cpp,../libraries/AP_Rally/AP_Rally.cpp
+    GOBJECT(rally,      "RALLY_",   AP_Rally_Rover),
+#endif
+
     // @Group: ARMING_
     // @Path: ../libraries/AP_Arming/AP_Arming.cpp
     GOBJECT(arming,                 "ARMING_", AP_Arming),

APMrover2/Parameters.h

@@ -54,6 +54,7 @@ public:
         k_param_serial0_baud,   // deprecated, can be deleted
         k_param_serial1_baud,   // deprecated, can be deleted
         k_param_serial2_baud,   // deprecated, can be deleted
+        k_param_rally,
 
         // 97: RSSI
         k_param_rssi = 97,

APMrover2/Rover.h

@@ -52,7 +52,6 @@
 #include <AP_Notify/AP_Notify.h>                    // Notify library
 #include <AP_OpticalFlow/AP_OpticalFlow.h>          // Optical Flow library
 #include <AP_Param/AP_Param.h>
-#include <AP_Rally/AP_Rally.h>
 #include <AP_RangeFinder/AP_RangeFinder.h>          // Range finder library
 #include <AP_RCMapper/AP_RCMapper.h>                // RC input mapping library
 #include <AP_Relay/AP_Relay.h>                      // APM relay
@@ -97,6 +96,7 @@
 #include "Parameters.h"
 #include "GCS_Mavlink.h"
 #include "GCS_Rover.h"
+#include "AP_Rally.h"
 #include "RC_Channel.h"                  // RC Channel Library
 
 class Rover : public AP_HAL::HAL::Callbacks {
@@ -104,6 +104,7 @@ public:
     friend class GCS_MAVLINK_Rover;
     friend class Parameters;
     friend class ParametersG2;
+    friend class AP_Rally_Rover;
     friend class AP_Arming_Rover;
 #if ADVANCED_FAILSAFE == ENABLED
     friend class AP_AdvancedFailsafe_Rover;
@@ -241,6 +242,9 @@ private:
     AP_Mount camera_mount{ahrs, current_loc};
 #endif
 
+    // Rally library
+    AP_Rally_Rover rally{ahrs};
+
     // true if initialisation has completed
     bool initialised;
 

APMrover2/config.h

@@ -116,6 +116,13 @@
   #define CAMERA ENABLED
 #endif
 
+//////////////////////////////////////////////////////////////////////////////
+// RALLY POINTS
+//
+#ifndef AC_RALLY
+ #define AC_RALLY   ENABLED
+#endif
+
 //////////////////////////////////////////////////////////////////////////////
 // NAVL1
 //

APMrover2/mode.h

@@ -398,6 +398,7 @@ public:
 protected:
 
     bool _enter() override;
+    Location return_target;
 };
 
 class ModeSmartRTL : public Mode

APMrover2/mode_rtl.cpp

@@ -10,10 +10,9 @@ bool ModeRTL::_enter()
 
     // initialise waypoint speed
     set_desired_speed_to_default(true);
-
+    return_target = rover.rally.calc_best_rally_or_home_location(rover.current_loc, ahrs.get_home().alt);
     // set destination
-    set_desired_location(rover.home);
+    set_desired_location(return_target);
-
     return true;
 }