Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 00:13:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: Autotune: Base angles limits on lean_angle_max

Browse Source
This commit is contained in:
Leonard Hall 2024-02-19 14:02:02 +10:30 committed by Peter Barker
parent 54618dd903
commit e2560371cf
3 changed files with 17 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AC_AutoTune/AC_AutoTune.cpp
View File

@ -22,7 +22,7 @@
#define AUTOTUNE_REQUIRED_LEVEL_TIME_MS 500 // time we require the aircraft to be level
#define AUTOTUNE_LEVEL_TIMEOUT_MS 2000 // time out for level
#define AUTOTUNE_LEVEL_WARNING_INTERVAL_MS 5000 // level failure warning messages sent at this interval to users
#define AUTOTUNE_ANGLE_MAX_RLLPIT 30.0f // maximum allowable angle in degrees during testing
#define AUTOTUNE_ANGLE_MAX_RLLPIT_SCALE 2.0 / 3.0 // maximum allowable angle during testing, as a fraction of angle_max
AC_AutoTune::AC_AutoTune()
{
@ -360,18 +360,19 @@ void AC_AutoTune::control_attitude()
// Initialize test-specific variables
switch (axis) {
case ROLL:
abort_angle = AUTOTUNE_TARGET_ANGLE_RLLPIT_CD;
abort_angle = attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_RLLPIT_SCALE;
start_rate = ToDeg(ahrs_view->get_gyro().x) * 100.0f;
start_angle = ahrs_view->roll_sensor;
break;
case PITCH:
abort_angle = AUTOTUNE_TARGET_ANGLE_RLLPIT_CD;
abort_angle = attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_RLLPIT_SCALE;
start_rate = ToDeg(ahrs_view->get_gyro().y) * 100.0f;
start_angle = ahrs_view->pitch_sensor;
break;
case YAW:
case YAW_D:
abort_angle = AUTOTUNE_TARGET_ANGLE_YAW_CD;
// Aircraft with small lean angle will generally benefit from proportional smaller yaw twitch size
abort_angle = attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_YAW_SCALE;
start_rate = ToDeg(ahrs_view->get_gyro().z) * 100.0f;
start_angle = ahrs_view->yaw_sensor;
break;
@ -391,13 +392,13 @@ void AC_AutoTune::control_attitude()
test_run(axis, direction_sign);
// Check for failure causing reverse response
if (lean_angle <= -AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD) {
if (lean_angle <= -attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_SCALE) {
step = WAITING_FOR_LEVEL;
}
// protect from roll over
float resultant_angle = degrees(acosf(ahrs_view->cos_roll() * ahrs_view->cos_pitch()));
if (resultant_angle > AUTOTUNE_ANGLE_MAX_RLLPIT) {
const float resultant_angle_cd = 100 * degrees(acosf(ahrs_view->cos_roll() * ahrs_view->cos_pitch()));
if (resultant_angle_cd > attitude_control->lean_angle_max_cd() * AUTOTUNE_ANGLE_MAX_RLLPIT_SCALE) {
step = WAITING_FOR_LEVEL;
}

7
libraries/AC_AutoTune/AC_AutoTune.h
View File

@ -44,9 +44,10 @@
#define AUTOTUNE_ANNOUNCE_INTERVAL_MS 2000
#define AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD 1000 // minimum target angle during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_TARGET_ANGLE_RLLPIT_CD 2000 // target angle during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_TARGET_ANGLE_YAW_CD 3000 // target angle during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_SCALE 1.0 / 4.0 // minimum target angle, as a fraction of angle_max, during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_TARGET_ANGLE_RLLPIT_SCALE 1.0 / 2.0 // target angle, as a fraction of angle_max, during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_TARGET_MIN_ANGLE_YAW_SCALE 1.0 / 8.0 // minimum target angle, as a fraction of angle_max, during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_TARGET_ANGLE_YAW_SCALE 2.0 / 3.0 // target angle, as a fraction of angle_max, during TESTING_RATE step that will cause us to move to next step
class AC_AutoTune
{

11
libraries/AC_AutoTune/AC_AutoTune_Multi.cpp
View File

@ -75,9 +75,8 @@
#define AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS 4500 // target roll/pitch rate during AUTOTUNE_STEP_TWITCHING step
// yaw axis
#define AUTOTUNE_TARGET_RATE_YAW_CDS 9000 // target yaw rate during AUTOTUNE_STEP_TWITCHING step
#define AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD 500 // minimum target angle during TESTING_RATE step that will cause us to move to next step
#define AUTOTUNE_TARGET_MIN_RATE_YAW_CDS 1500 // minimum target yaw rate during AUTOTUNE_STEP_TWITCHING step
#define AUTOTUNE_TARGET_RATE_YAW_CDS 9000 // target yaw rate during AUTOTUNE_STEP_TWITCHING step
#define AUTOTUNE_TARGET_MIN_RATE_YAW_CDS 1500 // minimum target yaw rate during AUTOTUNE_STEP_TWITCHING step
// second table of user settable parameters for quadplanes, this
// allows us to go beyond the 64 parameter limit
@ -1133,14 +1132,14 @@ void AC_AutoTune_Multi::twitch_test_init()
case ROLL: {
target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler*AUTOTUNE_TARGET_RATE_RLLPIT_CDS);
target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, target_max_rate);
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD);
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_roll())*100.0f, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_SCALE, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_RLLPIT_SCALE);
rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_roll_pid().filt_D_hz()*2.0f);
break;
}
case PITCH: {
target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, step_scaler*AUTOTUNE_TARGET_RATE_RLLPIT_CDS);
target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, target_max_rate);
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD);
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_pitch())*100.0f, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_SCALE, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_RLLPIT_SCALE);
rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_pitch_pid().filt_D_hz()*2.0f);
break;
}
@ -1148,7 +1147,7 @@ void AC_AutoTune_Multi::twitch_test_init()
case YAW_D: {
target_max_rate = MAX(AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, step_scaler*AUTOTUNE_TARGET_RATE_YAW_CDS);
target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, target_max_rate);
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD, AUTOTUNE_TARGET_ANGLE_YAW_CD);
target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_yaw()*0.75f)*100.0f, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_MIN_ANGLE_YAW_SCALE, attitude_control->lean_angle_max_cd() * AUTOTUNE_TARGET_ANGLE_YAW_SCALE);
if (axis == YAW_D) {
rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_yaw_pid().filt_D_hz()*2.0f);
} else {