Rover: add circle mode

also auto mode support loiter turns

Rover/Parameters.cpp

@@ -27,7 +27,7 @@ const AP_Param::Info Rover::var_info[] = {
     // @Param: INITIAL_MODE
     // @DisplayName: Initial driving mode
     // @Description: This selects the mode to start in on boot. This is useful for when you want to start in AUTO mode on boot without a receiver. Usually used in combination with when AUTO_TRIGGER_PIN or AUTO_KICKSTART.
-    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,7:Simple,8:Dock,10:Auto,11:RTL,12:SmartRTL,15:Guided
+    // @CopyValuesFrom: MODE1
     // @User: Advanced
     GSCALAR(initial_mode,        "INITIAL_MODE",     Mode::Number::MANUAL),
 
@@ -171,7 +171,7 @@ const AP_Param::Info Rover::var_info[] = {
 
     // @Param: MODE1
     // @DisplayName: Mode1
-    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,7:Simple,8:Dock,10:Auto,11:RTL,12:SmartRTL,15:Guided
+    // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,5:Loiter,6:Follow,7:Simple,8:Dock,9:Circle,10:Auto,11:RTL,12:SmartRTL,15:Guided
     // @User: Standard
     // @Description: Driving mode for switch position 1 (910 to 1230 and above 2049)
     GSCALAR(mode1,           "MODE1",         Mode::Number::MANUAL),
@@ -692,6 +692,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("FS_GCS_TIMEOUT", 56, ParametersG2, fs_gcs_timeout, 5),
 
+    // @Group: CIRC
+    // @Path: mode_circle.cpp
+    AP_SUBGROUPINFO(mode_circle, "CIRC", 57, ParametersG2, ModeCircle),
+
     AP_GROUPEND
 };
 

Rover/Parameters.h

@@ -436,6 +436,8 @@ public:
 
     // FS GCS timeout trigger time
     AP_Float fs_gcs_timeout;
+
+    class ModeCircle mode_circle;
 };
 
 extern const AP_Param::Info var_info[];

Rover/Rover.h

@@ -82,6 +82,7 @@ public:
     friend class Mode;
     friend class ModeAcro;
     friend class ModeAuto;
+    friend class ModeCircle;
     friend class ModeGuided;
     friend class ModeHold;
     friend class ModeLoiter;

Rover/mode.cpp

@@ -537,6 +537,9 @@ Mode *Rover::mode_from_mode_num(const enum Mode::Number num)
     case Mode::Number::SIMPLE:
         ret = &mode_simple;
         break;
+    case Mode::Number::CIRCLE:
+        ret = &g2.mode_circle;
+        break;
     case Mode::Number::AUTO:
         ret = &mode_auto;
         break;

Rover/mode.h

@@ -22,6 +22,7 @@ public:
 #if MODE_DOCK_ENABLED == ENABLED
         DOCK         = 8,
 #endif
+        CIRCLE       = 9,
         AUTO         = 10,
         RTL          = 11,
         SMART_RTL    = 12,
@@ -252,6 +253,12 @@ public:
     // return if external control is allowed in this mode (Guided or Guided-within-Auto)
     bool in_guided_mode() const override { return _submode == Auto_Guided || _submode == Auto_NavScriptTime; }
 
+    // return heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
+    float wp_bearing() const override;
+    float nav_bearing() const override;
+    float crosstrack_error() const override;
+    float get_desired_lat_accel() const override;
+
     // return distance (in meters) to destination
     float get_distance_to_destination() const override;
 
@@ -295,6 +302,7 @@ protected:
         Auto_Guided,            // handover control to external navigation system from within auto mode
         Auto_Stop,              // stop the vehicle as quickly as possible
         Auto_NavScriptTime,     // accept targets from lua scripts while NAV_SCRIPT_TIME commands are executing
+        Auto_Circle,            // circle a given location
     } _submode;
 
 private:
@@ -322,6 +330,8 @@ private:
     bool verify_loiter_time(const AP_Mission::Mission_Command& cmd);
     bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
     bool verify_nav_set_yaw_speed();
