Rover: replace vehicle code guided methods with calls to guided mode class

set_guided_WP replaced by mode_guided.set_desired_location
nav_set_yaw_speed replaced with mode_guided.set_desired_heading_and_speed
set_guided_velocity replaced with mode_guided.set_desired_turn_rate_and_speed
guided_control structure replaced with mode_guided members
use_pivot_steering accepts yaw-error argument instead of calculating it itself internally

APMrover2/GCS_Mavlink.cpp

@@ -657,7 +657,7 @@ bool GCS_MAVLINK_Rover::handle_guided_request(AP_Mission::Mission_Command &cmd)
     }
 
     // make any new wp uploaded instant (in case we are already in Guided mode)
-    rover.set_guided_WP(cmd.content.location);
+    rover.mode_guided.set_desired_location(cmd.content.location);
     return true;
 }
 
@@ -927,11 +927,9 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
                 break;
             }
 
-            rover.mode_guided.guided_mode = ModeGuided::Guided_Angle;
-            rover.guided_control.msg_time_ms = AP_HAL::millis();
-            rover.guided_control.turn_angle = packet.param1;
-            rover.guided_control.target_speed = constrain_float(packet.param2, 0.0f, 1.0f);
-            rover.nav_set_yaw_speed();
+            // send yaw change and target speed to guided mode controller
+            float target_speed = constrain_float(packet.param2 * rover.g.speed_cruise, -rover.g.speed_cruise, rover.g.speed_cruise);
+            rover.mode_guided.set_desired_heading_delta_and_speed(packet.param1, target_speed);
             result = MAV_RESULT_ACCEPTED;
             break;
         }
@@ -1044,8 +1042,6 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             if (rover.control_mode != &rover.mode_guided) {
                 break;
             }
-            // record the time when the last message arrived
-            rover.guided_control.msg_time_ms = AP_HAL::millis();
 
             // ensure type_mask specifies to use thrust
             if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_THROTTLE_IGNORE) != 0) {
@@ -1070,7 +1066,8 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
                 // int32_t target_heading_cd = static_cast<int32_t>(degrees(Quaternion(packet.q[0], packet.q[1], packet.q[2], packet.q[3]).get_euler_yaw()) * 100);
                 // TODO : handle that
             } else {
-                rover.set_guided_velocity((RAD_TO_DEG * packet.body_yaw_rate), target_speed);
+                // use body_yaw_rate field
+                rover.mode_guided.set_desired_turn_rate_and_speed((RAD_TO_DEG * packet.body_yaw_rate) * 100.0f, target_speed);
             }
             break;
         }
@@ -1098,9 +1095,6 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
             bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
 
-            // record the time when the last message arrived
-            rover.guided_control.msg_time_ms = AP_HAL::millis();
-
             // prepare and send target position
             Location target_loc = rover.current_loc;
             if (!pos_ignore) {
@@ -1128,7 +1122,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
                 }
             }
             float target_speed = 0.0f;
-            float target_steer_speed = 0.0f;
+            float target_turn_rate_cds = 0.0f;
             if (!vel_ignore) {
                 // use packet vy (forward in NED) for speed in m/s and packet yaw_rate for turn in rad/s
                 float velx = packet.vx;
@@ -1140,20 +1134,20 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
                     vely = velx * rover.ahrs.sin_yaw() + vely * rover.ahrs.cos_yaw();
                 }
                 target_speed = vely;
-                target_steer_speed = RAD_TO_DEG * packet.yaw_rate;
+                target_turn_rate_cds = RAD_TO_DEG * packet.yaw_rate;
                 // TODO : take into account reverse speed
                 // TODO : handle yaw heading cmd
             }
 
             if (!pos_ignore && !vel_ignore && acc_ignore) {
-                rover.set_guided_WP(target_loc);
+                rover.mode_guided.set_desired_location(target_loc);
                 if (!is_zero(target_speed)) {
-                    rover.guided_control.target_speed = target_speed;
+                    rover.mode_guided.set_desired_speed(target_speed);
                 }
             } else if (pos_ignore && !vel_ignore && acc_ignore) {
-                rover.set_guided_velocity(target_steer_speed, target_speed);
+                rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, target_speed);
             } else if (!pos_ignore && vel_ignore && acc_ignore) {
-                rover.set_guided_WP(target_loc);
+                rover.mode_guided.set_desired_location(target_loc);
             } else {
                 // result = MAV_RESULT_FAILED;  // TODO : support that
             }
