Sub: integrate s-curves, remove spline support

ArduSub/Sub.h

@@ -463,7 +463,6 @@ private:
     void auto_wp_start(const Vector3f& destination);
     void auto_wp_start(const Location& dest_loc);
     void auto_wp_run();
-    void auto_spline_run();
     void auto_circle_movetoedge_start(const Location &circle_center, float radius_m);
     void auto_circle_start();
     void auto_circle_run();
@@ -577,7 +576,6 @@ private:
     void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
     void do_circle(const AP_Mission::Mission_Command& cmd);
     void do_loiter_time(const AP_Mission::Mission_Command& cmd);
-    void do_spline_wp(const AP_Mission::Mission_Command& cmd);
 #if NAV_GUIDED == ENABLED
     void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
     void do_guided_limits(const AP_Mission::Mission_Command& cmd);
@@ -595,13 +593,11 @@ private:
     bool verify_surface(const AP_Mission::Mission_Command& cmd);
     bool verify_RTL(void);
     bool verify_circle(const AP_Mission::Mission_Command& cmd);
-    bool verify_spline_wp(const AP_Mission::Mission_Command& cmd);
 #if NAV_GUIDED == ENABLED
     bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd);
 #endif
     bool verify_nav_delay(const AP_Mission::Mission_Command& cmd);
 
-    void auto_spline_start(const Location& destination, bool stopped_at_start, AC_WPNav::spline_segment_end_type seg_end_type, const Location& next_destination);
     void log_init(void);
     void accel_cal_update(void);
     void read_airspeed();

ArduSub/commands_logic.cpp

@@ -55,10 +55,6 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
         do_loiter_time(cmd);
         break;
 
-    case MAV_CMD_NAV_SPLINE_WAYPOINT:           // 82  Navigate to Waypoint using spline
-        do_spline_wp(cmd);
-        break;
-
 #if NAV_GUIDED == ENABLED
     case MAV_CMD_NAV_GUIDED_ENABLE:             // 92  accept navigation commands from external nav computer
         do_nav_guided_enable(cmd);
@@ -167,9 +163,6 @@ bool Sub::verify_command(const AP_Mission::Mission_Command& cmd)
     case MAV_CMD_NAV_LOITER_TIME:
         return verify_loiter_time();
 
-    case MAV_CMD_NAV_SPLINE_WAYPOINT:
-        return verify_spline_wp(cmd);
-
 #if NAV_GUIDED == ENABLED
     case MAV_CMD_NAV_GUIDED_ENABLE:
         return verify_nav_guided_enable(cmd);
@@ -252,11 +245,6 @@ void Sub::do_nav_wp(const AP_Mission::Mission_Command& cmd)
 
     // Set wp navigation target
     auto_wp_start(target_loc);
-
-    // if no delay set the waypoint as "fast"
-    if (loiter_time_max == 0) {
-        wp_nav.set_fast_waypoint(true);
-    }
 }
 
 // do_surface - initiate surface procedure
@@ -383,79 +371,6 @@ void Sub::do_loiter_time(const AP_Mission::Mission_Command& cmd)
     loiter_time_max = cmd.p1;     // units are (seconds)
 }
 