+    bool do_circle(const AP_Mission::Mission_Command& cmd);
+    bool verify_circle(const AP_Mission::Mission_Command& cmd);
     void do_wait_delay(const AP_Mission::Mission_Command& cmd);
     void do_within_distance(const AP_Mission::Mission_Command& cmd);
     bool verify_wait_delay();
@@ -385,6 +395,87 @@ private:
     AP_Mission_ChangeDetector mis_change_detector;
 };
 
+class ModeCircle : public Mode
+{
+public:
+
+    // need a constructor for parameters
+    ModeCircle();
+
+    // Does not allow copies
+    CLASS_NO_COPY(ModeCircle);
+
+    uint32_t mode_number() const override { return CIRCLE; }
+    const char *name4() const override { return "CIRC"; }
+
+    // initialise with specific center location, radius (in meters) and direction
+    // replaces use of _enter when initialised from within Auto mode
+    bool set_center(const Location& center_loc, float radius_m, bool dir_ccw);
+
+    // methods that affect movement of the vehicle in this mode
+    void update() override;
+
+    bool is_autopilot_mode() const override { return true; }
+
+    // return desired heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
+    float wp_bearing() const override;
+    float nav_bearing() const override;
+    float crosstrack_error() const override { return dist_to_edge_m; }
+    float get_desired_lat_accel() const override;
+
+    // set desired speed in m/s
+    bool set_desired_speed(float speed_ms) override;
+
+    // return distance (in meters) to destination
+    float get_distance_to_destination() const override { return _distance_to_destination; }
+
+    // get or set desired location
+    bool get_desired_location(Location& destination) const override WARN_IF_UNUSED;
+
+    // return total angle in radians that vehicle has circled
+    // fabsf is used so that full rotations in either direction are counted
+    float get_angle_total_rad() const { return fabsf(angle_total_rad); }
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+protected:
+
+    AP_Float radius;        // circle radius in meters
+    AP_Float speed;         // vehicle speed in m/s.  If zero uses WP_SPEED
+    AP_Int8 direction;      // direction 0:clockwise, 1:counter-clockwise
+
+    // initialise mode
+    bool _enter() override;
+
+    // initialise target_yaw_rad using the vehicle's position and yaw
+    // if there is no current position estimate target_yaw_rad is set to vehicle yaw
+    void init_target_yaw_rad();
+
+    // enum for Direction parameter
+    enum class Direction {
+        CW = 0,
+        CCW = 1
+    };
+
+    // local members
+    struct {
+        Location center_loc;    // circle center as a Location
+        Vector2f center_pos;    // circle center as an offset (in meters) from the EKF origin
+        float radius;   // circle radius
+        float speed;    // desired speed around circle in m/s
+        Direction dir;  // direction, 0:clockwise, 1:counter-clockwise
+    } config;
+    struct {
+        float speed;    // vehicle's target speed around circle in m/s
+        float yaw_rad;  // earth-frame angle of tarrget point on the circle
+        Vector2p pos;   // latest position target sent to position controller
+        Vector2f vel;   // latest velocity target sent to position controller
+        Vector2f accel; // latest accel target sent to position controller
+    } target;
+    float angle_total_rad;  // total angle in radians that vehicle has circled
+    bool reached_edge;      // true once vehicle has reached edge of circle
+    float dist_to_edge_m;   // distance to edge of circle in meters (equivalent to crosstrack error)
+};
 
 class ModeGuided : public Mode
 {

Rover/mode_auto.cpp

@@ -139,6 +139,10 @@ void ModeAuto::update()
         case Auto_NavScriptTime:
             rover.mode_guided.update();
             break;
+
+        case Auto_Circle:
+            rover.g2.mode_circle.update();
+            break;
     }
 }
 
@@ -152,6 +156,102 @@ void ModeAuto::calc_throttle(float target_speed, bool avoidance_enabled)
     Mode::calc_throttle(target_speed, avoidance_enabled);
 }
 
