Rover: integrate position controller

Rover/Parameters.cpp

@@ -282,10 +282,6 @@ const AP_Param::Info Rover::var_info[] = {
     // @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp
     GOBJECT(serial_manager,         "SERIAL",   AP_SerialManager),
 
-    // @Group: NAVL1_
-    // @Path: ../libraries/AP_L1_Control/AP_L1_Control.cpp
-    GOBJECT(L1_controller,         "NAVL1_",   AP_L1_Control),
-
     // @Group: RNGFND
     // @Path: ../libraries/AP_RangeFinder/AP_RangeFinder.cpp
     GOBJECT(rangefinder,                 "RNGFND", RangeFinder),
@@ -660,6 +656,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     AP_SUBGROUPINFO(ais, "AIS_",  50, ParametersG2, AP_AIS),
 #endif
 
+    // @Group: PSC
+    // @Path: ../libraries/APM_Control/AR_PosControl.cpp
+    AP_SUBGROUPINFO(pos_control, "PSC", 51, ParametersG2, AR_PosControl),
+
     AP_GROUPEND
 };
 
@@ -709,7 +709,8 @@ ParametersG2::ParametersG2(void)
     avoid(),
     follow(),
     windvane(),
-    wp_nav(attitude_control, rover.L1_controller),
+    pos_control(attitude_control),
+    wp_nav(attitude_control, pos_control),
     sailboat()
 {
     AP_Param::setup_object_defaults(this, var_info);
@@ -744,7 +745,6 @@ const AP_Param::ConversionInfo conversion_table[] = {
     { Parameters::k_param_compass_enabled_deprecated,       0,      AP_PARAM_INT8, "COMPASS_ENABLE" },
     { Parameters::k_param_pivot_turn_angle_old,   0,  AP_PARAM_INT16,  "WP_PIVOT_ANGLE" },
     { Parameters::k_param_waypoint_radius_old,    0,  AP_PARAM_FLOAT,  "WP_RADIUS" },
-    { Parameters::k_param_waypoint_overshoot_old, 0,  AP_PARAM_FLOAT,  "WP_OVERSHOOT" },
     { Parameters::k_param_g2,                20,      AP_PARAM_INT16,  "WP_PIVOT_RATE" },
 
     { Parameters::k_param_g2,                32,      AP_PARAM_FLOAT,  "SAIL_ANGLE_MIN" },
@@ -792,9 +792,6 @@ void Rover::load_parameters(void)
     SRV_Channels::upgrade_parameters();
     hal.console->printf("load_all took %uus\n", unsigned(micros() - before));
 
-    // set a more reasonable default NAVL1_PERIOD for rovers
-    L1_controller.set_default_period(NAVL1_PERIOD);
-
     // convert CH7_OPTION to RC7_OPTION for Rover-3.4 to 3.5 upgrade
     const AP_Param::ConversionInfo ch7_option_info = { Parameters::k_param_ch7_option, 0, AP_PARAM_INT8, "RC7_OPTION" };
     AP_Int8 ch7_opt_old;

Rover/Parameters.h

@@ -206,7 +206,7 @@ public:
         k_param_ins,
         k_param_compass,
         k_param_rcmap,
-        k_param_L1_controller,
+        k_param_L1_controller,          // unused
         k_param_steerController_old,    // unused
         k_param_barometer,
         k_param_notify,
@@ -411,6 +411,9 @@ public:
     // Automatic Identification System - for tracking sea-going vehicles
     AP_AIS ais;
 #endif
+
+    // position controller
+    AR_PosControl pos_control;
 };
 
 extern const AP_Param::Info var_info[];

Rover/Rover.cpp

@@ -371,6 +371,11 @@ void Rover::update_logging1(void)
 
     if (should_log(MASK_LOG_NTUN)) {
         Log_Write_Nav_Tuning();
+        if (g2.pos_control.is_active()) {
+            g2.pos_control.write_log();
+            logger.Write_PID(LOG_PIDN_MSG, g2.pos_control.get_vel_pid().get_pid_info_x());
+            logger.Write_PID(LOG_PIDE_MSG, g2.pos_control.get_vel_pid().get_pid_info_y());
+        }
     }
 
 #if HAL_PROXIMITY_ENABLED
@@ -433,6 +438,7 @@ void Rover::one_second_loop(void)
 
     // send latest param values to wp_nav
     g2.wp_nav.set_turn_params(g2.turn_radius, g2.motors.have_skid_steering());
+    g2.pos_control.set_turn_params(g2.turn_radius, g2.motors.have_skid_steering());
 }
 
 void Rover::update_current_mode(void)

