ROVER: put all guided param in one structure

APMrover2/APMrover2.cpp

@@ -474,9 +474,9 @@ void Rover::update_current_mode(void)
             } else {
                 calc_lateral_acceleration();
                 calc_nav_steer();
-                calc_throttle(guided_target_speed);
+                calc_throttle(rover.guided_control.target_speed);
                 Log_Write_GuidedTarget(guided_mode, Vector3f(next_WP.lat, next_WP.lng, next_WP.alt),
-                                       Vector3f(guided_target_speed, SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), 0));
+                                       Vector3f(rover.guided_control.target_speed, SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), 0));
             }
             break;
 

APMrover2/GCS_Mavlink.cpp

@@ -1083,9 +1083,9 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             }
 
             rover.guided_mode = Guided_Angle;
-            rover.guided_yaw_speed.msg_time_ms = AP_HAL::millis();
-            rover.guided_yaw_speed.turn_angle = packet.param1;
-            rover.guided_yaw_speed.target_speed = constrain_float(packet.param2, 0.0f, 1.0f);
+            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();
             result = MAV_RESULT_ACCEPTED;
             break;
@@ -1252,7 +1252,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
                 break;
             }
             // record the time when the last message arrived
-            rover.guided_yaw_speed.msg_time_ms = AP_HAL::millis();
+            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) {
@@ -1306,7 +1306,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
 
             // record the time when the last message arrived
-            rover.guided_yaw_speed.msg_time_ms = AP_HAL::millis();
+            rover.guided_control.msg_time_ms = AP_HAL::millis();
 
             // prepare and send target position
             Location target_loc = rover.current_loc;
@@ -1355,7 +1355,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             if (!pos_ignore && !vel_ignore && acc_ignore) {
                 rover.set_guided_WP(target_loc);
                 if (!is_zero(target_speed)) {
-                    rover.guided_target_speed = target_speed;
+                    rover.guided_control.target_speed = target_speed;
                 }
             } else if (pos_ignore && !vel_ignore && acc_ignore) {
                 rover.set_guided_velocity(target_steer_speed, target_speed);
@@ -1391,7 +1391,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
 
             // record the time where the last message arrived
-            rover.guided_yaw_speed.msg_time_ms = AP_HAL::millis();
+            rover.guided_control.msg_time_ms = AP_HAL::millis();
 
             // prepare and send target position
             Location target_loc = rover.current_loc;
@@ -1417,7 +1417,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             if (!pos_ignore && !vel_ignore && acc_ignore) {
                 rover.set_guided_WP(target_loc);
                 if (!is_zero(target_speed)) {
-                    rover.guided_target_speed = target_speed;
+                    rover.guided_control.target_speed = target_speed;
                 }
             } else if (pos_ignore && !vel_ignore && acc_ignore) {
                 rover.set_guided_velocity(target_steer_speed, target_speed);

APMrover2/Rover.h

@@ -391,19 +391,17 @@ private:
 
     // Guided control
     GuidedMode guided_mode;             // controls which controller is run (waypoint or velocity)
-    float guided_target_steer_speed;   // target heading in centi-degrees
-    float guided_target_speed;          // target speed in m/s
+    // 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};
 
-    // Store parameters from NAV_SET_YAW_SPEED
-    struct {
-        float turn_angle;
-        float target_speed;
-        uint32_t msg_time_ms;
-    } guided_yaw_speed;
-
 private:
     // private member functions
     void ahrs_update();

APMrover2/commands.cpp

@@ -47,7 +47,7 @@ void Rover::set_guided_WP(const struct Location& loc)
     // Load the next_WP slot
     // ---------------------
     next_WP = loc;
-    guided_target_speed = g.speed_cruise;
+    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;
@@ -58,8 +58,8 @@ void Rover::set_guided_WP(const struct Location& loc)
 void Rover::set_guided_velocity(float target_steer_speed, float target_speed)
 {
     guided_mode = Guided_Velocity;
-    guided_target_steer_speed = target_steer_speed;
-    guided_target_speed = target_speed;
+    rover.guided_control.target_steer_speed = target_steer_speed;
+    rover.guided_control.target_speed = target_speed;
 
     next_WP = current_loc;
     lateral_acceleration = 0.0f;

APMrover2/commands_logic.cpp

@@ -375,7 +375,7 @@ bool Rover::verify_loiter_time(const AP_Mission::Mission_Command& cmd)
 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_yaw_speed.msg_time_ms) > 3000) {
+    if ((millis() - guided_control.msg_time_ms) > 3000) {
         gcs_send_text(MAV_SEVERITY_WARNING, "NAV_SET_YAW_SPEED not recvd last 3secs, stopping");
         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
         SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
@@ -383,13 +383,13 @@ void Rover::nav_set_yaw_speed()
         return;
     }
 
-    const int32_t steering = steerController.get_steering_out_angle_error(guided_yaw_speed.turn_angle);
+    const int32_t steering = steerController.get_steering_out_angle_error(guided_control.turn_angle);
     SRV_Channels::set_output_scaled(SRV_Channel::k_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.
-    float target_speed = g.speed_cruise * guided_yaw_speed.target_speed * 2;
+    float target_speed = g.speed_cruise * guided_control.target_speed * 2;
     calc_throttle(target_speed);
 
     Log_Write_GuidedTarget(guided_mode, Vector3f(steering, 0, 0), Vector3f(target_speed, 0, 0));
@@ -399,7 +399,7 @@ void Rover::nav_set_yaw_speed()
 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_yaw_speed.msg_time_ms) > 3000) {
+    if ((millis() - guided_control.msg_time_ms) > 3000) {
         gcs_send_text(MAV_SEVERITY_WARNING, "SET_VELOCITY not recvd last 3secs, stopping");
         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_min.get());
         SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
@@ -412,14 +412,14 @@ void Rover::nav_set_speed()
     prev_WP = current_loc;
     next_WP = current_loc;
 
-    const int32_t steer_value = steerController.get_steering_out_rate(guided_target_steer_speed);
+    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);
 
     SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steer_value);
-    calc_throttle(guided_target_speed);
+    calc_throttle(guided_control.target_speed);
 
-    Log_Write_GuidedTarget(guided_mode, Vector3f(guided_target_steer_speed, 0.0f, 0.0f), Vector3f(guided_target_speed, 0.0f, 0.0f));
+    Log_Write_GuidedTarget(guided_mode, Vector3f(steer_value, 0.0f, 0.0f), Vector3f(guided_control.target_speed, 0.0f, 0.0f));
 }
 
 /********************************************************************************/