+// return heading (in degrees) to target destination (aka waypoint)
+float ModeAuto::wp_bearing() const
+{
+    switch (_submode) {
+    case Auto_WP:
+        return g2.wp_nav.wp_bearing_cd() * 0.01f;
+    case Auto_HeadingAndSpeed:
+    case Auto_Stop:
+        return 0.0f;
+    case Auto_RTL:
+        return rover.mode_rtl.wp_bearing();
+    case Auto_Loiter:
+        return rover.mode_loiter.wp_bearing();
+    case Auto_Guided:
+    case Auto_NavScriptTime:
+        return rover.mode_guided.wp_bearing();
+    case Auto_Circle:
+        return rover.g2.mode_circle.wp_bearing();
+    }
+
+    // this line should never be reached
+    return 0.0f;
+}
+
+// return short-term target heading in degrees (i.e. target heading back to line between waypoints)
+float ModeAuto::nav_bearing() const
+{
+    switch (_submode) {
+    case Auto_WP:
+        return g2.wp_nav.nav_bearing_cd() * 0.01f;
+    case Auto_HeadingAndSpeed:
+    case Auto_Stop:
+        return 0.0f;
+    case Auto_RTL:
+        return rover.mode_rtl.nav_bearing();
+    case Auto_Loiter:
+        return rover.mode_loiter.nav_bearing();
+    case Auto_Guided:
+    case Auto_NavScriptTime:
+        return rover.mode_guided.nav_bearing();
+    case Auto_Circle:
+        return rover.g2.mode_circle.nav_bearing();
+    }
+
+    // this line should never be reached
+    return 0.0f;
+}
+
+// return cross track error (i.e. vehicle's distance from the line between waypoints)
+float ModeAuto::crosstrack_error() const
+{
+    switch (_submode) {
+    case Auto_WP:
+        return g2.wp_nav.crosstrack_error();
+    case Auto_HeadingAndSpeed:
+    case Auto_Stop:
+        return 0.0f;
+    case Auto_RTL:
+        return rover.mode_rtl.crosstrack_error();
+    case Auto_Loiter:
+        return rover.mode_loiter.crosstrack_error();
+    case Auto_Guided:
+    case Auto_NavScriptTime:
+        return rover.mode_guided.crosstrack_error();
+    case Auto_Circle:
+        return rover.g2.mode_circle.crosstrack_error();
+    }
+
+    // this line should never be reached
+    return 0.0f;
+}
+
+// return desired lateral acceleration
+float ModeAuto::get_desired_lat_accel() const
+{
+    switch (_submode) {
+    case Auto_WP:
+        return g2.wp_nav.get_lat_accel();
+    case Auto_HeadingAndSpeed:
+    case Auto_Stop:
+        return 0.0f;
+    case Auto_RTL:
+        return rover.mode_rtl.get_desired_lat_accel();
+    case Auto_Loiter:
+        return rover.mode_loiter.get_desired_lat_accel();
+    case Auto_Guided:
+    case Auto_NavScriptTime:
+        return rover.mode_guided.get_desired_lat_accel();
+    case Auto_Circle:
+        return rover.g2.mode_circle.get_desired_lat_accel();
+    }
+
+    // this line should never be reached
+    return 0.0f;
+}
+
 // return distance (in meters) to destination
 float ModeAuto::get_distance_to_destination() const
 {
@@ -169,6 +269,8 @@ float ModeAuto::get_distance_to_destination() const
     case Auto_Guided:
     case Auto_NavScriptTime:
         return rover.mode_guided.get_distance_to_destination();
+    case Auto_Circle:
+        return rover.g2.mode_circle.get_distance_to_destination();
     }
 
     // this line should never be reached
@@ -196,6 +298,8 @@ bool ModeAuto::get_desired_location(Location& destination) const
     case Auto_Guided:
     case Auto_NavScriptTime:
         return rover.mode_guided.get_desired_location(destination);
+    case Auto_Circle:
+        return rover.g2.mode_circle.get_desired_location(destination);
     }
 
     // we should never reach here but just in case
