Rover: Circle mode enhancements

Rover/mode.h

@@ -499,6 +499,7 @@ class ModeCircle : public Mode
     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)
+    bool tracking_back;     // true if the vehicle is trying to track back onto the circle
 };
 
 class ModeGuided : public Mode

Rover/mode_circle.cpp

@@ -70,7 +70,7 @@ bool ModeCircle::set_center(const Location& center_loc, float radius_m, bool dir
     return true;
 }
 
-// initialize dock mode
+// initialize circle mode from current position
 bool ModeCircle::_enter()
 {
     // capture starting point and yaw
@@ -85,9 +85,15 @@ bool ModeCircle::_enter()
     target.yaw_rad = AP::ahrs().get_yaw();
     target.speed = 0;
 
+    // record center as location (only used for reporting)
+    config.center_loc = rover.current_loc;
+
     // check speed around circle does not lead to excessive lateral acceleration
     check_config_speed();
 
+    // reset tracking_back 
+    tracking_back = false;
+
     // 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());
@@ -146,13 +152,20 @@ void ModeCircle::update()
 
     // Update distance to destination and distance to edge
     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);
+    _distance_to_destination = (target.pos.tofloat() - curr_pos).length();
+    dist_to_edge_m = fabsf(center_to_veh.length() - config.radius);
 
     // Update depending on stage
     if (!reached_edge) {
         update_drive_to_radius();
-
+    } else if (dist_to_edge_m > 2 * MAX(g2.turn_radius, AR_CIRCLE_REACHED_EDGE_DIST) && !tracking_back) {
+        // if more than 2* turn_radius outside circle radius, slow vehicle by 20%
+        config.speed = 0.8 * config.speed;
+        GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Circle: cannot keep up, slowing to %.1fm/s", config.speed);
+        tracking_back = true;
+    } else if (dist_to_edge_m < MAX(g2.turn_radius, AR_CIRCLE_REACHED_EDGE_DIST) && tracking_back) {
+        // if within turn_radius, call the vehicle back on track
+        tracking_back = false;
     } else {
         update_circling();
     }
@@ -190,12 +203,16 @@ void ModeCircle::update_circling()
     const float accel_fb = constrain_float(config.speed - target.speed, -speed_change_max, speed_change_max);
     target.speed += accel_fb;
 
-    // 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 angular rate and update target angle, if the vehicle is not trying to track back
+    if (!tracking_back) {
+        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;
+    } else {
+        init_target_yaw_rad();
+    }
 
     // calculate target point's position, velocity and acceleration
     target.pos = config.center_pos.topostype();
@@ -221,7 +238,7 @@ float ModeCircle::wp_bearing() const
         return 0;
     }
     // calc vector from circle center to vehicle
-    Vector2f veh_to_center = (config.center_pos - curr_pos_NE);
+    Vector2f veh_to_center = (target.pos.tofloat() - curr_pos_NE);
     if (veh_to_center.is_zero()) {
         return 0;
     }
@@ -266,6 +283,7 @@ bool ModeCircle::set_desired_speed(float speed_ms)
 bool ModeCircle::get_desired_location(Location& destination) const
 {
     destination = config.center_loc;
+    destination.offset_bearing(degrees(target.yaw_rad), config.radius);
     return true;
 }