@@ -1181,9 +1175,6 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
             bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
 
-            // record the time where the last message arrived
-            rover.guided_control.msg_time_ms = AP_HAL::millis();
-
             // prepare and send target position
             Location target_loc = rover.current_loc;
             if (!pos_ignore) {
@@ -1196,24 +1187,24 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
                 target_loc.lng = packet.lon_int;
             }
             float target_speed = 0.0f;
-            float target_steer_speed = 0.0f;
+            float target_turn_rate_cds = 0.0f;
             if (!vel_ignore) {
                 // use packet vy (forward in NED) for speed in m/s and packet yaw_rate for turn in rad/s
                 target_speed = packet.vy;
-                target_steer_speed = RAD_TO_DEG * packet.yaw_rate;
+                target_turn_rate_cds = RAD_TO_DEG * packet.yaw_rate;
                 // TODO : take into account reverse speed
                 // TODO : handle yaw heading cmd
             }
 
             if (!pos_ignore && !vel_ignore && acc_ignore) {
-                rover.set_guided_WP(target_loc);
+                rover.mode_guided.set_desired_location(target_loc);
                 if (!is_zero(target_speed)) {
-                    rover.guided_control.target_speed = target_speed;
+                    rover.mode_guided.set_desired_speed(target_speed);
                 }
             } else if (pos_ignore && !vel_ignore && acc_ignore) {
-                rover.set_guided_velocity(target_steer_speed, target_speed);
+                rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, target_speed);
             } else if (!pos_ignore && vel_ignore && acc_ignore) {
-                rover.set_guided_WP(target_loc);
+                rover.mode_guided.set_desired_location(target_loc);
             } else {
                 // result = MAV_RESULT_FAILED;  // TODO : support that
             }

APMrover2/Rover.h

@@ -395,14 +395,6 @@ private:
     // time that rudder/steering arming has been running
     uint32_t rudder_arm_timer;
 
-    // Store parameters from Guided msg
-    struct {
-      float turn_angle;          // target heading in centi-degrees
-      float target_speed;        // target speed in m/s
-      float target_steer_speed;  // target steer speed in degree/s
-      uint32_t msg_time_ms;      // time of last guided message
-    } guided_control;
-
     // Store the time the last GPS message was received.
     uint32_t last_gps_msg_ms{0};
 
@@ -488,11 +480,9 @@ private:
     void Log_Arm_Disarm();
 
     void load_parameters(void);
-    bool use_pivot_steering(void);
+    bool use_pivot_steering(float yaw_error_cd);
     void set_servos(void);
     void set_auto_WP(const struct Location& loc);
-    void set_guided_WP(const struct Location& loc);
-    void set_guided_velocity(float target_steer_speed, float target_speed);
     void update_home_from_EKF();
     bool set_home_to_current_location(bool lock);
     bool set_home(const Location& loc, bool lock);
@@ -576,9 +566,7 @@ private:
     bool motor_active();
     void update_home();
     void accel_cal_update(void);
-    void nav_set_yaw_speed();
     bool do_yaw_rotation();
-    void nav_set_speed();
     bool in_stationary_loiter(void);
     void crash_check();
 #if ADVANCED_FAILSAFE == ENABLED

APMrover2/Steering.cpp

@@ -3,7 +3,7 @@
 /*
     work out if we are going to use pivot steering
 */