@@ -236,7 +340,8 @@ bool ModeAuto::reached_destination() const
     case Auto_Guided:
     case Auto_NavScriptTime:
         return rover.mode_guided.reached_destination();
-        break;
+    case Auto_Circle:
+        return rover.g2.mode_circle.reached_destination();
     }
 
     // we should never reach here but just in case, return true to allow missions to continue
@@ -260,6 +365,8 @@ bool ModeAuto::set_desired_speed(float speed)
     case Auto_Guided:
     case Auto_NavScriptTime:
         return rover.mode_guided.set_desired_speed(speed);
+    case Auto_Circle:
+        return rover.g2.mode_circle.set_desired_speed(speed);
     }
     return false;
 }
@@ -422,6 +529,9 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
     case MAV_CMD_NAV_LOITER_TIME:   // Loiter for specified time
         return do_nav_wp(cmd, true);
 
+    case MAV_CMD_NAV_LOITER_TURNS:
+        return do_circle(cmd);
+
     case MAV_CMD_NAV_GUIDED_ENABLE: // accept navigation commands from external nav computer
         do_nav_guided_enable(cmd);
         break;
@@ -564,6 +674,9 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
     case MAV_CMD_NAV_LOITER_UNLIM:
         return verify_loiter_unlimited(cmd);
 
+    case MAV_CMD_NAV_LOITER_TURNS:
+        return verify_circle(cmd);
+
     case MAV_CMD_NAV_LOITER_TIME:
         return verify_loiter_time(cmd);
 
@@ -802,6 +915,34 @@ bool ModeAuto::verify_nav_set_yaw_speed()
     return true;
 }
 
+bool ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd)
+{
+    // retrieve and sanitize target location
+    Location circle_center = cmd.content.location;
+    circle_center.sanitize(rover.current_loc);
+
+    // calculate radius
+    uint16_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1
+    if (cmd.id == MAV_CMD_NAV_LOITER_TURNS &&
+        cmd.type_specific_bits & (1U << 0)) {
+        // special storage handling allows for larger radii
+        circle_radius_m *= 10;
+    }
+
+    // initialise circle mode
+    if (g2.mode_circle.set_center(circle_center, circle_radius_m, cmd.content.location.loiter_ccw)) {
+        _submode = Auto_Circle;
+        return true;
+    }
+    return false;
+}
+
+bool ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd)
+{
+    // check if we have completed circling
+    return ((g2.mode_circle.get_angle_total_rad() / M_2PI) >= LOWBYTE(cmd.p1));
+}
+
 /********************************************************************************/
 //  Condition (May) commands
 /********************************************************************************/

Rover/mode_circle.cpp