Rover/Rover.h

@@ -34,7 +34,6 @@
 #include <AP_Compass/AP_Compass.h>                  // ArduPilot Mega Magnetometer Library
 #include <AP_Declination/AP_Declination.h>          // Compass declination library
 #include <AP_InertialSensor/AP_InertialSensor.h>    // Inertial Sensor (uncalibated IMU) Library
-#include <AP_L1_Control/AP_L1_Control.h>
 #include <AP_Math/AP_Math.h>                        // ArduPilot Mega Vector/Matrix math Library
 #include <AP_Mission/AP_Mission.h>                  // Mission command library
 #include <AP_Mount/AP_Mount.h>                      // Camera/Antenna mount
@@ -52,6 +51,7 @@
 #include <AP_WheelEncoder/AP_WheelEncoder.h>
 #include <AP_WheelEncoder/AP_WheelRateControl.h>
 #include <APM_Control/AR_AttitudeControl.h>
+#include <APM_Control/AR_PosControl.h>
 #include <AR_WPNav/AR_WPNav.h>
 #include <AP_SmartRTL/AP_SmartRTL.h>
 #include <AP_Logger/AP_Logger.h>
@@ -156,8 +156,6 @@ private:
     // Arming/Disarming management class
     AP_Arming_Rover arming;
 
-    AP_L1_Control L1_controller{ahrs, nullptr};
-
 #if AP_OPTICALFLOW_ENABLED
     OpticalFlow optflow;
 #endif

Rover/mode.cpp

@@ -246,9 +246,9 @@ float Mode::get_desired_lat_accel() const
 }
 
 // set desired location
-bool Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
+bool Mode::set_desired_location(const Location &destination, Location next_destination )
 {
-    if (!g2.wp_nav.set_desired_location(destination, next_leg_bearing_cd)) {
+    if (!g2.wp_nav.set_desired_location(destination, next_destination)) {
         return false;
     }
 

Rover/mode.h

@@ -112,8 +112,8 @@ public:
     virtual bool get_desired_location(Location& destination) const WARN_IF_UNUSED { return false; }
 
     // set desired location (used in Guided, Auto)
-    //   next_leg_bearing_cd should be heading to the following waypoint (used to slow the vehicle in order to make the turn)
+    // set next_destination (if known).  If not provided vehicle stops at destination
-    virtual bool set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) WARN_IF_UNUSED;
+    virtual bool set_desired_location(const Location &destination, Location next_destination = Location()) WARN_IF_UNUSED;
 
     // true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck
     virtual bool reached_destination() const { return true; }
@@ -262,7 +262,7 @@ public:
 
     // get or set desired location
     bool get_desired_location(Location& destination) const override WARN_IF_UNUSED;
-    bool set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) override WARN_IF_UNUSED;
+    bool set_desired_location(const Location &destination, Location next_destination = Location()) override WARN_IF_UNUSED;
     bool reached_destination() const override;
 
     // set desired speed in m/s
@@ -413,7 +413,7 @@ public:
 
     // get or set desired location
     bool get_desired_location(Location& destination) const override WARN_IF_UNUSED;
-    bool set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) override WARN_IF_UNUSED;
+    bool set_desired_location(const Location &destination, Location next_destination = Location()) override WARN_IF_UNUSED;
 
     // set desired heading and speed
     void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed);

Rover/mode_auto.cpp