-bool Rover::use_pivot_steering(void)
+bool Rover::use_pivot_steering(float yaw_error_cd)
 {
     // check cases where we clearly cannot use pivot steering
     if (control_mode->is_autopilot_mode() || !g2.motors.have_skid_steering() || g.pivot_turn_angle <= 0) {
@@ -12,16 +12,16 @@ bool Rover::use_pivot_steering(void)
     }
 
     // calc bearing error
-    const int16_t bearing_error = wrap_180_cd(nav_controller->target_bearing_cd() - ahrs.yaw_sensor) / 100;
+    const float yaw_error = yaw_error_cd / 100.0f;
 
     // if error is larger than pivot_turn_angle start pivot steering
-    if (bearing_error > g.pivot_turn_angle) {
+    if (yaw_error > g.pivot_turn_angle) {
         pivot_steering_active = true;
         return true;
     }
 
     // if within 10 degrees of the target heading, exit pivot steering
-    if (bearing_error < 10) {
+    if (yaw_error < 10.0f) {
         pivot_steering_active = false;
         return false;
     }

APMrover2/commands.cpp

@@ -27,38 +27,6 @@ void Rover::set_auto_WP(const struct Location& loc)
     wp_distance      = wp_totalDistance;
 }
 
-void Rover::set_guided_WP(const struct Location& loc)
-{
-    rover.mode_guided.guided_mode = ModeGuided::Guided_WP;
-    // copy the current location into the OldWP slot
-    // ---------------------------------------
-    prev_WP = current_loc;
-
-    // Load the next_WP slot
-    // ---------------------
-    next_WP = loc;
-    rover.guided_control.target_speed = g.speed_cruise;
-    // this is handy for the groundstation
-    wp_totalDistance = get_distance(current_loc, next_WP);
-    wp_distance      = wp_totalDistance;
-
-    rover.rtl_complete = false;
-}
-
-void Rover::set_guided_velocity(float target_steer_speed, float target_speed)
-{
-    rover.mode_guided.guided_mode = ModeGuided::Guided_Velocity;
-    rover.guided_control.target_steer_speed = target_steer_speed;
-    rover.guided_control.target_speed = target_speed;
-
-    next_WP = current_loc;
-    // this is handy for the groundstation
-    wp_totalDistance = 0;
-    wp_distance      = 0.0f;
-
-    rover.rtl_complete = false;
-}
-
 // checks if we should update ahrs home position from the EKF's position
 void Rover::update_home_from_EKF()
 {
@@ -118,9 +86,6 @@ bool Rover::set_home(const Location& loc, bool lock)
 
         // initialise navigation to home
         next_WP = prev_WP = home;
-
-        // Load home for a default guided_WP
-        set_guided_WP(home);
     }
 
     // lock home position

APMrover2/commands_logic.cpp

@@ -360,53 +360,6 @@ bool Rover::verify_loiter_time(const AP_Mission::Mission_Command& cmd)
     return result;
 }
 
-void Rover::nav_set_yaw_speed()
-{
-    // if we haven't received a MAV_CMD_NAV_SET_YAW_SPEED message within the last 3 seconds bring the rover to a halt
-    if ((millis() - guided_control.msg_time_ms) > 3000) {
-        gcs().send_text(MAV_SEVERITY_WARNING, "NAV_SET_YAW_SPEED not recvd last 3secs, stopping");
-        g2.motors.set_throttle(g.throttle_min.get());
-        g2.motors.set_steering(0.0f);
-        return;
-    }
-
-    const int32_t steering = steerController.get_steering_out_angle_error(guided_control.turn_angle);
-    g2.motors.set_steering(steering);
-
-    // speed param in the message gives speed as a proportion of cruise speed.
-    // 0.5 would set speed to the cruise speed
-    // 1 is double the cruise speed.
-    const float target_speed = g.speed_cruise * guided_control.target_speed * 2.0f;
-    rover.control_mode->calc_throttle(target_speed);
-
-    Log_Write_GuidedTarget(rover.mode_guided.guided_mode, Vector3f(steering, 0.0f, 0.0f), Vector3f(target_speed, 0.0f, 0.0f));
-}
-
-void Rover::nav_set_speed()
-{
-    // if we haven't received a MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED message within the last 3 seconds bring the rover to a halt
-    if ((millis() - guided_control.msg_time_ms) > 3000) {
-        gcs().send_text(MAV_SEVERITY_WARNING, "SET_VELOCITY not recvd last 3secs, stopping");
-        g2.motors.set_throttle(g.throttle_min.get());
-        g2.motors.set_steering(0.0f);
-        prev_WP = current_loc;
-        next_WP = current_loc;
-        set_guided_WP(current_loc);  // exit Guided_Velocity to prevent spam
-        return;
-    }
-    prev_WP = current_loc;
-    next_WP = current_loc;
-
-    const int32_t steer_value = steerController.get_steering_out_rate(guided_control.target_steer_speed);
-    location_update(next_WP, (steer_value + ahrs.yaw_sensor) * 0.01f, 4.0f);  // put the next wp at 4m forward at steer direction
-    nav_controller->update_waypoint(current_loc, next_WP);
-
-    g2.motors.set_steering(steer_value);
-    rover.control_mode->calc_throttle(guided_control.target_speed);
-
-    Log_Write_GuidedTarget(rover.mode_guided.guided_mode, Vector3f(steer_value, 0.0f, 0.0f), Vector3f(guided_control.target_speed, 0.0f, 0.0f));
-}
-
 /********************************************************************************/
 //  Condition (May) commands
 /********************************************************************************/