Rover: add Guided velocity controller

APMrover2/APMrover2.cpp

@@ -474,12 +474,16 @@ void Rover::update_current_mode(void)
             } else {
                 calc_lateral_acceleration();
                 calc_nav_steer();
-                calc_throttle(g.speed_cruise);
+                calc_throttle(guided_target_speed);
-                Log_Write_GuidedTarget(guided_mode, Vector3f(guided_WP.lat, guided_WP.lng, guided_WP.alt),
+                Log_Write_GuidedTarget(guided_mode, Vector3f(next_WP.lat, next_WP.lng, next_WP.alt),
-                                       Vector3f(g.speed_cruise, SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), 0));
+                                       Vector3f(guided_target_speed, SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), 0));
             }
             break;
 
+        case Guided_Velocity:
+            nav_set_speed();
+            break;
+
         default:
             gcs_send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
             break;
@@ -591,6 +595,10 @@ void Rover::update_navigation()
             }
             break;
 
+        case Guided_Velocity:
+            nav_set_speed();
+            break;
+
         default:
             gcs_send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode");
             break;

APMrover2/GCS_Mavlink.cpp

@@ -1241,6 +1241,47 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             break;
         }
 
+    case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:   // MAV ID: 82
+        {
+            // decode packet
+            mavlink_set_attitude_target_t packet;
+            mavlink_msg_set_attitude_target_decode(msg, &packet);
+
+            // exit if vehicle is not in Guided mode
+            if (rover.control_mode != GUIDED) {
+                break;
+            }
+            // record the time when the last message arrived
+            rover.guided_yaw_speed.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) {
+                break;
+            }
+
+            // convert thrust to ground speed
+            packet.thrust = constrain_float(packet.thrust, 0.0f, 1.0f);
+            float target_speed = 0.0f;
+            if (is_equal(packet.thrust, 0.5f)) {
+                target_speed = 0.0f;
+            } else if (packet.thrust > 0.5f) {
+                target_speed = (packet.thrust - 0.5f) * 2.0f * rover.g.speed_cruise;
+            } else {
+                target_speed = (0.5f - packet.thrust) * 2.0f * rover.g.speed_cruise;
+            }
+
+            // if the body_yaw_rate field is ignored, use the commanded yaw position
+            // otherwise use the commanded yaw rate
+            if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_YAW_RATE_IGNORE) != 0) {
+                // convert quaternion to heading
+                // 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);
+            }
+            break;
+        }
+
     case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:     // MAV ID: 84
         {
             // decode packet
@@ -1261,10 +1302,15 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             }
 
             bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
+            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_yaw_speed.msg_time_ms = AP_HAL::millis();
 
             // prepare and send target position
+            Location target_loc = rover.current_loc;
             if (!pos_ignore) {
-                Location loc = rover.current_loc;
                 switch (packet.coordinate_frame) {
                 case MAV_FRAME_BODY_NED:
                 case MAV_FRAME_BODY_OFFSET_NED: {
@@ -1272,25 +1318,52 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
                     float ne_x = packet.x*rover.ahrs.cos_yaw() - packet.y*rover.ahrs.sin_yaw();
                     float ne_y = packet.x*rover.ahrs.sin_yaw() + packet.y*rover.ahrs.cos_yaw();
                     // add offset to current location
-                    location_offset(loc, ne_x, ne_y);
+                    location_offset(target_loc, ne_x, ne_y);
                     }
                     break;
 
                 case MAV_FRAME_LOCAL_OFFSET_NED:
                     // add offset to current location
-                    location_offset(loc, packet.x, packet.y);
+                    location_offset(target_loc, packet.x, packet.y);
                     break;
 
                 default:
                     // MAV_FRAME_LOCAL_NED interpret as an offset from home
-                    loc = rover.ahrs.get_home();
+                    target_loc = rover.ahrs.get_home();
-                    location_offset(loc, packet.x, packet.y);
+                    location_offset(target_loc, packet.x, packet.y);
                     break;
                 }
-
+            }
-                rover.set_guided_WP(loc);
+            float target_speed = 0.0f;
+            float target_steer_speed = 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;
+                float vely = packet.vy;
+                // rotate to body-frame if necessary
+                if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
+                    // rotate from body-frame to NE frame
+                    velx = velx * rover.ahrs.cos_yaw() - vely * rover.ahrs.sin_yaw();
+                    vely = velx * rover.ahrs.sin_yaw() + vely * rover.ahrs.cos_yaw();
+                }
+                target_speed = vely;
+                target_steer_speed = 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);
+                if (!is_zero(target_speed)) {
+                    rover.guided_target_speed = target_speed;
+                }
+            } else if (pos_ignore && !vel_ignore && acc_ignore) {
+                rover.set_guided_velocity(target_steer_speed, target_speed);
+            } else if (!pos_ignore && vel_ignore && acc_ignore) {
+                rover.set_guided_WP(target_loc);
+            } else {
+                // result = MAV_RESULT_FAILED;  // TODO : support that
+            }
             break;
         }
 
@@ -1314,15 +1387,45 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             }
 
             bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
+            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_yaw_speed.msg_time_ms = AP_HAL::millis();
 
             // prepare and send target position
+            Location target_loc = rover.current_loc;
             if (!pos_ignore) {
-                Location loc = rover.current_loc;
+                // sanity check location
-                loc.lat = packet.lat_int;
+                if (!check_latlng(packet.lat_int, packet.lon_int)) {
-                loc.lng = packet.lon_int;
+                    // result = MAV_RESULT_FAILED;
-                rover.set_guided_WP(loc);
+                    break;
+                }
+                target_loc.lat = packet.lat_int;
+                target_loc.lng = packet.lon_int;
+            }
+            float target_speed = 0.0f;
+            float target_steer_speed = 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;
+                // TODO : take into account reverse speed
+                // TODO : handle yaw heading cmd
             }
 