@@ -61,17 +61,18 @@ void ModeAuto::update()
     switch (_submode) {
         case Auto_WP:
         {
-            if (!g2.wp_nav.reached_destination()) {
+            // check if we've reached the destination
+            if (!g2.wp_nav.reached_destination() || g2.wp_nav.is_fast_waypoint()) {
                 // update navigation controller
                 navigate_to_waypoint();
             } else {
                 // we have reached the destination so stay here
                 if (rover.is_boat()) {
                     if (!start_loiter()) {
-                        stop_vehicle();
+                        start_stop();
                     }
                 } else {
-                    stop_vehicle();
+                    start_stop();
                 }
                 // update distance to destination
                 _distance_to_destination = rover.current_loc.get_distance(g2.wp_nav.get_destination());
@@ -187,10 +188,10 @@ bool ModeAuto::get_desired_location(Location& destination) const
 }
 
 // set desired location to drive to
-bool ModeAuto::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
+bool ModeAuto::set_desired_location(const Location &destination, Location next_destination)
 {
     // call parent
-    if (!Mode::set_desired_location(destination, next_leg_bearing_cd)) {
+    if (!Mode::set_desired_location(destination, next_destination)) {
         return false;
     }
 
@@ -602,29 +603,36 @@ void ModeAuto::do_RTL(void)
 
 bool ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination)
 {
-    // get heading to following waypoint (auto mode reduces speed to allow corning without large overshoot)
-    // in case of non-zero loiter duration, we provide heading-unknown to signal we should stop at the point
-    float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN;
-    if (!always_stop_at_destination && loiter_duration == 0) {
-        next_leg_bearing_cd = mission.get_next_ground_course_cd(AR_WPNAV_HEADING_UNKNOWN);
-    }
-
     // retrieve and sanitize target location
     Location cmdloc = cmd.content.location;
     cmdloc.sanitize(rover.current_loc);
-    if (!set_desired_location(cmdloc, next_leg_bearing_cd)) {
+
+    // delayed stored in p1 in seconds
+    loiter_duration = ((int16_t) cmd.p1 < 0) ? 0 : cmd.p1;
+    loiter_start_time = 0;
+    if (loiter_duration > 0) {
+        always_stop_at_destination = true;
+    }
+
+    // do not add next wp if there are no more navigation commands
+    AP_Mission::Mission_Command next_cmd;
+    if (always_stop_at_destination || !mission.get_next_nav_cmd(cmd.index+1, next_cmd)) {
+        // single destination
+        if (!set_desired_location(cmdloc)) {
             return false;
         }
+    } else {
+        // retrieve and sanitize next destination location
+        Location next_cmdloc = next_cmd.content.location;
+        next_cmdloc.sanitize(cmdloc);
+        if (!set_desired_location(cmdloc, next_cmdloc)) {
+            return false;
+        }
+    }
 
     // just starting so we haven't previously reached the waypoint
     previously_reached_wp = false;
 
-    // this will be used to remember the time in millis after we reach or pass the WP.
-    loiter_start_time = 0;
-
-    // this is the delay, stored in seconds, checked such that commanded delays < 0 delay 0 seconds
-    loiter_duration = ((int16_t) cmd.p1 < 0) ? 0 : cmd.p1;
-
     return true;
 }
 

Rover/mode_guided.cpp

@@ -232,10 +232,9 @@ bool ModeGuided::get_desired_location(Location& destination) const
 }
 
 // set desired location
-bool ModeGuided::set_desired_location(const struct Location& destination,
+bool ModeGuided::set_desired_location(const Location &destination, Location next_destination)
-                                      float next_leg_bearing_cd)
 {
-    if (g2.wp_nav.set_desired_location(destination, next_leg_bearing_cd)) {
+    if (g2.wp_nav.set_desired_location(destination, next_destination)) {
 
         // handle guided specific initialisation and logging
         _guided_mode = ModeGuided::Guided_WP;

Rover/mode_smart_rtl.cpp

@@ -49,20 +49,33 @@ void ModeSmartRTL::update()
         case SmartRTL_PathFollow:
             // load point if required
             if (_load_point) {
-                Vector3f next_point;
+                Vector3f dest_NED;
-                if (!g2.smart_rtl.pop_point(next_point)) {
+                if (!g2.smart_rtl.pop_point(dest_NED)) {
                     // if not more points, we have reached home
                     gcs().send_text(MAV_SEVERITY_INFO, "Reached destination");
                     smart_rtl_state = SmartRTL_StopAtHome;
                     break;
-                }
+                } else {
-                _load_point = false;
+                    // peek at the next point.  this can fail if the IO task currently has the path semaphore
-                // set target destination to new point
+                    Vector3f next_dest_NED;
-                if (!g2.wp_nav.set_desired_location_NED(next_point)) {
+                    if (g2.smart_rtl.peek_point(next_dest_NED)) {
-                    // this failure should never happen but we add it just in case
+                        if (!g2.wp_nav.set_desired_location_NED(dest_NED, next_dest_NED)) {
+                            // this should never happen because the EKF origin should already be set
                             gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination");
                             smart_rtl_state = SmartRTL_Failure;
+                            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
                         }
+                    } else {
+                        // no next point so add only immediate point
+                        if (!g2.wp_nav.set_desired_location_NED(dest_NED)) {
+                            // this should never happen because the EKF origin should already be set
+                            gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination");
+                            smart_rtl_state = SmartRTL_Failure;
+                            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+                        }
+                    }
+                }
+                _load_point = false;
             }
             // update navigation controller
             navigate_to_waypoint();