@@ -0,0 +1,237 @@
+#include "Rover.h"
+
+#define AR_CIRCLE_ACCEL_DEFAULT         1.0 // default acceleration in m/s/s if not specified by user
+#define AR_CIRCLE_RADIUS_MIN            0.5 // minimum radius in meters
+#define AR_CIRCLE_REACHED_EDGE_DIST     1.0 // vehicle has reached edge if within 1m
+
+const AP_Param::GroupInfo ModeCircle::var_info[] = {
+
+    // @Param: _RADIUS
+    // @DisplayName: Circle Radius
+    // @Description: Vehicle will circle the center at this distance
+    // @Units: m
+    // @Range: 0 100
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("_RADIUS", 1, ModeCircle, radius, 20),
+
+    // @Param: _SPEED
+    // @DisplayName: Circle Speed
+    // @Description: Vehicle will move at this speed around the circle.  If set to zero WP_SPEED will be used
+    // @Units: m/s
+    // @Range: 0 10
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("_SPEED", 2, ModeCircle, speed, 0),
+
+    // @Param: _DIR
+    // @DisplayName: Circle Direction
+    // @Description: Circle Direction
+    // @Values: 0:Clockwise, 1:Counter-Clockwise
+    // @User: Standard
+    AP_GROUPINFO("_DIR", 3, ModeCircle, direction, 0),
+
+    AP_GROUPEND
+};
+
+ModeCircle::ModeCircle() : Mode()
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+// initialise with specific center location, radius (in meters) and direction
+// replaces use of _enter when initialised from within Auto mode
+bool ModeCircle::set_center(const Location& center_loc, float radius_m, bool dir_ccw)
+{
+    Vector2f center_pos_cm;
+    if (!center_loc.get_vector_xy_from_origin_NE(center_pos_cm)) {
+        return false;
+    }
+    if (!_enter()) {
+        return false;
+    }
+
+    // convert center position from cm to m
+    config.center_pos = center_pos_cm * 0.01;
+
+    // set radius
+    config.radius = MAX(fabsf(radius_m), AR_CIRCLE_RADIUS_MIN);
+
+    // set direction
+    config.dir = dir_ccw ? Direction::CCW : Direction::CW;
+
+    // set target yaw rad (target point on circle)
+    init_target_yaw_rad();
+
+    // record center as location (only used for reporting)
+    config.center_loc = center_loc;
+
+    return true;
+}
+
+// initialize dock mode
+bool ModeCircle::_enter()
+{
+    // capture starting point and yaw
+    if (!AP::ahrs().get_relative_position_NE_origin(config.center_pos)) {
+        return false;
+    }
+    config.radius = MAX(fabsf(radius), AR_CIRCLE_RADIUS_MIN);
+    config.dir = (direction == 1) ? Direction::CCW : Direction::CW;
+    config.speed = is_positive(speed) ? speed : g2.wp_nav.get_default_speed();
+    target.yaw_rad = AP::ahrs().get_yaw();
+    target.speed = 0;
+
+    // calculate speed, accel and jerk limits
+    // otherwise the vehicle uses wp_nav default speed limit
+    float atc_accel_max = MIN(g2.attitude_control.get_accel_max(), g2.attitude_control.get_decel_max());
+    if (!is_positive(atc_accel_max)) {
+        atc_accel_max = AR_CIRCLE_ACCEL_DEFAULT;
+    }
+    const float accel_max = is_positive(g2.wp_nav.get_default_accel()) ? MIN(g2.wp_nav.get_default_accel(), atc_accel_max) : atc_accel_max;
+    const float jerk_max = is_positive(g2.wp_nav.get_default_jerk()) ? g2.wp_nav.get_default_jerk() : accel_max;
+
+    // initialise position controller
+    g2.pos_control.set_limits(config.speed, accel_max, g2.attitude_control.get_turn_lat_accel_max(), jerk_max);
+    g2.pos_control.init();
+
+    // initialise angles covered and reached_edge state
+    angle_total_rad = 0;
+    reached_edge = false;
+    dist_to_edge_m = 0;
+
+    return true;
+}
+
+// initialise target_yaw_rad using the vehicle's position and yaw
+// if there is no current position estimate target_yaw_rad is set to 0
+void ModeCircle::init_target_yaw_rad()
+{
+    // if no position estimate use vehicle yaw
+    Vector2f curr_pos_NE;
+    if (!AP::ahrs().get_relative_position_NE_origin(curr_pos_NE)) {
+        target.yaw_rad = AP::ahrs().yaw;
+        return;
+    }
+
+    // calc vector from circle center to vehicle
+    Vector2f center_to_veh = (curr_pos_NE - config.center_pos);
+    float dist_m = center_to_veh.length();
+
+    // if current position is exactly at the center of the circle return vehicle yaw
+    if (is_zero(dist_m)) {
+        target.yaw_rad = AP::ahrs().yaw;
+    } else {
+        target.yaw_rad = center_to_veh.angle();
+    }
+}
+
+void ModeCircle::update()
+{
+    // get current position
+    Vector2f curr_pos;
+    const bool position_ok = AP::ahrs().get_relative_position_NE_origin(curr_pos);
+
+    // if no position estimate stop vehicle
+    if (!position_ok) {
+        stop_vehicle();
+        return;
+    }
+
+    // check if vehicle has reached edge of circle
+    const Vector2f center_to_veh = curr_pos - config.center_pos;
+    _distance_to_destination = center_to_veh.length();
+    dist_to_edge_m = fabsf(_distance_to_destination - config.radius);
+    if (!reached_edge) {
+        const float dist_thresh_m = MAX(rover.g2.turn_radius, AR_CIRCLE_REACHED_EDGE_DIST);
+        reached_edge = dist_to_edge_m <= dist_thresh_m;
+    }
+
+    // accelerate speed up to desired speed
+    const float speed_max = reached_edge ? config.speed : 0.0;
+    const float speed_change_max = (g2.pos_control.get_accel_max() * rover.G_Dt);
+    target.speed = constrain_float(speed_max, target.speed - speed_change_max, target.speed + speed_change_max);
+
+    // calculate angular rate and update target angle
+    const float circumference = 2.0 * M_PI * config.radius;
+    const float angular_rate_rad = (target.speed / circumference) * M_2PI * (config.dir == Direction::CW ? 1.0 : -1.0);
+    const float angle_dt = angular_rate_rad * rover.G_Dt;
+    target.yaw_rad = wrap_PI(target.yaw_rad + angle_dt);
+    angle_total_rad += angle_dt;
+
+    // calculate target point's position, velocity and acceleration
+    target.pos = config.center_pos.topostype();
+    target.pos.offset_bearing(degrees(target.yaw_rad), config.radius);
+
+    // velocity is perpendicular to angle from the circle's center to the target point on the edge of the circle
+    target.vel = Vector2f(target.speed, 0);
+    target.vel.rotate(target.yaw_rad + radians(90));
+
+    // acceleration is towards center of circle and is speed^2 / radius
+    target.accel = Vector2f(sq(target.speed) / config.radius, 0);
+    target.accel.rotate(target.yaw_rad + radians(180));
+
+    g2.pos_control.input_pos_vel_accel_target(target.pos, target.vel, target.accel, rover.G_Dt);
+    g2.pos_control.update(rover.G_Dt);
+
+    // get desired speed and turn rate from pos_control
+    const float desired_speed = g2.pos_control.get_desired_speed();
+    const float desired_turn_rate = g2.pos_control.get_desired_turn_rate_rads();
+
+    // run steering and throttle controllers
+    calc_steering_from_turn_rate(desired_turn_rate);
+    calc_throttle(desired_speed, true);
+}
+
+// return desired heading (in degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
+float ModeCircle::wp_bearing() const
+{
+    Vector2f curr_pos_NE;
+    if (!AP::ahrs().get_relative_position_NE_origin(curr_pos_NE)) {
+        return 0;
+    }
+    // calc vector from circle center to vehicle
+    Vector2f veh_to_center = (config.center_pos - curr_pos_NE);
+    if (veh_to_center.is_zero()) {
+        return 0;
+    }
+    return degrees(veh_to_center.angle());
+}
+
+float ModeCircle::nav_bearing() const
+{
+    // get position error as a vector from the current position to the target position
+    const Vector2p pos_error = g2.pos_control.get_pos_error();
+    if (pos_error.is_zero()) {
+        return 0;
+    }
+    return degrees(pos_error.angle());
+}
+
+float ModeCircle::get_desired_lat_accel() const
+{
+    return g2.pos_control.get_desired_lat_accel();
+}
+
+// set desired speed in m/s
+bool ModeCircle::set_desired_speed(float speed_ms)
+{
+    if (is_positive(speed_ms)) {
+        config.speed = speed_ms;
+
+        // update position controller limits if required
+        if (config.speed > g2.pos_control.get_speed_max()) {
+            g2.pos_control.set_limits(config.speed, g2.pos_control.get_accel_max(), g2.pos_control.get_lat_accel_max(), g2.pos_control.get_jerk_max());
+        }
+        return true;
+    }
+ 
+    return false;
+}
+
+// return desired location
+bool ModeCircle::get_desired_location(Location& destination) const
+{
+    destination = config.center_loc;
+    return true;
+}