Sub: Remove ignore_check argument from control mode init functions

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);

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);

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.

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) {
+    if (!position_ok() || mission.num_commands() < 2) {
-        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 {
         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

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) {
+    if (!position_ok()) {
-        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{
         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

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) {
+    if (!position_ok() && !ignore_checks) {
-            // initialise yaw
+        return false;
-            set_auto_yaw_mode(get_default_auto_yaw_mode(false));
+    }
-            // start in position control mode
+
-            guided_pos_control_start();
+    // initialise yaw
-            return true;
+    set_auto_yaw_mode(get_default_auto_yaw_mode(false));
-        }else{
+    // start in position control mode
-            return false;
+    guided_pos_control_start();
-        }
+    return true;
 }
 
 // initialise guided mode's position controller

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);

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;
     }
 

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);

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.

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: