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

AC_AutoTune: remove start method and always init position controller

This commit is contained in:
Randy Mackay 2021-04-30 11:29:34 +09:00
parent 67e15f8d07
commit e78c6804a0
2 changed files with 26 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
libraries/AC_AutoTune/AC_AutoTune.cpp
View File

@ -140,8 +140,6 @@ bool AC_AutoTune::init_internals(bool _use_poshold,
AP_AHRS_View *_ahrs_view,
AP_InertialNav *_inertial_nav)
{
bool success = true;
use_poshold = _use_poshold;
attitude_control = _attitude_control;
pos_control = _pos_control;
@ -149,6 +147,14 @@ bool AC_AutoTune::init_internals(bool _use_poshold,
inertial_nav = _inertial_nav;
motors = AP_Motors::get_singleton();
// exit immediately if motor are not armed
if ((motors == nullptr) || !motors->armed()) {
return false;
}
// initialise position controller
init_position_controller();
switch (mode) {
case FAILED:
// autotune has been run but failed so reset state to uninitialized
@ -158,31 +164,24 @@ bool AC_AutoTune::init_internals(bool _use_poshold,
case UNINITIALISED:
// autotune has never been run
success = start();
if (success) {
// so store current gains as original gains
backup_gains_and_initialise();
// advance mode to tuning
mode = TUNING;
// send message to ground station that we've started tuning
update_gcs(AUTOTUNE_MESSAGE_STARTED);
}
// so store current gains as original gains
backup_gains_and_initialise();
// advance mode to tuning
mode = TUNING;
// send message to ground station that we've started tuning
update_gcs(AUTOTUNE_MESSAGE_STARTED);
break;
case TUNING:
// we are restarting tuning after the user must have switched ch7/ch8 off so we restart tuning where we left off
success = start();
if (success) {
// reset gains to tuning-start gains (i.e. low I term)
load_gains(GAIN_INTRA_TEST);
AP::logger().Write_Event(LogEvent::AUTOTUNE_RESTART);
update_gcs(AUTOTUNE_MESSAGE_STARTED);
}
// we are restarting tuning so restart where we left off
// reset gains to tuning-start gains (i.e. low I term)
load_gains(GAIN_INTRA_TEST);
AP::logger().Write_Event(LogEvent::AUTOTUNE_RESTART);
update_gcs(AUTOTUNE_MESSAGE_STARTED);
break;
case 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)
// we have completed a tune and the pilot wishes to test the new gains
load_gains(GAIN_TUNED);
update_gcs(AUTOTUNE_MESSAGE_TESTING);
AP::logger().Write_Event(LogEvent::AUTOTUNE_PILOT_TESTING);
@ -191,7 +190,7 @@ bool AC_AutoTune::init_internals(bool _use_poshold,
have_position = false;
return success;
return true;
}
// stop - should be called when the ch7/ch8 switch is switched OFF
@ -210,13 +209,9 @@ void AC_AutoTune::stop()
// we expect the caller will change the flight mode back to the flight mode indicated by the flight mode switch
}
// start - Initialize autotune flight mode
bool AC_AutoTune::start(void)
// initialise position controller
bool AC_AutoTune::init_position_controller(void)
{
if (!motors->armed()) {
return false;
}
// initialize vertical speeds and leash lengths
init_z_limits();

6
libraries/AC_AutoTune/AC_AutoTune.h
View File

@ -61,9 +61,6 @@ protected:
// log PIDs at full rate for during twitch
virtual void log_pids() = 0;
// start tune - virtual so that vehicle code can add additional pre-conditions
virtual bool start(void);
// return true if we have a good position estimate
virtual bool position_ok();
@ -75,6 +72,9 @@ protected:
AP_AHRS_View *ahrs_view,
AP_InertialNav *inertial_nav);
// initialise position controller
bool init_position_controller();
private:
void control_attitude();
void backup_gains_and_initialise();