From f7c4810eaa1d6174b48d04c57cbdf366ea715d68 Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Fri, 14 Apr 2017 15:10:29 -0400 Subject: [PATCH] Sub: Remove ignore_check argument from control mode init functions --- ArduSub/Sub.h | 18 ++++++++-------- ArduSub/control_acro.cpp | 2 +- ArduSub/control_althold.cpp | 2 +- ArduSub/control_auto.cpp | 40 +++++++++++++++++------------------ ArduSub/control_circle.cpp | 32 ++++++++++++++-------------- ArduSub/control_guided.cpp | 18 ++++++++-------- ArduSub/control_manual.cpp | 2 +- ArduSub/control_poshold.cpp | 4 ++-- ArduSub/control_stabilize.cpp | 2 +- ArduSub/control_surface.cpp | 2 +- ArduSub/flight_mode.cpp | 19 ++++++++--------- 11 files changed, 70 insertions(+), 71 deletions(-) diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index d5ac190b1c..3893ab668f 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -539,12 +539,12 @@ private: bool verify_wait_delay(); bool verify_within_distance(); bool verify_yaw(); - bool acro_init(bool ignore_checks); + bool acro_init(void); void acro_run(); void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out); - bool althold_init(bool ignore_checks); + bool althold_init(void); void althold_run(); - bool auto_init(bool ignore_checks); + bool auto_init(void); void auto_run(); void auto_wp_start(const Vector3f& destination); void auto_wp_start(const Location_Class& dest_loc); @@ -562,9 +562,9 @@ private: void set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle); void set_auto_yaw_roi(const Location &roi_location); float get_auto_heading(void); - bool circle_init(bool ignore_checks); + bool circle_init(void); void circle_run(); - bool guided_init(bool ignore_checks); + bool guided_init(bool ignore_checks = false); void guided_pos_control_start(); void guided_vel_control_start(); void guided_posvel_control_start(); @@ -584,12 +584,12 @@ private: void guided_limit_init_time_and_pos(); bool guided_limit_check(); - bool poshold_init(bool ignore_checks); + bool poshold_init(void); void poshold_run(); - bool stabilize_init(bool ignore_checks); + bool stabilize_init(void); void stabilize_run(); - bool manual_init(bool ignore_checks); + bool manual_init(void); void manual_run(); void failsafe_crash_check(); void failsafe_ekf_check(void); @@ -732,7 +732,7 @@ private: void translate_circle_nav_rp(float &lateral_out, float &forward_out); void translate_pos_control_rp(float &lateral_out, float &forward_out); - bool surface_init(bool ignore_flags); + bool surface_init(void); void surface_run(); void convert_old_parameters(void); diff --git a/ArduSub/control_acro.cpp b/ArduSub/control_acro.cpp index fa9d3600b1..ca7826428e 100644 --- a/ArduSub/control_acro.cpp +++ b/ArduSub/control_acro.cpp @@ -6,7 +6,7 @@ */ // acro_init - initialise acro controller -bool Sub::acro_init(bool ignore_checks) +bool Sub::acro_init() { // set target altitude to zero for reporting pos_control.set_alt_target(0); diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index 110adc4556..f9767a3994 100644 --- a/ArduSub/control_althold.cpp +++ b/ArduSub/control_althold.cpp @@ -6,7 +6,7 @@ */ // althold_init - initialise althold controller -bool Sub::althold_init(bool ignore_checks) +bool Sub::althold_init() { #if CONFIG_HAL_BOARD != HAL_BOARD_SITL if (!ap.depth_sensor_present) { // can't hold depth without a depth sensor, exit immediately. diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp index 5c03ddfb38..fd6cc38526 100644 --- a/ArduSub/control_auto.cpp +++ b/ArduSub/control_auto.cpp @@ -9,29 +9,29 @@ */ // auto_init - initialise auto controller -bool Sub::auto_init(bool ignore_checks) +bool Sub::auto_init() { - if ((position_ok() && mission.num_commands() > 1) || ignore_checks) { - auto_mode = Auto_Loiter; - - // stop ROI from carrying over from previous runs of the mission - // To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check - if (auto_yaw_mode == AUTO_YAW_ROI) { - set_auto_yaw_mode(AUTO_YAW_HOLD); - } - - // initialise waypoint and spline controller - wp_nav.wp_and_spline_init(); - - // clear guided limits - guided_limit_clear(); - - // start/resume the mission (based on MIS_RESTART parameter) - mission.start_or_resume(); - return true; - } else { + if (!position_ok() || mission.num_commands() < 2) { return false; } + + auto_mode = Auto_Loiter; + + // stop ROI from carrying over from previous runs of the mission + // To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check + if (auto_yaw_mode == AUTO_YAW_ROI) { + set_auto_yaw_mode(AUTO_YAW_HOLD); + } + + // initialise waypoint and spline controller + wp_nav.wp_and_spline_init(); + + // clear guided limits + guided_limit_clear(); + + // start/resume the mission (based on MIS_RESTART parameter) + mission.start_or_resume(); + return true; } // auto_run - runs the appropriate auto controller diff --git a/ArduSub/control_circle.cpp b/ArduSub/control_circle.cpp index ec9ad75f2b..eabf2488e1 100644 --- a/ArduSub/control_circle.cpp +++ b/ArduSub/control_circle.cpp @@ -5,25 +5,25 @@ */ // circle_init - initialise circle controller flight mode -bool Sub::circle_init(bool ignore_checks) +bool Sub::circle_init() { - if (position_ok() || ignore_checks) { - circle_pilot_yaw_override = false; - - // initialize speeds and accelerations - pos_control.set_speed_xy(wp_nav.get_speed_xy()); - pos_control.set_accel_xy(wp_nav.get_wp_acceleration()); - pos_control.set_jerk_xy_to_default(); - pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); - pos_control.set_accel_z(g.pilot_accel_z); - - // initialise circle controller including setting the circle center based on vehicle speed - circle_nav.init(); - - return true; - }else{ + if (!position_ok()) { return false; } + + circle_pilot_yaw_override = false; + + // initialize speeds and accelerations + pos_control.set_speed_xy(wp_nav.get_speed_xy()); + pos_control.set_accel_xy(wp_nav.get_wp_acceleration()); + pos_control.set_jerk_xy_to_default(); + pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); + pos_control.set_accel_z(g.pilot_accel_z); + + // initialise circle controller including setting the circle center based on vehicle speed + circle_nav.init(); + + return true; } // circle_run - runs the circle flight mode diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp index 3fbadb134b..80462bb71b 100644 --- a/ArduSub/control_guided.cpp +++ b/ArduSub/control_guided.cpp @@ -36,15 +36,15 @@ struct Guided_Limit { // guided_init - initialise guided controller bool Sub::guided_init(bool ignore_checks) { - if (position_ok() || ignore_checks) { - // initialise yaw - set_auto_yaw_mode(get_default_auto_yaw_mode(false)); - // start in position control mode - guided_pos_control_start(); - return true; - }else{ - return false; - } + if (!position_ok() && !ignore_checks) { + return false; + } + + // initialise yaw + set_auto_yaw_mode(get_default_auto_yaw_mode(false)); + // start in position control mode + guided_pos_control_start(); + return true; } // initialise guided mode's position controller diff --git a/ArduSub/control_manual.cpp b/ArduSub/control_manual.cpp index 8edbd0bde0..75a39a506f 100644 --- a/ArduSub/control_manual.cpp +++ b/ArduSub/control_manual.cpp @@ -1,7 +1,7 @@ #include "Sub.h" // manual_init - initialise manual controller -bool Sub::manual_init(bool ignore_checks) +bool Sub::manual_init() { // set target altitude to zero for reporting pos_control.set_alt_target(0); diff --git a/ArduSub/control_poshold.cpp b/ArduSub/control_poshold.cpp index 90f634dabe..78a9048774 100644 --- a/ArduSub/control_poshold.cpp +++ b/ArduSub/control_poshold.cpp @@ -7,10 +7,10 @@ #if POSHOLD_ENABLED == ENABLED // poshold_init - initialise PosHold controller -bool Sub::poshold_init(bool ignore_checks) +bool Sub::poshold_init() { // fail to initialise PosHold mode if no GPS lock - if (!position_ok() && !ignore_checks) { + if (!position_ok()) { return false; } diff --git a/ArduSub/control_stabilize.cpp b/ArduSub/control_stabilize.cpp index 727ac7cc65..a422996962 100644 --- a/ArduSub/control_stabilize.cpp +++ b/ArduSub/control_stabilize.cpp @@ -1,7 +1,7 @@ #include "Sub.h" // stabilize_init - initialise stabilize controller -bool Sub::stabilize_init(bool ignore_checks) +bool Sub::stabilize_init() { // set target altitude to zero for reporting pos_control.set_alt_target(0); diff --git a/ArduSub/control_surface.cpp b/ArduSub/control_surface.cpp index 3f818d2df6..5ce5a42932 100644 --- a/ArduSub/control_surface.cpp +++ b/ArduSub/control_surface.cpp @@ -1,7 +1,7 @@ #include "Sub.h" -bool Sub::surface_init(bool ignore_checks) +bool Sub::surface_init() { #if CONFIG_HAL_BOARD != HAL_BOARD_SITL if (!ap.depth_sensor_present) { // can't hold depth without a depth sensor, exit immediately. diff --git a/ArduSub/flight_mode.cpp b/ArduSub/flight_mode.cpp index c98131bb8e..9070a5ac71 100644 --- a/ArduSub/flight_mode.cpp +++ b/ArduSub/flight_mode.cpp @@ -6,7 +6,6 @@ bool Sub::set_mode(control_mode_t mode, mode_reason_t reason) { // boolean to record if flight mode could be set bool success = false; - bool ignore_checks = false; // Always check for now // return immediately if we are already in the desired mode if (mode == control_mode) { @@ -19,41 +18,41 @@ bool Sub::set_mode(control_mode_t mode, mode_reason_t reason) switch (mode) { case ACRO: - success = acro_init(ignore_checks); + success = acro_init(); break; case STABILIZE: - success = stabilize_init(ignore_checks); + success = stabilize_init(); break; case ALT_HOLD: - success = althold_init(ignore_checks); + success = althold_init(); break; case AUTO: - success = auto_init(ignore_checks); + success = auto_init(); break; case CIRCLE: - success = circle_init(ignore_checks); + success = circle_init(); break; case GUIDED: - success = guided_init(ignore_checks); + success = guided_init(); break; case SURFACE: - success = surface_init(ignore_checks); + success = surface_init(); break; #if POSHOLD_ENABLED == ENABLED case POSHOLD: - success = poshold_init(ignore_checks); + success = poshold_init(); break; #endif case MANUAL: - success = manual_init(ignore_checks); + success = manual_init(); break; default: