Sub: Make checks stricter on flight mode init

Also return fail mode init for unimplemented modes

ArduSub/control_autotune.cpp

@@ -164,48 +164,50 @@ static float    tune_yaw_rp, tune_yaw_rLPF, tune_yaw_sp, tune_yaw_accel;
 // autotune_init - should be called when autotune mode is selected
 bool Copter::autotune_init(bool ignore_checks)
 {
-    bool success = true;
+	return false; // Not implemented
-
+
-    switch (autotune_state.mode) {
+//    bool success = true;
-        case AUTOTUNE_MODE_FAILED:
+//
-        // autotune has been run but failed so reset state to uninitialized
+//    switch (autotune_state.mode) {
-            autotune_state.mode = AUTOTUNE_MODE_UNINITIALISED;
+//        case AUTOTUNE_MODE_FAILED:
-            // no break to allow fall through to restart the tuning
+//        // autotune has been run but failed so reset state to uninitialized
-
+//            autotune_state.mode = AUTOTUNE_MODE_UNINITIALISED;
-        case AUTOTUNE_MODE_UNINITIALISED:
+//            // no break to allow fall through to restart the tuning
-            // autotune has never been run
+//
-            success = autotune_start(false);
+//        case AUTOTUNE_MODE_UNINITIALISED:
-            if (success) {
+//            // autotune has never been run
-                // so store current gains as original gains
+//            success = autotune_start(false);
-                autotune_backup_gains_and_initialise();
+//            if (success) {
-                // advance mode to tuning
+//                // so store current gains as original gains
-                autotune_state.mode = AUTOTUNE_MODE_TUNING;
+//                autotune_backup_gains_and_initialise();
-                // send message to ground station that we've started tuning
+//                // advance mode to tuning
-                autotune_update_gcs(AUTOTUNE_MESSAGE_STARTED);
+//                autotune_state.mode = AUTOTUNE_MODE_TUNING;
-            }
+//                // send message to ground station that we've started tuning
-            break;
+//                autotune_update_gcs(AUTOTUNE_MESSAGE_STARTED);
-
+//            }
-        case AUTOTUNE_MODE_TUNING:
+//            break;
-            // we are restarting tuning after the user must have switched ch7/ch8 off so we restart tuning where we left off
+//
-            success = autotune_start(false);
+//        case AUTOTUNE_MODE_TUNING:
-            if (success) {
+//            // we are restarting tuning after the user must have switched ch7/ch8 off so we restart tuning where we left off
-                // reset gains to tuning-start gains (i.e. low I term)
+//            success = autotune_start(false);
-                autotune_load_intra_test_gains();
+//            if (success) {
-                // write dataflash log even and send message to ground station
+//                // reset gains to tuning-start gains (i.e. low I term)
-                Log_Write_Event(DATA_AUTOTUNE_RESTART);
+//                autotune_load_intra_test_gains();
-                autotune_update_gcs(AUTOTUNE_MESSAGE_STARTED);
+//                // write dataflash log even and send message to ground station
-            }
+//                Log_Write_Event(DATA_AUTOTUNE_RESTART);
-            break;
+//                autotune_update_gcs(AUTOTUNE_MESSAGE_STARTED);
-
+//            }
-        case AUTOTUNE_MODE_SUCCESS:
+//            break;
-            // we have completed a tune and the pilot wishes to test the new gains in the current flight mode
+//
-            // so simply apply tuning gains (i.e. do not change flight mode)
+//        case AUTOTUNE_MODE_SUCCESS:
-            autotune_load_tuned_gains();
+//            // we have completed a tune and the pilot wishes to test the new gains in the current flight mode
-            Log_Write_Event(DATA_AUTOTUNE_PILOT_TESTING);
+//            // so simply apply tuning gains (i.e. do not change flight mode)
-            break;
+//            autotune_load_tuned_gains();
-    }
+//            Log_Write_Event(DATA_AUTOTUNE_PILOT_TESTING);
-
+//            break;
-    return success;
+//    }
+//
+//    return success;
 }
 
 // autotune_stop - should be called when the ch7/ch8 switch is switched OFF

ArduSub/control_circle.cpp

@@ -9,23 +9,25 @@
 // circle_init - initialise circle controller flight mode
 bool Sub::circle_init(bool ignore_checks)
 {
-    if (position_ok() || ignore_checks) {
+	return false; // Not implemented
-        circle_pilot_yaw_override = false;
+
-
+//    if (position_ok() || ignore_checks) {
-        // initialize speeds and accelerations
+//        circle_pilot_yaw_override = false;
-        pos_control.set_speed_xy(wp_nav.get_speed_xy());
+//
-        pos_control.set_accel_xy(wp_nav.get_wp_acceleration());
+//        // initialize speeds and accelerations
-        pos_control.set_jerk_xy_to_default();
+//        pos_control.set_speed_xy(wp_nav.get_speed_xy());
-        pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
+//        pos_control.set_accel_xy(wp_nav.get_wp_acceleration());
-        pos_control.set_accel_z(g.pilot_accel_z);
+//        pos_control.set_jerk_xy_to_default();
-
+//        pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
-        // initialise circle controller including setting the circle center based on vehicle speed
+//        pos_control.set_accel_z(g.pilot_accel_z);
-        circle_nav.init();
+//
-
+//        // initialise circle controller including setting the circle center based on vehicle speed
-        return true;
+//        circle_nav.init();
-    }else{
+//
-        return false;
+//        return true;
-    }
+//    }else{
+//        return false;
+//    }
 }
 
 // circle_run - runs the circle flight mode

ArduSub/control_guided.cpp

@@ -38,15 +38,17 @@ struct Guided_Limit {
 // guided_init - initialise guided controller
 bool Sub::guided_init(bool ignore_checks)
 {
-    if (position_ok() || ignore_checks) {
+	return false; // Not implemented
-        // initialise yaw
+
-        set_auto_yaw_mode(get_default_auto_yaw_mode(false));
+//    if (position_ok() || ignore_checks) {
-        // start in position control mode
+//        // initialise yaw
-        guided_pos_control_start();
+//        set_auto_yaw_mode(get_default_auto_yaw_mode(false));
-        return true;
+//        // start in position control mode
-    }else{
+//        guided_pos_control_start();
-        return false;
+//        return true;
-    }
+//    }else{
+//        return false;
+//    }
 }
 
 

ArduSub/control_stabilize.cpp

@@ -13,7 +13,6 @@ bool Sub::stabilize_init(bool ignore_checks)
     pos_control.set_alt_target(0);
     last_pilot_heading = ahrs.yaw_sensor;
 
-
     return true;
 }
 

ArduSub/flight_mode.cpp

@@ -15,7 +15,7 @@ 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 = !motors.armed();   // allow switching to any mode if disarmed.  We rely on the arming check to perform
+    bool ignore_checks = false; // Always check for now
 
     // return immediately if we are already in the desired mode
     if (mode == control_mode) {