Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: Make checks stricter on flight mode init 

Also return fail mode init for unimplemented modes
This commit is contained in:
Jacob Walser 2016-12-09 12:16:56 -05:00 committed by Andrew Tridgell
parent 3ddb714e20
commit bb3e32d391
5 changed files with 73 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
ArduSub/control_autotune.cpp
View File

@ -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

34
ArduSub/control_circle.cpp
View File

@ -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

20
ArduSub/control_guided.cpp
View File

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

1
ArduSub/control_stabilize.cpp
View File

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

2
ArduSub/flight_mode.cpp
View File

@ -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) {