+            if (!pos_ignore && !vel_ignore && acc_ignore) {
+                rover.set_guided_WP(target_loc);
+                if (!is_zero(target_speed)) {
+                    rover.guided_target_speed = target_speed;
+                }
+            } else if (pos_ignore && !vel_ignore && acc_ignore) {
+                rover.set_guided_velocity(target_steer_speed, target_speed);
+            } else if (!pos_ignore && vel_ignore && acc_ignore) {
+                rover.set_guided_WP(target_loc);
+            } else {
+                // result = MAV_RESULT_FAILED;  // TODO : support that
+            }
             break;
         }
 

APMrover2/Rover.h

@@ -389,6 +389,11 @@ private:
     // we need to run the speed controller
     bool auto_throttle_mode;
 
+    // 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 the time the last GPS message was received.
     uint32_t last_gps_msg_ms{0};
 
@@ -399,9 +404,6 @@ private:
         uint32_t msg_time_ms;
     } guided_yaw_speed;
 
-    // Guided
-    GuidedMode guided_mode;  // stores which GUIDED mode the vehicle is in
-
 private:
     // private member functions
     void ahrs_update();
@@ -471,6 +473,7 @@ private:
     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 init_home();
     void restart_nav();
     void exit_mission();
@@ -568,6 +571,7 @@ private:
     void accel_cal_update(void);
     void nav_set_yaw_speed();
     bool do_yaw_rotation();
+    void nav_set_speed();
     bool in_stationary_loiter(void);
     void set_loiter_active(const AP_Mission::Mission_Command& cmd);
     void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);

APMrover2/Steering.cpp

@@ -129,7 +129,7 @@ void Rover::calc_throttle(float target_speed) {
 
     float reduction = 1.0f - steer_rate * speed_turn_reduction;
 
-    if (control_mode >= AUTO && wp_distance <= g.speed_turn_dist) {
+    if (control_mode >= AUTO && guided_mode != Guided_Velocity && wp_distance <= g.speed_turn_dist) {
         // in auto-modes we reduce speed when approaching waypoints
         const float reduction2 = 1.0f - speed_turn_reduction;
         if (reduction2 < reduction) {
@@ -171,10 +171,12 @@ void Rover::calc_throttle(float target_speed) {
         set_reverse(true);
     }
 
+    if (guided_mode != Guided_Velocity) {
         if (use_pivot_steering()) {
             SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
         }
     }
+}
 
 /*****************************************
     Calculate desired turn angles (in medium freq loop)

APMrover2/commands.cpp

@@ -39,6 +39,7 @@ void Rover::set_auto_WP(const struct Location& loc)
 
 void Rover::set_guided_WP(const struct Location& loc)
 {
+    guided_mode = Guided_WP;
     // copy the current location into the OldWP slot
     // ---------------------------------------
     prev_WP = current_loc;
@@ -46,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;
     // this is handy for the groundstation
     wp_totalDistance = get_distance(current_loc, next_WP);
     wp_distance      = wp_totalDistance;
@@ -54,6 +55,21 @@ void Rover::set_guided_WP(const struct Location& loc)
     rover.rtl_complete = false;
 }
 
+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;
+
+    next_WP = current_loc;
+    lateral_acceleration = 0.0f;
+    // this is handy for the groundstation
+    wp_totalDistance = 0;
+    wp_distance      = 0.0f;
+
+    rover.rtl_complete = false;
+}
+
 // run this at setup on the ground
 // -------------------------------
 void Rover::init_home()

APMrover2/commands_logic.cpp

@@ -396,6 +396,32 @@ void Rover::nav_set_yaw_speed()
     return;
 }
 
+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) {
+        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);
+        lateral_acceleration = 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_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);
+
+    Log_Write_GuidedTarget(guided_mode, Vector3f(guided_target_steer_speed, 0.0f, 0.0f), Vector3f(guided_target_speed, 0.0f, 0.0f));
+}
+
 /********************************************************************************/
 //  Condition (May) commands
 /********************************************************************************/

APMrover2/defines.h

@@ -42,10 +42,10 @@ enum mode {
 
 enum GuidedMode {
     Guided_WP,
-    Guided_Angle
+    Guided_Angle,
+    Guided_Velocity
 };
 
-
 // types of failsafe events
 #define FAILSAFE_EVENT_THROTTLE (1<<0)
 #define FAILSAFE_EVENT_GCS      (1<<1)
@@ -92,6 +92,13 @@ enum GuidedMode {
 #define MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE      (1<<10)
 #define MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE (1<<11)
 
+// for mavlink SET_ATTITUDE_TARGET messages
+#define MAVLINK_SET_ATT_TYPE_MASK_ROLL_RATE_IGNORE     (1<<0)
+#define MAVLINK_SET_ATT_TYPE_MASK_PITCH_RATE_IGNORE    (1<<1)
+#define MAVLINK_SET_ATT_TYPE_MASK_YAW_RATE_IGNORE      (1<<2)
+#define MAVLINK_SET_ATT_TYPE_MASK_THROTTLE_IGNORE      (1<<6)
+#define MAVLINK_SET_ATT_TYPE_MASK_ATTITUDE_IGNORE      (1<<7)
+
 // Error message sub systems and error codes
 #define ERROR_SUBSYSTEM_CRASH_CHECK  12
 // subsystem specific error codes -- crash checker