Rover: replace do-yaw with nav-set-yaw-speed

APMrover2/Rover.cpp

@@ -45,7 +45,6 @@ Rover::Rover(void) :
 #if FRSKY_TELEM_ENABLED == ENABLED
     frsky_telemetry(ahrs, battery, rangefinder),
 #endif
-    do_auto_rotation(false),
     home(ahrs.get_home()),
     G_Dt(0.02f)
 {

APMrover2/Rover.h

@@ -324,9 +324,6 @@ private:
     // For example in a delay command the condition_start records that start time for the delay
     int32_t condition_start;
 
-    // Use for stopping navigation in auto mode and do rotation on spot.
-    bool do_auto_rotation;
-
     // 3D Location vectors
     // Location structure defined in AP_Common
     // The home location used for RTL.  The location is set when we first get stable GPS lock
@@ -493,7 +490,7 @@ private:
     bool verify_RTL();
     bool verify_wait_delay();
     bool verify_within_distance();
-    bool verify_yaw();
+    bool verify_nav_set_yaw_speed();
     void update_commands(void);
     void delay(uint32_t ms);
     void mavlink_delay(uint32_t ms);
@@ -550,7 +547,7 @@ private:
     bool verify_loiter_time(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);
-    void do_yaw(const AP_Mission::Mission_Command& cmd);
+    void do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd);
     void do_change_speed(const AP_Mission::Mission_Command& cmd);
     void do_set_home(const AP_Mission::Mission_Command& cmd);
 #if CAMERA == ENABLED
@@ -566,7 +563,6 @@ private:
     bool motor_active();
     void update_home();
     void accel_cal_update(void);
-    bool do_yaw_rotation();
     bool in_stationary_loiter(void);
     void crash_check();
 #if ADVANCED_FAILSAFE == ENABLED

APMrover2/commands_logic.cpp

@@ -32,6 +32,10 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd)
         do_loiter_time(cmd);
         break;
 
+    case MAV_CMD_NAV_SET_YAW_SPEED:
+        do_nav_set_yaw_speed(cmd);
+        break;
+
     // Conditional commands
     case MAV_CMD_CONDITION_DELAY:
         do_wait_delay(cmd);
@@ -41,10 +45,6 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd)
         do_within_distance(cmd);
         break;
 
-    case MAV_CMD_CONDITION_YAW:             // 115
-        do_yaw(cmd);
-        break;
-
     // Do commands
     case MAV_CMD_DO_CHANGE_SPEED:
         do_change_speed(cmd);
@@ -173,8 +173,8 @@ bool Rover::verify_command(const AP_Mission::Mission_Command& cmd)
     case MAV_CMD_CONDITION_DISTANCE:
         return verify_within_distance();
 
-    case MAV_CMD_CONDITION_YAW:
+    case MAV_CMD_NAV_SET_YAW_SPEED:
-        return verify_yaw();
+        return verify_nav_set_yaw_speed();
 
     // do commands (always return true)
     case MAV_CMD_DO_CHANGE_SPEED:
@@ -243,6 +243,24 @@ void Rover::do_loiter_time(const AP_Mission::Mission_Command& cmd)
     do_nav_wp(cmd);
 }
 
+// do_set_yaw_speed - turn to a specified heading and achieve and given speed
+void Rover::do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd)
+{
+    float desired_heading_cd;
+
+    // get final angle, 1 = Relative, 0 = Absolute
+    if (cmd.content.set_yaw_speed.relative_angle > 0) {
+        // relative angle
+        desired_heading_cd = wrap_180_cd(ahrs.yaw_sensor + cmd.content.set_yaw_speed.angle_deg * 100.0f);
+    } else {
+        // absolute angle
+        desired_heading_cd = cmd.content.set_yaw_speed.angle_deg * 100.0f;
+    }
+
+    // set auto target
+    mode_auto.set_desired_heading_and_speed(desired_heading_cd, constrain_float(cmd.content.set_yaw_speed.speed, -g.speed_cruise, g.speed_cruise));
+}
+
 /********************************************************************************/
 //  Verify Nav (Must) commands
 /********************************************************************************/
@@ -360,6 +378,12 @@ bool Rover::verify_loiter_time(const AP_Mission::Mission_Command& cmd)
     return result;
 }
 
+// verify_yaw - return true if we have reached the desired heading
+bool Rover::verify_nav_set_yaw_speed()
+{
+    return mode_auto.reached_heading();
+}
+
 /********************************************************************************/
 //  Condition (May) commands
 /********************************************************************************/
@@ -375,59 +399,6 @@ void Rover::do_within_distance(const AP_Mission::Mission_Command& cmd)
     condition_value = cmd.content.distance.meters;
 }
 
-void Rover::do_yaw(const AP_Mission::Mission_Command& cmd)
-{
-    // Only support target yaw for now
-    condition_start = condition_value;  // save condition_value from current navigation wp loaded
-    // get final angle, 1 = Relative, 0 = Absolute
-    if (cmd.content.yaw.relative_angle == 0) {
-        // absolute angle
-        condition_value = cmd.content.yaw.angle_deg * 100;
-    } else {
-        // relative angle
-        condition_value = cmd.content.yaw.angle_deg * 100;
-        if (cmd.content.yaw.direction < 0) {
-            condition_value = -condition_value;
-        }
-
-        condition_value = condition_value + ahrs.yaw_sensor;
-    }
-
-    // absolute angle error
-    const int32_t error_to_target_yaw = abs((condition_value - ahrs.yaw_sensor));
-
-    // Calculate the steering to apply base on error calculated
-    const int32_t steering = steerController.get_steering_out_angle_error(error_to_target_yaw);
-    g2.motors.set_steering(steering);
-    next_navigation_leg_cd = condition_value;
-    control_mode->calc_throttle(g.speed_cruise);
-
-    do_auto_rotation = true;
-}
-
-bool Rover::do_yaw_rotation()
-{
-    // absolute angle error
-    const int32_t error_to_target_yaw = abs(condition_value - ahrs.yaw_sensor);
-
-    // check if we are within 5 degrees of the target heading
-    if (error_to_target_yaw <= 500) {
-        g2.motors.set_steering(0.0f);  // stop the current rotation
-        condition_value = condition_start;  // reset the condition value to its previous value
-        g2.motors.set_throttle(0.0f);
-        next_navigation_leg_cd = mission.get_next_ground_course_cd(0);
-        do_auto_rotation = false;
-        return true;
-    } else {
-        // Calculate the steering to apply base on error calculated
-        const int32_t steering = steerController.get_steering_out_angle_error(error_to_target_yaw);
-        g2.motors.set_steering(steering);
-        control_mode->calc_throttle(g.speed_cruise);
-        do_auto_rotation = true;
-        return false;
-    }
-}
-
 /********************************************************************************/
 // Verify Condition (May) commands
 /********************************************************************************/
@@ -450,16 +421,6 @@ bool Rover::verify_within_distance()
     return false;
 }
 
-// verify_yaw - return true if we have reached the desired heading
-bool Rover::verify_yaw()
-{
-    // override by do_yaw_rotation()
-    if (do_auto_rotation) {
-        return false;
-    } else {
-        return true;
-    }
-}
 
 /********************************************************************************/
 //  Do (Now) commands