-// do_spline_wp - initiate move to next waypoint
-void Sub::do_spline_wp(const AP_Mission::Mission_Command& cmd)
-{
-    Location target_loc(cmd.content.location);
-    // use current lat, lon if zero
-    if (target_loc.lat == 0 && target_loc.lng == 0) {
-        target_loc.lat = current_loc.lat;
-        target_loc.lng = current_loc.lng;
-    }
-    // use current altitude if not provided
-    if (target_loc.alt == 0) {
-        // set to current altitude but in command's alt frame
-        int32_t curr_alt;
-        if (current_loc.get_alt_cm(target_loc.get_alt_frame(),curr_alt)) {
-            target_loc.set_alt_cm(curr_alt, target_loc.get_alt_frame());
-        } else {
-            // default to current altitude as alt-above-home
-            target_loc.set_alt_cm(current_loc.alt, current_loc.get_alt_frame());
-        }
-    }
-
-    // this will be used to remember the time in millis after we reach or pass the WP.
-    loiter_time = 0;
-    // this is the delay, stored in seconds
-    loiter_time_max = cmd.p1;
-
-    // determine segment start and end type
-    bool stopped_at_start = true;
-    AC_WPNav::spline_segment_end_type seg_end_type = AC_WPNav::SEGMENT_END_STOP;
-    AP_Mission::Mission_Command temp_cmd;
-
-    // if previous command was a wp_nav command with no delay set stopped_at_start to false
-    // To-Do: move processing of delay into wp-nav controller to allow it to determine the stopped_at_start value itself?
-    uint16_t prev_cmd_idx = mission.get_prev_nav_cmd_index();
-    if (prev_cmd_idx != AP_MISSION_CMD_INDEX_NONE) {
-        if (mission.read_cmd_from_storage(prev_cmd_idx, temp_cmd)) {
-            if ((temp_cmd.id == MAV_CMD_NAV_WAYPOINT || temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT) && temp_cmd.p1 == 0) {
-                stopped_at_start = false;
-            }
-        }
-    }
-
-    // if there is no delay at the end of this segment get next nav command
-    Location next_loc;
-    if (cmd.p1 == 0 && mission.get_next_nav_cmd(cmd.index+1, temp_cmd)) {
-        next_loc = temp_cmd.content.location;
-        // default lat, lon to first waypoint's lat, lon
-        if (next_loc.lat == 0 && next_loc.lng == 0) {
-            next_loc.lat = target_loc.lat;
-            next_loc.lng = target_loc.lng;
-        }
-        // default alt to first waypoint's alt but in next waypoint's alt frame
-        if (next_loc.alt == 0) {
-            int32_t next_alt;
-            if (target_loc.get_alt_cm(next_loc.get_alt_frame(), next_alt)) {
-                next_loc.set_alt_cm(next_alt, next_loc.get_alt_frame());
-            } else {
-                // default to first waypoints altitude
-                next_loc.set_alt_cm(target_loc.alt, target_loc.get_alt_frame());
-            }
-        }
-        // if the next nav command is a waypoint set end type to spline or straight
-        if (temp_cmd.id == MAV_CMD_NAV_WAYPOINT) {
-            seg_end_type = AC_WPNav::SEGMENT_END_STRAIGHT;
-        } else if (temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT) {
-            seg_end_type = AC_WPNav::SEGMENT_END_SPLINE;
-        }
-    }
-
-    // set spline navigation target
-    auto_spline_start(target_loc, stopped_at_start, seg_end_type, next_loc);
-}
-
 #if NAV_GUIDED == ENABLED
 // do_nav_guided_enable - initiate accepting commands from external nav computer
 void Sub::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
@@ -464,7 +379,7 @@ void Sub::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd)
         // initialise guided limits
         guided_limit_init_time_and_pos();
 
-        // set spline navigation target
+        // set navigation target
         auto_nav_guided_start();
     }
 }
@@ -618,28 +533,6 @@ bool Sub::verify_circle(const AP_Mission::Mission_Command& cmd)
     return fabsf(circle_nav.get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1);
 }
 
-// verify_spline_wp - check if we have reached the next way point using spline
-bool Sub::verify_spline_wp(const AP_Mission::Mission_Command& cmd)
-{
-    // check if we have reached the waypoint
-    if (!wp_nav.reached_wp_destination()) {
-        return false;
-    }
-
-    // start timer if necessary
-    if (loiter_time == 0) {
-        loiter_time = AP_HAL::millis();
-    }
-
-    // check if timer has run out
-    if (((AP_HAL::millis() - loiter_time) / 1000) >= loiter_time_max) {
-        gcs().send_text(MAV_SEVERITY_INFO, "Reached command #%i",cmd.index);
-        return true;
-    }
-
-    return false;
-}
-
 #if NAV_GUIDED == ENABLED
 // verify_nav_guided - check if we have breached any limits
 bool Sub::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd)

ArduSub/control_auto.cpp

@@ -23,7 +23,7 @@ bool Sub::auto_init()
         set_auto_yaw_mode(AUTO_YAW_HOLD);
     }
 
-    // initialise waypoint and spline controller
+    // initialise waypoint controller
     wp_nav.wp_and_spline_init();
 
     // clear guided limits
@@ -53,10 +53,6 @@ void Sub::auto_run()
         auto_circle_run();
         break;
 
-    case Auto_Spline:
-        auto_spline_run();
-        break;
-
     case Auto_NavGuided:
 #if NAV_GUIDED == ENABLED
         auto_nav_guided_run();
@@ -94,7 +90,7 @@ void Sub::auto_wp_start(const Location& dest_loc)
     auto_mode = Auto_WP;
 
     // send target to waypoint controller
-    if (!wp_nav.set_wp_destination(dest_loc)) {
+    if (!wp_nav.set_wp_destination_loc(dest_loc)) {
         // failure to set destination can only be because of missing terrain data
         failsafe_terrain_on_event();
         return;
@@ -172,83 +168,6 @@ void Sub::auto_wp_run()
     }
 }
 
