From afa59b9a60b2f44ff49a2ec04b28cf017439f7e6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 10 Jun 2020 09:15:51 +0900 Subject: [PATCH] Sub: integrate s-curves, remove spline support --- ArduSub/Sub.h | 4 -- ArduSub/commands_logic.cpp | 109 +------------------------------------ ArduSub/control_auto.cpp | 94 ++------------------------------ ArduSub/control_guided.cpp | 6 +- ArduSub/defines.h | 1 - 5 files changed, 9 insertions(+), 205 deletions(-) diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 9bb08d4a75..660c321b7f 100644 --- a/ArduSub/Sub.h +++ b/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(); diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index 1f97a86b0f..0fcb05dada 100644 --- a/ArduSub/commands_logic.cpp +++ b/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) diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp index ade69a1a4a..99e377a8c3 100644 --- a/ArduSub/control_auto.cpp +++ b/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); - pos_control.get_stopping_point_z(stopping_point); + wp_nav.get_wp_stopping_point(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); diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp index 91fd19b6ba..b607a96b93 100644 --- a/ArduSub/control_guided.cpp +++ b/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() { diff --git a/ArduSub/defines.h b/ArduSub/defines.h index 1b8777c568..6220addac8 100644 --- a/ArduSub/defines.h +++ b/ArduSub/defines.h @@ -60,7 +60,6 @@ enum AutoMode { Auto_WP, Auto_CircleMoveToEdge, Auto_Circle, - Auto_Spline, Auto_NavGuided, Auto_Loiter, Auto_TerrainRecover