From bb3e32d3911623192799871d0c9ed43985204d8f Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Fri, 9 Dec 2016 12:16:56 -0500 Subject: [PATCH] Sub: Make checks stricter on flight mode init Also return fail mode init for unimplemented modes --- ArduSub/control_autotune.cpp | 84 ++++++++++++++++++----------------- ArduSub/control_circle.cpp | 34 +++++++------- ArduSub/control_guided.cpp | 20 +++++---- ArduSub/control_stabilize.cpp | 1 - ArduSub/flight_mode.cpp | 2 +- 5 files changed, 73 insertions(+), 68 deletions(-) diff --git a/ArduSub/control_autotune.cpp b/ArduSub/control_autotune.cpp index 2b4cf444a3..bb5414638b 100644 --- a/ArduSub/control_autotune.cpp +++ b/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) { - case AUTOTUNE_MODE_FAILED: - // autotune has been run but failed so reset state to uninitialized - autotune_state.mode = AUTOTUNE_MODE_UNINITIALISED; - // no break to allow fall through to restart the tuning - - case AUTOTUNE_MODE_UNINITIALISED: - // autotune has never been run - success = autotune_start(false); - if (success) { - // so store current gains as original gains - autotune_backup_gains_and_initialise(); - // advance mode to tuning - autotune_state.mode = AUTOTUNE_MODE_TUNING; - // send message to ground station that we've started tuning - autotune_update_gcs(AUTOTUNE_MESSAGE_STARTED); - } - break; - - case AUTOTUNE_MODE_TUNING: - // 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); - if (success) { - // reset gains to tuning-start gains (i.e. low I term) - autotune_load_intra_test_gains(); - // write dataflash log even and send message to ground station - Log_Write_Event(DATA_AUTOTUNE_RESTART); - autotune_update_gcs(AUTOTUNE_MESSAGE_STARTED); - } - break; - - case AUTOTUNE_MODE_SUCCESS: - // 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) - autotune_load_tuned_gains(); - Log_Write_Event(DATA_AUTOTUNE_PILOT_TESTING); - break; - } - - return success; +// bool success = true; +// +// switch (autotune_state.mode) { +// case AUTOTUNE_MODE_FAILED: +// // autotune has been run but failed so reset state to uninitialized +// autotune_state.mode = AUTOTUNE_MODE_UNINITIALISED; +// // no break to allow fall through to restart the tuning +// +// case AUTOTUNE_MODE_UNINITIALISED: +// // autotune has never been run +// success = autotune_start(false); +// if (success) { +// // so store current gains as original gains +// autotune_backup_gains_and_initialise(); +// // advance mode to tuning +// autotune_state.mode = AUTOTUNE_MODE_TUNING; +// // send message to ground station that we've started tuning +// autotune_update_gcs(AUTOTUNE_MESSAGE_STARTED); +// } +// break; +// +// case AUTOTUNE_MODE_TUNING: +// // 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); +// if (success) { +// // reset gains to tuning-start gains (i.e. low I term) +// autotune_load_intra_test_gains(); +// // write dataflash log even and send message to ground station +// Log_Write_Event(DATA_AUTOTUNE_RESTART); +// autotune_update_gcs(AUTOTUNE_MESSAGE_STARTED); +// } +// break; +// +// case AUTOTUNE_MODE_SUCCESS: +// // 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) +// autotune_load_tuned_gains(); +// Log_Write_Event(DATA_AUTOTUNE_PILOT_TESTING); +// break; +// } +// +// return success; } // autotune_stop - should be called when the ch7/ch8 switch is switched OFF diff --git a/ArduSub/control_circle.cpp b/ArduSub/control_circle.cpp index 3f40d85f42..d6ff76d169 100644 --- a/ArduSub/control_circle.cpp +++ b/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) { - circle_pilot_yaw_override = false; + return false; // Not implemented - // 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; - } +// 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{ +// return false; +// } } // circle_run - runs the circle flight mode diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp index 4bc53efda7..ccee6e332d 100644 --- a/ArduSub/control_guided.cpp +++ b/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) { - // 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; - } + return false; // Not implemented + +// 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; +// } } diff --git a/ArduSub/control_stabilize.cpp b/ArduSub/control_stabilize.cpp index 195e89083c..1da6945bb7 100644 --- a/ArduSub/control_stabilize.cpp +++ b/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; } diff --git a/ArduSub/flight_mode.cpp b/ArduSub/flight_mode.cpp index f65b0eaac7..a647abdda4 100644 --- a/ArduSub/flight_mode.cpp +++ b/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) {