-// auto_spline_start - initialises waypoint controller to implement flying to a particular destination using the spline controller
-//  seg_end_type can be SEGMENT_END_STOP, SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE.  If Straight or Spline the next_destination should be provided
-void Sub::auto_spline_start(const Location& destination, bool stopped_at_start,
-                            AC_WPNav::spline_segment_end_type seg_end_type,
-                            const Location& next_destination)
-{
-    auto_mode = Auto_Spline;
-
-    // initialise wpnav
-    if (!wp_nav.set_spline_destination(destination, stopped_at_start, seg_end_type, next_destination)) {
-        // failure to set destination can only be because of missing terrain data
-        failsafe_terrain_on_event();
-        return;
-    }
-
-    // initialise yaw
-    // To-Do: reset the yaw only when the previous navigation command is not a WP.  this would allow removing the special check for ROI
-    if (auto_yaw_mode != AUTO_YAW_ROI) {
-        set_auto_yaw_mode(get_default_auto_yaw_mode(false));
-    }
-}
-
-// auto_spline_run - runs the auto spline controller
-//      called by auto_run at 100hz or more
-void Sub::auto_spline_run()
-{
-    // if not armed set throttle to zero and exit immediately
-    if (!motors.armed()) {
-        // To-Do: reset waypoint origin to current location because vehicle is probably on the ground so we don't want it lurching left or right on take-off
-        //    (of course it would be better if people just used take-off)
-        // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
-        motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
-        attitude_control.set_throttle_out(0,true,g.throttle_filt);
-        attitude_control.relax_attitude_controllers();
-        return;
-    }
-
-    // process pilot's yaw input
-    float target_yaw_rate = 0;
-    if (!failsafe.pilot_input) {
-        // get pilot's desired yaw rat
-        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
-        if (!is_zero(target_yaw_rate)) {
-            set_auto_yaw_mode(AUTO_YAW_HOLD);
-        }
-    }
-
-    // set motors to full range
-    motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
-
-    // run waypoint controller
-    wp_nav.update_spline();
-
-    float lateral_out, forward_out;
-    translate_wpnav_rp(lateral_out, forward_out);
-
-    // Send to forward/lateral outputs
-    motors.set_lateral(lateral_out);
-    motors.set_forward(forward_out);
-
-    // call z-axis position controller (wpnav should have already updated it's alt target)
-    pos_control.update_z_controller();
-
-    // get pilot desired lean angles
-    float target_roll, target_pitch;
-    get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, aparm.angle_max);
-
-    // call attitude controller
-    if (auto_yaw_mode == AUTO_YAW_HOLD) {
-        // roll & pitch from waypoint controller, yaw rate from pilot
-        attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
-    } else {
-        // roll, pitch from waypoint controller, yaw heading from auto_heading()
-        attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, get_auto_heading(), true);
-    }
-}
-
 // auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
 //  we assume the caller has set the circle's circle with circle_nav.set_center()
 //  we assume the caller has performed all required GPS_ok checks
@@ -279,7 +198,7 @@ void Sub::auto_circle_movetoedge_start(const Location &circle_center, float radi
         circle_edge.set_alt_cm(circle_center.alt, circle_center.get_alt_frame());
 
         // initialise wpnav to move to edge of circle
-        if (!wp_nav.set_wp_destination(circle_edge)) {
+        if (!wp_nav.set_wp_destination_loc(circle_edge)) {
             // failure to set destination can only be because of missing terrain data
             failsafe_terrain_on_event();
         }
@@ -362,15 +281,12 @@ bool Sub::auto_loiter_start()
     }
     auto_mode = Auto_Loiter;
 
-    Vector3f origin = inertial_nav.get_position();
-
     // calculate stopping point
     Vector3f stopping_point;
-    pos_control.get_stopping_point_xy(stopping_point);
+    wp_nav.get_wp_stopping_point(stopping_point);
-    pos_control.get_stopping_point_z(stopping_point);
 
     // initialise waypoint controller target to stopping point
-    wp_nav.set_wp_origin_and_destination(origin, stopping_point);
+    wp_nav.set_wp_destination(stopping_point);
 
     // hold yaw at current heading
     set_auto_yaw_mode(AUTO_YAW_HOLD);

ArduSub/control_guided.cpp

@@ -51,7 +51,7 @@ void Sub::guided_pos_control_start()
     // set to position control mode
     guided_mode = Guided_WP;
 
-    // initialise waypoint and spline controller
+    // initialise waypoint controller
     wp_nav.wp_and_spline_init();
 
     // initialise wpnav to stopping point at current altitude
@@ -181,7 +181,7 @@ bool Sub::guided_set_destination(const Location& dest_loc)
     }
 #endif
 
-    if (!wp_nav.set_wp_destination(dest_loc)) {
+    if (!wp_nav.set_wp_destination_loc(dest_loc)) {
         // failure to set destination can only be because of missing terrain data
         AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
         // failure is propagated to GCS with NAK
@@ -386,7 +386,7 @@ void Sub::guided_vel_control_run()
     }
 }
 
-// guided_posvel_control_run - runs the guided spline controller
+// guided_posvel_control_run - runs the guided posvel controller
 // called from guided_run
 void Sub::guided_posvel_control_run()
 {

ArduSub/defines.h

@@ -60,7 +60,6 @@ enum AutoMode {
     Auto_WP,
     Auto_CircleMoveToEdge,
     Auto_Circle,
-    Auto_Spline,
     Auto_NavGuided,
     Auto_Loiter,
     Auto_TerrainRecover