2020-10-19 23:51:05 -03:00
/*
This program is free software : you can redistribute it and / or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation , either version 3 of the License , or
( at your option ) any later version .
This program is distributed in the hope that it will be useful ,
but WITHOUT ANY WARRANTY ; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE . See the
GNU General Public License for more details .
You should have received a copy of the GNU General Public License
along with this program . If not , see < http : //www.gnu.org/licenses/>.
*/
/*
support for autotune of helicopters . Based on original autotune code from ArduCopter , written by Leonard Hall
Converted to a library by Andrew Tridgell , and rewritten to include helicopters by Bill Geyer
*/
2023-12-07 20:03:15 -04:00
# include "AC_AutoTune_config.h"
# if AC_AUTOTUNE_ENABLED
2021-01-17 22:28:20 -04:00
# include "AC_AutoTune_Heli.h"
2022-03-03 23:29:45 -04:00
# include <AP_Logger/AP_Logger.h>
2022-11-08 06:10:56 -04:00
# include <GCS_MAVLink/GCS.h>
2022-03-03 23:29:45 -04:00
2021-12-28 00:23:19 -04:00
# define AUTOTUNE_TESTING_STEP_TIMEOUT_MS 5000U // timeout for tuning mode's testing step
2020-10-19 23:51:05 -03:00
# define AUTOTUNE_RD_STEP 0.0005f // minimum increment when increasing/decreasing Rate D term
# define AUTOTUNE_RP_STEP 0.005f // minimum increment when increasing/decreasing Rate P term
# define AUTOTUNE_SP_STEP 0.05f // minimum increment when increasing/decreasing Stab P term
# define AUTOTUNE_PI_RATIO_FOR_TESTING 0.1f // I is set 10x smaller than P during testing
2021-12-27 23:52:15 -04:00
# define AUTOTUNE_PI_RATIO_FINAL 1.0f // I is set 1x P after testing
# define AUTOTUNE_YAW_PI_RATIO_FINAL 0.1f // I is set 1x P after testing
2020-10-19 23:51:05 -03:00
# define AUTOTUNE_RD_MAX 0.020f // maximum Rate D value
2022-01-07 14:37:02 -04:00
# define AUTOTUNE_RP_MIN 0.02f // minimum Rate P value
2021-01-18 00:16:08 -04:00
# define AUTOTUNE_RP_MAX 0.3f // maximum Rate P value
# define AUTOTUNE_SP_MAX 10.0f // maximum Stab P value
# define AUTOTUNE_SP_MIN 3.0f // maximum Stab P value
# define AUTOTUNE_RFF_MAX 0.5f // maximum Stab P value
# define AUTOTUNE_RFF_MIN 0.025f // maximum Stab P value
2021-12-27 00:53:50 -04:00
# define AUTOTUNE_RD_BACKOFF 1.0f // Rate D gains are reduced to 50% of their maximum value discovered during tuning
# define AUTOTUNE_RP_BACKOFF 1.0f // Rate P gains are reduced to 97.5% of their maximum value discovered during tuning
2021-12-27 23:52:15 -04:00
# define AUTOTUNE_SP_BACKOFF 1.0f // Stab P gains are reduced to 90% of their maximum value discovered during tuning
2021-12-27 00:53:50 -04:00
# define AUTOTUNE_ACCEL_RP_BACKOFF 1.0f // back off from maximum acceleration
# define AUTOTUNE_ACCEL_Y_BACKOFF 1.0f // back off from maximum acceleration
2021-12-27 23:52:15 -04:00
2022-02-01 18:03:38 -04:00
# define AUTOTUNE_HELI_TARGET_ANGLE_RLLPIT_CD 1500 // target roll/pitch angle during AUTOTUNE FeedForward rate test
2021-12-27 23:52:15 -04:00
# define AUTOTUNE_HELI_TARGET_RATE_RLLPIT_CDS 5000 // target roll/pitch rate during AUTOTUNE FeedForward rate test
# define AUTOTUNE_FFI_RATIO_FOR_TESTING 0.5f // I is set 2x smaller than VFF during testing
# define AUTOTUNE_FFI_RATIO_FINAL 0.5f // I is set 0.5x VFF after testing
2021-12-27 00:53:50 -04:00
# define AUTOTUNE_RP_ACCEL_MIN 20000.0f // Minimum acceleration for Roll and Pitch
# define AUTOTUNE_Y_ACCEL_MIN 10000.0f // Minimum acceleration for Yaw
2021-01-18 00:16:08 -04:00
# define AUTOTUNE_SEQ_BITMASK_VFF 1
# define AUTOTUNE_SEQ_BITMASK_RATE_D 2
# define AUTOTUNE_SEQ_BITMASK_ANGLE_P 4
# define AUTOTUNE_SEQ_BITMASK_MAX_GAIN 8
2022-05-08 00:28:50 -03:00
# define AUTOTUNE_SEQ_BITMASK_TUNE_CHECK 16
2021-01-18 00:16:08 -04:00
2024-02-29 02:42:39 -04:00
// angle limits preserved from previous behaviour as Multi changed:
# define AUTOTUNE_ANGLE_TARGET_MAX_RP_CD 2000 // target angle during TESTING_RATE step that will cause us to move to next step
# define AUTOTUNE_ANGLE_TARGET_MIN_RP_CD 1000 // minimum target angle during TESTING_RATE step that will cause us to move to next step
# define AUTOTUNE_ANGLE_TARGET_MAX_Y_CD 3000 // target angle during TESTING_RATE step that will cause us to move to next step
# define AUTOTUNE_ANGLE_TARGET_MIN_Y_CD 500 // target angle during TESTING_RATE step that will cause us to move to next step
# define AUTOTUNE_ANGLE_MAX_RP_CD 3000 // maximum allowable angle in degrees during testing
# define AUTOTUNE_ANGLE_NEG_RPY_CD 1000 // maximum allowable angle in degrees during testing
2021-01-18 00:16:08 -04:00
const AP_Param : : GroupInfo AC_AutoTune_Heli : : var_info [ ] = {
2021-10-01 17:35:52 -03:00
// @Param: AXES
// @DisplayName: Autotune axis bitmask
// @Description: 1-byte bitmap of axes to autotune
// @Bitmask: 0:Roll,1:Pitch,2:Yaw
// @User: Standard
2022-02-06 23:45:24 -04:00
AP_GROUPINFO ( " AXES " , 1 , AC_AutoTune_Heli , axis_bitmask , 1 ) ,
2021-01-18 00:16:08 -04:00
// @Param: SEQ
// @DisplayName: AutoTune Sequence Bitmask
2022-05-08 00:28:50 -03:00
// @Description: 2-byte bitmask to select what tuning should be performed. Max gain automatically performed if Rate D is selected. Values: 7:All,1:VFF Only,2:Rate D/Rate P Only(incl max gain),4:Angle P Only,8:Max Gain Only,16:Tune Check,3:VFF and Rate D/Rate P(incl max gain),5:VFF and Angle P,6:Rate D/Rate P(incl max gain) and angle P
// @Bitmask: 0:VFF,1:Rate D/Rate P(incl max gain),2:Angle P,3:Max Gain Only,4:Tune Check
2021-01-18 00:16:08 -04:00
// @User: Standard
2022-01-23 00:36:34 -04:00
AP_GROUPINFO ( " SEQ " , 2 , AC_AutoTune_Heli , seq_bitmask , 3 ) ,
2021-01-18 00:16:08 -04:00
2021-12-28 00:23:19 -04:00
// @Param: FRQ_MIN
2021-01-18 00:16:08 -04:00
// @DisplayName: AutoTune minimum sweep frequency
// @Description: Defines the start frequency for sweeps and dwells
// @Range: 10 30
// @User: Standard
2021-12-28 00:23:19 -04:00
AP_GROUPINFO ( " FRQ_MIN " , 3 , AC_AutoTune_Heli , min_sweep_freq , 10.0f ) ,
2021-01-18 00:16:08 -04:00
2021-12-28 00:23:19 -04:00
// @Param: FRQ_MAX
2021-01-18 00:16:08 -04:00
// @DisplayName: AutoTune maximum sweep frequency
// @Description: Defines the end frequency for sweeps and dwells
// @Range: 50 120
// @User: Standard
2021-12-28 00:23:19 -04:00
AP_GROUPINFO ( " FRQ_MAX " , 4 , AC_AutoTune_Heli , max_sweep_freq , 70.0f ) ,
2021-01-18 00:16:08 -04:00
2021-12-28 00:23:19 -04:00
// @Param: GN_MAX
2021-01-18 00:16:08 -04:00
// @DisplayName: AutoTune maximum response gain
// @Description: Defines the response gain (output/input) to tune
// @Range: 1 2.5
// @User: Standard
2022-02-06 23:45:24 -04:00
AP_GROUPINFO ( " GN_MAX " , 5 , AC_AutoTune_Heli , max_resp_gain , 1.0f ) ,
2021-01-18 00:16:08 -04:00
2021-09-12 17:12:15 -03:00
// @Param: VELXY_P
// @DisplayName: AutoTune velocity xy P gain
// @Description: Velocity xy P gain used to hold position during Max Gain, Rate P, and Rate D frequency sweeps
// @Range: 0 1
// @User: Standard
2021-10-01 17:35:52 -03:00
AP_GROUPINFO ( " VELXY_P " , 6 , AC_AutoTune_Heli , vel_hold_gain , 0.1f ) ,
2021-09-12 17:12:15 -03:00
2024-04-01 00:41:59 -03:00
// @Param: ACC_MAX
// @DisplayName: AutoTune maximum allowable angular acceleration
// @Description: maximum angular acceleration in deg/s/s allowed during autotune maneuvers
// @Range: 1 4000
// @User: Standard
AP_GROUPINFO ( " ACC_MAX " , 7 , AC_AutoTune_Heli , accel_max , 5000.0f ) ,
// @Param: RAT_MAX
// @DisplayName: Autotune maximum allowable angular rate
// @Description: maximum angular rate in deg/s allowed during autotune maneuvers
// @Range: 0 500
// @User: Standard
AP_GROUPINFO ( " RAT_MAX " , 8 , AC_AutoTune_Heli , rate_max , 50.0f ) ,
2021-01-18 00:16:08 -04:00
AP_GROUPEND
} ;
// constructor
AC_AutoTune_Heli : : AC_AutoTune_Heli ( )
2020-10-19 23:51:05 -03:00
{
2021-01-18 00:16:08 -04:00
tune_seq [ 0 ] = TUNE_COMPLETE ;
AP_Param : : setup_object_defaults ( this , var_info ) ;
}
2020-10-19 23:51:05 -03:00
2021-12-09 23:42:02 -04:00
// initialize tests for each tune type
2021-01-18 00:16:08 -04:00
void AC_AutoTune_Heli : : test_init ( )
{
2022-01-16 19:13:59 -04:00
switch ( tune_type ) {
case RFF_UP :
2024-04-04 22:42:18 -03:00
freq_cnt = 12 ;
test_freq [ freq_cnt ] = 0.25f * 3.14159f * 2.0f ;
curr_test . freq = test_freq [ freq_cnt ] ;
start_freq = curr_test . freq ;
stop_freq = curr_test . freq ;
attitude_control - > bf_feedforward ( false ) ;
attitude_control - > use_sqrt_controller ( false ) ;
freqresp . init ( AC_AutoTune_FreqResp : : InputType : : DWELL , AC_AutoTune_FreqResp : : ResponseType : : RATE ) ;
dwell_test_init ( start_freq , stop_freq , start_freq , RATE ) ;
step_time_limit_ms = ( uint32_t ) 23000 ;
2022-01-16 19:13:59 -04:00
break ;
case MAX_GAINS :
case RP_UP :
case RD_UP :
// initialize start frequency
2021-01-18 00:16:08 -04:00
if ( is_zero ( start_freq ) ) {
2022-01-16 19:13:59 -04:00
if ( tune_type = = RP_UP ) {
// continue using frequency where testing left off or RD_UP completed
if ( test_phase [ 12 ] > 0.0f & & test_phase [ 12 ] < 180.0f ) {
freq_cnt = 12 ;
// start with freq found for sweep where phase was 180 deg
2022-02-08 19:31:51 -04:00
} else if ( ! is_zero ( sweep . ph180 . freq ) ) {
2022-01-16 19:13:59 -04:00
freq_cnt = 12 ;
2022-02-08 19:31:51 -04:00
test_freq [ freq_cnt ] = sweep . ph180 . freq - 0.25f * 3.14159f * 2.0f ;
2022-01-16 19:13:59 -04:00
// otherwise start at min freq to step up in dwell frequency until phase > 160 deg
} else {
freq_cnt = 0 ;
test_freq [ freq_cnt ] = min_sweep_freq ;
}
2022-02-08 19:31:51 -04:00
curr_test . freq = test_freq [ freq_cnt ] ;
start_freq = curr_test . freq ;
stop_freq = curr_test . freq ;
2022-01-16 19:13:59 -04:00
// MAX_GAINS and RD_UP both start with a sweep initially but if it has been completed then start dwells at the freq for 180 deg phase
2021-01-18 00:16:08 -04:00
} else {
2022-02-08 19:31:51 -04:00
if ( ! is_zero ( sweep . ph180 . freq ) ) {
2022-01-16 19:13:59 -04:00
freq_cnt = 12 ;
2022-02-08 19:31:51 -04:00
test_freq [ freq_cnt ] = sweep . ph180 . freq - 0.25f * 3.14159f * 2.0f ;
curr_test . freq = test_freq [ freq_cnt ] ;
start_freq = curr_test . freq ;
stop_freq = curr_test . freq ;
2022-01-16 19:13:59 -04:00
if ( tune_type = = MAX_GAINS ) {
reset_maxgains_update_gain_variables ( ) ;
}
} else {
start_freq = min_sweep_freq ;
stop_freq = max_sweep_freq ;
}
2021-01-18 00:16:08 -04:00
}
2020-10-19 23:51:05 -03:00
}
2024-04-01 00:41:59 -03:00
attitude_control - > bf_feedforward ( false ) ;
attitude_control - > use_sqrt_controller ( false ) ;
2021-01-18 00:16:08 -04:00
if ( ! is_equal ( start_freq , stop_freq ) ) {
// initialize determine_gain function whenever test is initialized
2022-03-06 20:32:12 -04:00
freqresp . init ( AC_AutoTune_FreqResp : : InputType : : SWEEP , AC_AutoTune_FreqResp : : ResponseType : : RATE ) ;
2022-03-07 01:23:41 -04:00
dwell_test_init ( start_freq , stop_freq , stop_freq , RATE ) ;
2021-01-18 00:16:08 -04:00
} else {
// initialize determine_gain function whenever test is initialized
2022-03-06 20:32:12 -04:00
freqresp . init ( AC_AutoTune_FreqResp : : InputType : : DWELL , AC_AutoTune_FreqResp : : ResponseType : : RATE ) ;
2022-03-07 01:23:41 -04:00
dwell_test_init ( start_freq , stop_freq , start_freq , RATE ) ;
2021-01-18 00:16:08 -04:00
}
if ( ! is_zero ( start_freq ) ) {
2020-10-19 23:51:05 -03:00
// 4 seconds is added to allow aircraft to achieve start attitude. Then the time to conduct the dwells is added to it.
2021-09-12 16:12:57 -03:00
step_time_limit_ms = ( uint32_t ) ( 4000 + ( float ) ( AUTOTUNE_DWELL_CYCLES + 2 ) * 1000.0f * M_2PI / start_freq ) ;
2020-10-19 23:51:05 -03:00
}
2022-01-16 19:13:59 -04:00
break ;
case SP_UP :
// initialize start frequency
2021-01-18 00:16:08 -04:00
if ( is_zero ( start_freq ) ) {
2022-02-08 19:31:51 -04:00
if ( ! is_zero ( sweep . maxgain . freq ) ) {
2022-01-16 19:13:59 -04:00
freq_cnt = 12 ;
2022-02-08 19:31:51 -04:00
test_freq [ freq_cnt ] = sweep . maxgain . freq - 0.25f * 3.14159f * 2.0f ;
curr_test . freq = test_freq [ freq_cnt ] ;
start_freq = curr_test . freq ;
stop_freq = curr_test . freq ;
2022-01-16 19:13:59 -04:00
test_accel_max = 0.0f ;
} else {
start_freq = min_sweep_freq ;
stop_freq = max_sweep_freq ;
}
2021-01-18 00:16:08 -04:00
}
2022-05-03 00:35:50 -03:00
attitude_control - > bf_feedforward ( false ) ;
2024-04-01 00:41:59 -03:00
attitude_control - > use_sqrt_controller ( false ) ;
2021-01-18 00:16:08 -04:00
if ( ! is_equal ( start_freq , stop_freq ) ) {
// initialize determine gain function
2022-03-06 20:32:12 -04:00
freqresp . init ( AC_AutoTune_FreqResp : : InputType : : SWEEP , AC_AutoTune_FreqResp : : ResponseType : : ANGLE ) ;
2022-05-08 00:28:50 -03:00
dwell_test_init ( start_freq , stop_freq , stop_freq , DRB ) ;
2021-01-18 00:16:08 -04:00
} else {
// initialize determine gain function
2022-03-06 20:32:12 -04:00
freqresp . init ( AC_AutoTune_FreqResp : : InputType : : DWELL , AC_AutoTune_FreqResp : : ResponseType : : ANGLE ) ;
2022-05-08 00:28:50 -03:00
dwell_test_init ( start_freq , stop_freq , start_freq , DRB ) ;
2020-10-19 23:51:05 -03:00
}
2021-01-18 00:16:08 -04:00
2022-05-08 00:28:50 -03:00
// TODO add time limit for sweep test
if ( ! is_zero ( start_freq ) ) {
// 1 seconds is added for a little buffer. Then the time to conduct the dwells is added to it.
step_time_limit_ms = ( uint32_t ) ( 2000 + ( float ) ( AUTOTUNE_DWELL_CYCLES + 7 ) * 1000.0f * M_2PI / start_freq ) ;
}
break ;
case TUNE_CHECK :
// initialize start frequency
if ( is_zero ( start_freq ) ) {
start_freq = min_sweep_freq ;
stop_freq = max_sweep_freq ;
}
// initialize determine gain function
freqresp . init ( AC_AutoTune_FreqResp : : InputType : : SWEEP , AC_AutoTune_FreqResp : : ResponseType : : ANGLE ) ;
dwell_test_init ( start_freq , stop_freq , stop_freq , ANGLE ) ;
2021-01-18 00:16:08 -04:00
// TODO add time limit for sweep test
if ( ! is_zero ( start_freq ) ) {
2020-10-19 23:51:05 -03:00
// 1 seconds is added for a little buffer. Then the time to conduct the dwells is added to it.
2021-09-12 16:12:57 -03:00
step_time_limit_ms = ( uint32_t ) ( 2000 + ( float ) ( AUTOTUNE_DWELL_CYCLES + 7 ) * 1000.0f * M_2PI / start_freq ) ;
2020-10-19 23:51:05 -03:00
}
2022-01-16 19:13:59 -04:00
break ;
default :
break ;
2020-10-19 23:51:05 -03:00
}
2021-10-06 00:50:02 -03:00
2020-10-19 23:51:05 -03:00
start_angles = Vector3f ( roll_cd , pitch_cd , desired_yaw_cd ) ; // heli specific
}
2021-12-09 23:42:02 -04:00
// run tests for each tune type
2020-10-19 23:51:05 -03:00
void AC_AutoTune_Heli : : test_run ( AxisType test_axis , const float dir_sign )
{
2022-01-21 14:44:36 -04:00
// if tune complete or beyond frequency range or no max allowed gains then exit testing
if ( tune_type = = TUNE_COMPLETE | |
2022-04-14 00:41:58 -03:00
( ( tune_type = = RP_UP | | tune_type = = RD_UP ) & & ( max_rate_p . max_allowed < = 0.0f | | max_rate_d . max_allowed < = 0.0f ) ) | |
2022-01-21 14:44:36 -04:00
( ( tune_type = = MAX_GAINS | | tune_type = = RP_UP | | tune_type = = RD_UP | | tune_type = = SP_UP ) & & exceeded_freq_range ( start_freq ) ) ) {
2022-04-10 00:17:28 -03:00
load_gains ( GAIN_ORIGINAL ) ;
2022-01-21 14:44:36 -04:00
attitude_control - > use_sqrt_controller ( true ) ;
get_poshold_attitude ( roll_cd , pitch_cd , desired_yaw_cd ) ;
// hold level attitude
attitude_control - > input_euler_angle_roll_pitch_yaw ( roll_cd , pitch_cd , desired_yaw_cd , true ) ;
2022-04-14 00:41:58 -03:00
if ( ( tune_type = = RP_UP | | tune_type = = RD_UP ) & & ( max_rate_p . max_allowed < = 0.0f | | max_rate_d . max_allowed < = 0.0f ) ) {
2022-04-10 00:17:28 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " AutoTune: Max Gain Determination Failed " ) ;
mode = FAILED ;
2023-07-13 21:58:04 -03:00
LOGGER_WRITE_EVENT ( LogEvent : : AUTOTUNE_FAILED ) ;
2022-04-10 00:17:28 -03:00
update_gcs ( AUTOTUNE_MESSAGE_FAILED ) ;
2022-01-21 14:44:36 -04:00
} else if ( ( tune_type = = MAX_GAINS | | tune_type = = RP_UP | | tune_type = = RD_UP | | tune_type = = SP_UP ) & & exceeded_freq_range ( start_freq ) ) {
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " AutoTune: Exceeded frequency range " ) ;
2022-04-10 00:17:28 -03:00
mode = FAILED ;
2023-07-13 21:58:04 -03:00
LOGGER_WRITE_EVENT ( LogEvent : : AUTOTUNE_FAILED ) ;
2022-04-10 00:17:28 -03:00
update_gcs ( AUTOTUNE_MESSAGE_FAILED ) ;
2022-01-21 14:44:36 -04:00
} else if ( tune_type = = TUNE_COMPLETE ) {
counter = AUTOTUNE_SUCCESS_COUNT ;
step = UPDATE_GAINS ;
}
return ;
}
2022-01-16 19:13:59 -04:00
switch ( tune_type ) {
case RFF_UP :
2024-04-04 22:42:18 -03:00
dwell_test_run ( 1 , start_freq , stop_freq , test_gain [ freq_cnt ] , test_phase [ freq_cnt ] , RATE ) ;
2022-01-16 19:13:59 -04:00
break ;
case RP_UP :
case RD_UP :
2022-03-06 20:32:12 -04:00
dwell_test_run ( 1 , start_freq , stop_freq , test_gain [ freq_cnt ] , test_phase [ freq_cnt ] , RATE ) ;
2022-01-16 19:13:59 -04:00
break ;
case MAX_GAINS :
2022-03-06 20:32:12 -04:00
dwell_test_run ( 0 , start_freq , stop_freq , test_gain [ freq_cnt ] , test_phase [ freq_cnt ] , RATE ) ;
2022-01-16 19:13:59 -04:00
break ;
case SP_UP :
2022-05-08 00:28:50 -03:00
dwell_test_run ( 1 , start_freq , stop_freq , test_gain [ freq_cnt ] , test_phase [ freq_cnt ] , DRB ) ;
break ;
case TUNE_CHECK :
2022-03-06 20:32:12 -04:00
dwell_test_run ( 1 , start_freq , stop_freq , test_gain [ freq_cnt ] , test_phase [ freq_cnt ] , ANGLE ) ;
2022-01-16 19:13:59 -04:00
break ;
default :
2020-10-19 23:51:05 -03:00
step = UPDATE_GAINS ;
2022-01-16 19:13:59 -04:00
break ;
2020-10-19 23:51:05 -03:00
}
}
2021-12-09 23:42:02 -04:00
// heli specific gcs announcements
2020-10-19 23:51:05 -03:00
void AC_AutoTune_Heli : : do_gcs_announcements ( )
{
const uint32_t now = AP_HAL : : millis ( ) ;
if ( now - announce_time < AUTOTUNE_ANNOUNCE_INTERVAL_MS ) {
return ;
}
2022-01-07 14:37:02 -04:00
2022-02-03 17:24:23 -04:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " AutoTune: %s %s " , axis_string ( ) , type_string ( ) ) ;
2022-01-07 14:37:02 -04:00
send_step_string ( ) ;
switch ( tune_type ) {
2024-04-04 22:42:18 -03:00
case RFF_UP :
2022-01-07 14:37:02 -04:00
case RD_UP :
case RP_UP :
case MAX_GAINS :
case SP_UP :
2022-05-08 00:28:50 -03:00
case TUNE_CHECK :
2022-01-07 14:37:02 -04:00
if ( is_equal ( start_freq , stop_freq ) ) {
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " AutoTune: Dwell " ) ;
} else {
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " AutoTune: Sweep " ) ;
if ( settle_time = = 0 ) {
2022-02-08 19:31:51 -04:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " AutoTune: freq=%f gain=%f phase=%f " , ( double ) ( curr_test . freq ) , ( double ) ( curr_test . gain ) , ( double ) ( curr_test . phase ) ) ;
2022-01-07 14:37:02 -04:00
}
}
break ;
default :
break ;
}
announce_time = now ;
}
// send post test updates to user
void AC_AutoTune_Heli : : do_post_test_gcs_announcements ( ) {
2020-10-19 23:51:05 -03:00
float tune_rp = 0.0f ;
float tune_rd = 0.0f ;
float tune_rff = 0.0f ;
float tune_sp = 0.0f ;
float tune_accel = 0.0f ;
2022-01-07 14:37:02 -04:00
2020-10-19 23:51:05 -03:00
switch ( axis ) {
case ROLL :
tune_rp = tune_roll_rp ;
tune_rd = tune_roll_rd ;
tune_rff = tune_roll_rff ;
tune_sp = tune_roll_sp ;
tune_accel = tune_roll_accel ;
break ;
case PITCH :
tune_rp = tune_pitch_rp ;
tune_rd = tune_pitch_rd ;
tune_rff = tune_pitch_rff ;
tune_sp = tune_pitch_sp ;
tune_accel = tune_pitch_accel ;
break ;
case YAW :
2023-03-06 05:39:47 -04:00
case YAW_D :
2020-10-19 23:51:05 -03:00
tune_rp = tune_yaw_rp ;
tune_rd = tune_yaw_rd ;
tune_rff = tune_yaw_rff ;
tune_sp = tune_yaw_sp ;
tune_accel = tune_yaw_accel ;
break ;
}
2022-01-07 14:37:02 -04:00
if ( step = = UPDATE_GAINS ) {
switch ( tune_type ) {
case RFF_UP :
case RP_UP :
case RD_UP :
case SP_UP :
case MAX_GAINS :
if ( is_equal ( start_freq , stop_freq ) ) {
2022-02-10 23:27:08 -04:00
// announce results of dwell
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " AutoTune: freq=%f gain=%f " , ( double ) ( test_freq [ freq_cnt ] ) , ( double ) ( test_gain [ freq_cnt ] ) ) ;
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " AutoTune: ph=%f " , ( double ) ( test_phase [ freq_cnt ] ) ) ;
if ( tune_type = = RP_UP ) {
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " AutoTune: rate_p=%f " , ( double ) ( tune_rp ) ) ;
} else if ( tune_type = = RD_UP ) {
2024-04-04 22:42:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " AutoTune: rate_d=%f " , ( double ) ( tune_rd ) ) ;
} else if ( tune_type = = RFF_UP ) {
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " AutoTune: rate_ff=%f " , ( double ) ( tune_rff ) ) ;
2022-02-10 23:27:08 -04:00
} else if ( tune_type = = SP_UP ) {
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " AutoTune: angle_p=%f tune_accel=%f max_accel=%f " , ( double ) ( tune_sp ) , ( double ) ( tune_accel ) , ( double ) ( test_accel_max ) ) ;
}
2022-01-07 14:37:02 -04:00
} else {
2022-02-08 19:31:51 -04:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " AutoTune: max_freq=%f max_gain=%f " , ( double ) ( sweep . maxgain . freq ) , ( double ) ( sweep . maxgain . gain ) ) ;
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " AutoTune: ph180_freq=%f ph180_gain=%f " , ( double ) ( sweep . ph180 . freq ) , ( double ) ( sweep . ph180 . gain ) ) ;
2022-01-07 14:37:02 -04:00
}
break ;
default :
break ;
2020-10-19 23:51:05 -03:00
}
}
}
2021-12-22 10:45:38 -04:00
// backup_gains_and_initialise - store current gains as originals
// called before tuning starts to backup original gains
void AC_AutoTune_Heli : : backup_gains_and_initialise ( )
{
AC_AutoTune : : backup_gains_and_initialise ( ) ;
2021-12-22 23:37:31 -04:00
// initializes dwell test sequence for rate_p_up and rate_d_up tests for tradheli
freq_cnt = 0 ;
start_freq = 0.0f ;
stop_freq = 0.0f ;
2021-12-22 10:45:38 -04:00
orig_bf_feedforward = attitude_control - > get_bf_feedforward ( ) ;
// backup original pids and initialise tuned pid values
orig_roll_rp = attitude_control - > get_rate_roll_pid ( ) . kP ( ) ;
orig_roll_ri = attitude_control - > get_rate_roll_pid ( ) . kI ( ) ;
orig_roll_rd = attitude_control - > get_rate_roll_pid ( ) . kD ( ) ;
orig_roll_rff = attitude_control - > get_rate_roll_pid ( ) . ff ( ) ;
orig_roll_fltt = attitude_control - > get_rate_roll_pid ( ) . filt_T_hz ( ) ;
orig_roll_smax = attitude_control - > get_rate_roll_pid ( ) . slew_limit ( ) ;
orig_roll_sp = attitude_control - > get_angle_roll_p ( ) . kP ( ) ;
orig_roll_accel = attitude_control - > get_accel_roll_max_cdss ( ) ;
tune_roll_rp = attitude_control - > get_rate_roll_pid ( ) . kP ( ) ;
tune_roll_rd = attitude_control - > get_rate_roll_pid ( ) . kD ( ) ;
tune_roll_rff = attitude_control - > get_rate_roll_pid ( ) . ff ( ) ;
tune_roll_sp = attitude_control - > get_angle_roll_p ( ) . kP ( ) ;
tune_roll_accel = attitude_control - > get_accel_roll_max_cdss ( ) ;
orig_pitch_rp = attitude_control - > get_rate_pitch_pid ( ) . kP ( ) ;
orig_pitch_ri = attitude_control - > get_rate_pitch_pid ( ) . kI ( ) ;
orig_pitch_rd = attitude_control - > get_rate_pitch_pid ( ) . kD ( ) ;
orig_pitch_rff = attitude_control - > get_rate_pitch_pid ( ) . ff ( ) ;
orig_pitch_fltt = attitude_control - > get_rate_pitch_pid ( ) . filt_T_hz ( ) ;
orig_pitch_smax = attitude_control - > get_rate_pitch_pid ( ) . slew_limit ( ) ;
orig_pitch_sp = attitude_control - > get_angle_pitch_p ( ) . kP ( ) ;
orig_pitch_accel = attitude_control - > get_accel_pitch_max_cdss ( ) ;
tune_pitch_rp = attitude_control - > get_rate_pitch_pid ( ) . kP ( ) ;
tune_pitch_rd = attitude_control - > get_rate_pitch_pid ( ) . kD ( ) ;
tune_pitch_rff = attitude_control - > get_rate_pitch_pid ( ) . ff ( ) ;
tune_pitch_sp = attitude_control - > get_angle_pitch_p ( ) . kP ( ) ;
tune_pitch_accel = attitude_control - > get_accel_pitch_max_cdss ( ) ;
orig_yaw_rp = attitude_control - > get_rate_yaw_pid ( ) . kP ( ) ;
orig_yaw_ri = attitude_control - > get_rate_yaw_pid ( ) . kI ( ) ;
orig_yaw_rd = attitude_control - > get_rate_yaw_pid ( ) . kD ( ) ;
orig_yaw_rff = attitude_control - > get_rate_yaw_pid ( ) . ff ( ) ;
orig_yaw_fltt = attitude_control - > get_rate_yaw_pid ( ) . filt_T_hz ( ) ;
orig_yaw_smax = attitude_control - > get_rate_yaw_pid ( ) . slew_limit ( ) ;
orig_yaw_rLPF = attitude_control - > get_rate_yaw_pid ( ) . filt_E_hz ( ) ;
orig_yaw_accel = attitude_control - > get_accel_yaw_max_cdss ( ) ;
orig_yaw_sp = attitude_control - > get_angle_yaw_p ( ) . kP ( ) ;
tune_yaw_rp = attitude_control - > get_rate_yaw_pid ( ) . kP ( ) ;
tune_yaw_rd = attitude_control - > get_rate_yaw_pid ( ) . kD ( ) ;
tune_yaw_rff = attitude_control - > get_rate_yaw_pid ( ) . ff ( ) ;
tune_yaw_rLPF = attitude_control - > get_rate_yaw_pid ( ) . filt_E_hz ( ) ;
tune_yaw_sp = attitude_control - > get_angle_yaw_p ( ) . kP ( ) ;
tune_yaw_accel = attitude_control - > get_accel_yaw_max_cdss ( ) ;
2023-07-13 21:58:04 -03:00
LOGGER_WRITE_EVENT ( LogEvent : : AUTOTUNE_INITIALISED ) ;
2021-12-22 10:45:38 -04:00
}
// load_orig_gains - set gains to their original values
// called by stop and failed functions
void AC_AutoTune_Heli : : load_orig_gains ( )
{
attitude_control - > bf_feedforward ( orig_bf_feedforward ) ;
if ( roll_enabled ( ) ) {
2022-03-31 17:52:40 -03:00
load_gain_set ( ROLL , orig_roll_rp , orig_roll_ri , orig_roll_rd , orig_roll_rff , orig_roll_sp , orig_roll_accel , orig_roll_fltt , 0.0f , orig_roll_smax ) ;
2021-12-22 10:45:38 -04:00
}
if ( pitch_enabled ( ) ) {
2022-03-31 17:52:40 -03:00
load_gain_set ( PITCH , orig_pitch_rp , orig_pitch_ri , orig_pitch_rd , orig_pitch_rff , orig_pitch_sp , orig_pitch_accel , orig_pitch_fltt , 0.0f , orig_pitch_smax ) ;
2021-12-22 10:45:38 -04:00
}
if ( yaw_enabled ( ) ) {
2022-03-31 17:52:40 -03:00
load_gain_set ( YAW , orig_yaw_rp , orig_yaw_ri , orig_yaw_rd , orig_yaw_rff , orig_yaw_sp , orig_yaw_accel , orig_yaw_fltt , orig_yaw_rLPF , orig_yaw_smax ) ;
2021-12-22 10:45:38 -04:00
}
}
// load_tuned_gains - load tuned gains
void AC_AutoTune_Heli : : load_tuned_gains ( )
{
if ( ! attitude_control - > get_bf_feedforward ( ) ) {
attitude_control - > bf_feedforward ( true ) ;
attitude_control - > set_accel_roll_max_cdss ( 0.0f ) ;
attitude_control - > set_accel_pitch_max_cdss ( 0.0f ) ;
}
if ( roll_enabled ( ) ) {
2022-03-31 17:52:40 -03:00
load_gain_set ( ROLL , tune_roll_rp , tune_roll_rff * AUTOTUNE_FFI_RATIO_FINAL , tune_roll_rd , tune_roll_rff , tune_roll_sp , tune_roll_accel , orig_roll_fltt , 0.0f , orig_roll_smax ) ;
2021-12-22 10:45:38 -04:00
}
if ( pitch_enabled ( ) ) {
2022-03-31 17:52:40 -03:00
load_gain_set ( PITCH , tune_pitch_rp , tune_pitch_rff * AUTOTUNE_FFI_RATIO_FINAL , tune_pitch_rd , tune_pitch_rff , tune_pitch_sp , tune_pitch_accel , orig_pitch_fltt , 0.0f , orig_pitch_smax ) ;
2021-12-22 10:45:38 -04:00
}
if ( yaw_enabled ( ) ) {
if ( ! is_zero ( tune_yaw_rp ) ) {
2022-03-31 17:52:40 -03:00
load_gain_set ( YAW , tune_yaw_rp , tune_yaw_rp * AUTOTUNE_YAW_PI_RATIO_FINAL , tune_yaw_rd , tune_yaw_rff , tune_yaw_sp , tune_yaw_accel , orig_yaw_fltt , tune_yaw_rLPF , orig_yaw_smax ) ;
2021-12-22 10:45:38 -04:00
}
}
}
// load_intra_test_gains - gains used between tests
// called during testing mode's update-gains step to set gains ahead of return-to-level step
void AC_AutoTune_Heli : : load_intra_test_gains ( )
{
// we are restarting tuning so reset gains to tuning-start gains (i.e. low I term)
// sanity check the gains
attitude_control - > bf_feedforward ( true ) ;
if ( roll_enabled ( ) ) {
2022-03-31 17:52:40 -03:00
load_gain_set ( ROLL , orig_roll_rp , orig_roll_rff * AUTOTUNE_FFI_RATIO_FOR_TESTING , orig_roll_rd , orig_roll_rff , orig_roll_sp , orig_roll_accel , orig_roll_fltt , 0.0f , orig_roll_smax ) ;
2021-12-22 10:45:38 -04:00
}
if ( pitch_enabled ( ) ) {
2022-03-31 17:52:40 -03:00
load_gain_set ( PITCH , orig_pitch_rp , orig_pitch_rff * AUTOTUNE_FFI_RATIO_FOR_TESTING , orig_pitch_rd , orig_pitch_rff , orig_pitch_sp , orig_pitch_accel , orig_pitch_fltt , 0.0f , orig_pitch_smax ) ;
2021-12-22 10:45:38 -04:00
}
if ( yaw_enabled ( ) ) {
2022-03-31 17:52:40 -03:00
load_gain_set ( YAW , orig_yaw_rp , orig_yaw_rp * AUTOTUNE_PI_RATIO_FOR_TESTING , orig_yaw_rd , orig_yaw_rff , orig_yaw_sp , orig_yaw_accel , orig_yaw_fltt , orig_yaw_rLPF , orig_yaw_smax ) ;
2021-12-22 10:45:38 -04:00
}
}
2020-10-19 23:51:05 -03:00
// load_test_gains - load the to-be-tested gains for a single axis
// called by control_attitude() just before it beings testing a gain (i.e. just before it twitches)
void AC_AutoTune_Heli : : load_test_gains ( )
{
2022-03-31 17:52:40 -03:00
float rate_p , rate_i , rate_d ;
2020-10-19 23:51:05 -03:00
switch ( axis ) {
case ROLL :
2022-05-20 23:45:12 -03:00
if ( tune_type = = SP_UP | | tune_type = = TUNE_CHECK ) {
rate_i = tune_roll_rff * AUTOTUNE_FFI_RATIO_FINAL ;
2020-10-19 23:51:05 -03:00
} else {
2021-01-18 00:16:08 -04:00
// freeze integrator to hold trim by making i term small during rate controller tuning
2022-03-31 17:52:40 -03:00
rate_i = 0.01f * orig_roll_ri ;
2020-10-19 23:51:05 -03:00
}
2021-12-11 00:43:40 -04:00
if ( tune_type = = MAX_GAINS & & ! is_zero ( tune_roll_rff ) ) {
2022-03-31 17:52:40 -03:00
rate_p = 0.0f ;
rate_d = 0.0f ;
2021-12-11 00:43:40 -04:00
} else {
2022-03-31 17:52:40 -03:00
rate_p = tune_roll_rp ;
rate_d = tune_roll_rd ;
2021-12-11 00:43:40 -04:00
}
2022-03-31 17:52:40 -03:00
load_gain_set ( ROLL , rate_p , rate_i , rate_d , tune_roll_rff , tune_roll_sp , tune_roll_accel , orig_roll_fltt , 0.0f , 0.0f ) ;
2020-10-19 23:51:05 -03:00
break ;
case PITCH :
2022-05-20 23:45:12 -03:00
if ( tune_type = = SP_UP | | tune_type = = TUNE_CHECK ) {
rate_i = tune_pitch_rff * AUTOTUNE_FFI_RATIO_FINAL ;
2020-10-19 23:51:05 -03:00
} else {
2021-01-18 00:16:08 -04:00
// freeze integrator to hold trim by making i term small during rate controller tuning
2022-03-31 17:52:40 -03:00
rate_i = 0.01f * orig_pitch_ri ;
2020-10-19 23:51:05 -03:00
}
2021-12-11 00:43:40 -04:00
if ( tune_type = = MAX_GAINS & & ! is_zero ( tune_pitch_rff ) ) {
2022-03-31 17:52:40 -03:00
rate_p = 0.0f ;
rate_d = 0.0f ;
2021-12-11 00:43:40 -04:00
} else {
2022-03-31 17:52:40 -03:00
rate_p = tune_pitch_rp ;
rate_d = tune_pitch_rd ;
2021-12-11 00:43:40 -04:00
}
2022-03-31 17:52:40 -03:00
load_gain_set ( PITCH , rate_p , rate_i , rate_d , tune_pitch_rff , tune_pitch_sp , tune_pitch_accel , orig_pitch_fltt , 0.0f , 0.0f ) ;
2020-10-19 23:51:05 -03:00
break ;
case YAW :
2023-03-06 05:39:47 -04:00
case YAW_D :
2022-05-20 23:45:12 -03:00
if ( tune_type = = SP_UP | | tune_type = = TUNE_CHECK ) {
rate_i = tune_yaw_rp * AUTOTUNE_YAW_PI_RATIO_FINAL ;
} else {
// freeze integrator to hold trim by making i term small during rate controller tuning
rate_i = 0.01f * orig_yaw_ri ;
}
load_gain_set ( YAW , tune_yaw_rp , rate_i , tune_yaw_rd , tune_yaw_rff , tune_yaw_sp , tune_yaw_accel , orig_yaw_fltt , tune_yaw_rLPF , 0.0f ) ;
2022-03-31 17:52:40 -03:00
break ;
}
}
// load gains
void AC_AutoTune_Heli : : load_gain_set ( AxisType s_axis , float rate_p , float rate_i , float rate_d , float rate_ff , float angle_p , float max_accel , float rate_fltt , float rate_flte , float smax )
{
2022-06-18 00:21:39 -03:00
switch ( s_axis ) {
2022-03-31 17:52:40 -03:00
case ROLL :
attitude_control - > get_rate_roll_pid ( ) . kP ( rate_p ) ;
attitude_control - > get_rate_roll_pid ( ) . kI ( rate_i ) ;
attitude_control - > get_rate_roll_pid ( ) . kD ( rate_d ) ;
attitude_control - > get_rate_roll_pid ( ) . ff ( rate_ff ) ;
attitude_control - > get_rate_roll_pid ( ) . filt_T_hz ( rate_fltt ) ;
attitude_control - > get_rate_roll_pid ( ) . slew_limit ( smax ) ;
attitude_control - > get_angle_roll_p ( ) . kP ( angle_p ) ;
attitude_control - > set_accel_roll_max_cdss ( max_accel ) ;
break ;
case PITCH :
attitude_control - > get_rate_pitch_pid ( ) . kP ( rate_p ) ;
attitude_control - > get_rate_pitch_pid ( ) . kI ( rate_i ) ;
attitude_control - > get_rate_pitch_pid ( ) . kD ( rate_d ) ;
attitude_control - > get_rate_pitch_pid ( ) . ff ( rate_ff ) ;
attitude_control - > get_rate_pitch_pid ( ) . filt_T_hz ( rate_fltt ) ;
attitude_control - > get_rate_pitch_pid ( ) . slew_limit ( smax ) ;
attitude_control - > get_angle_pitch_p ( ) . kP ( angle_p ) ;
attitude_control - > set_accel_pitch_max_cdss ( max_accel ) ;
break ;
case YAW :
2023-03-06 05:39:47 -04:00
case YAW_D :
2022-03-31 17:52:40 -03:00
attitude_control - > get_rate_yaw_pid ( ) . kP ( rate_p ) ;
attitude_control - > get_rate_yaw_pid ( ) . kI ( rate_i ) ;
attitude_control - > get_rate_yaw_pid ( ) . kD ( rate_d ) ;
attitude_control - > get_rate_yaw_pid ( ) . ff ( rate_ff ) ;
attitude_control - > get_rate_yaw_pid ( ) . filt_T_hz ( rate_fltt ) ;
attitude_control - > get_rate_yaw_pid ( ) . slew_limit ( smax ) ;
attitude_control - > get_rate_yaw_pid ( ) . filt_E_hz ( rate_flte ) ;
attitude_control - > get_angle_yaw_p ( ) . kP ( angle_p ) ;
attitude_control - > set_accel_yaw_max_cdss ( max_accel ) ;
2020-10-19 23:51:05 -03:00
break ;
}
}
// save_tuning_gains - save the final tuned gains for each axis
// save discovered gains to eeprom if autotuner is enabled (i.e. switch is in the high position)
void AC_AutoTune_Heli : : save_tuning_gains ( )
{
2021-12-11 00:43:40 -04:00
// see if we successfully completed tuning of at least one axis
if ( axes_completed = = 0 ) {
return ;
}
2020-10-19 23:51:05 -03:00
2021-12-11 00:43:40 -04:00
if ( ! attitude_control - > get_bf_feedforward ( ) ) {
attitude_control - > bf_feedforward_save ( true ) ;
attitude_control - > save_accel_roll_max_cdss ( 0.0f ) ;
attitude_control - > save_accel_pitch_max_cdss ( 0.0f ) ;
}
2020-10-19 23:51:05 -03:00
// sanity check the rate P values
2021-01-18 00:16:08 -04:00
if ( ( axes_completed & AUTOTUNE_AXIS_BITMASK_ROLL ) & & roll_enabled ( ) ) {
2022-03-31 17:52:40 -03:00
load_gain_set ( ROLL , tune_roll_rp , tune_roll_rff * AUTOTUNE_FFI_RATIO_FINAL , tune_roll_rd , tune_roll_rff , tune_roll_sp , tune_roll_accel , orig_roll_fltt , 0.0f , orig_roll_smax ) ;
// save rate roll gains
2020-10-19 23:51:05 -03:00
attitude_control - > get_rate_roll_pid ( ) . save_gains ( ) ;
2022-03-31 17:52:40 -03:00
// save stabilize roll
2021-12-11 00:43:40 -04:00
attitude_control - > get_angle_roll_p ( ) . save_gains ( ) ;
2020-10-19 23:51:05 -03:00
// resave pids to originals in case the autotune is run again
2021-12-11 00:43:40 -04:00
orig_roll_rp = attitude_control - > get_rate_roll_pid ( ) . kP ( ) ;
2020-10-19 23:51:05 -03:00
orig_roll_ri = attitude_control - > get_rate_roll_pid ( ) . kI ( ) ;
2021-12-11 00:43:40 -04:00
orig_roll_rd = attitude_control - > get_rate_roll_pid ( ) . kD ( ) ;
orig_roll_rff = attitude_control - > get_rate_roll_pid ( ) . ff ( ) ;
orig_roll_sp = attitude_control - > get_angle_roll_p ( ) . kP ( ) ;
orig_roll_accel = attitude_control - > get_accel_roll_max_cdss ( ) ;
2020-10-19 23:51:05 -03:00
}
2021-01-18 00:16:08 -04:00
if ( ( axes_completed & AUTOTUNE_AXIS_BITMASK_PITCH ) & & pitch_enabled ( ) ) {
2022-03-31 17:52:40 -03:00
load_gain_set ( PITCH , tune_pitch_rp , tune_pitch_rff * AUTOTUNE_FFI_RATIO_FINAL , tune_pitch_rd , tune_pitch_rff , tune_pitch_sp , tune_pitch_accel , orig_pitch_fltt , 0.0f , orig_pitch_smax ) ;
// save rate pitch gains
2020-10-19 23:51:05 -03:00
attitude_control - > get_rate_pitch_pid ( ) . save_gains ( ) ;
2022-03-31 17:52:40 -03:00
// save stabilize pitch
2021-12-11 00:43:40 -04:00
attitude_control - > get_angle_pitch_p ( ) . save_gains ( ) ;
2020-10-19 23:51:05 -03:00
// resave pids to originals in case the autotune is run again
2021-12-11 00:43:40 -04:00
orig_pitch_rp = attitude_control - > get_rate_pitch_pid ( ) . kP ( ) ;
2020-10-19 23:51:05 -03:00
orig_pitch_ri = attitude_control - > get_rate_pitch_pid ( ) . kI ( ) ;
2021-12-11 00:43:40 -04:00
orig_pitch_rd = attitude_control - > get_rate_pitch_pid ( ) . kD ( ) ;
orig_pitch_rff = attitude_control - > get_rate_pitch_pid ( ) . ff ( ) ;
orig_pitch_sp = attitude_control - > get_angle_pitch_p ( ) . kP ( ) ;
orig_pitch_accel = attitude_control - > get_accel_pitch_max_cdss ( ) ;
2020-10-19 23:51:05 -03:00
}
if ( ( axes_completed & AUTOTUNE_AXIS_BITMASK_YAW ) & & yaw_enabled ( ) & & ! is_zero ( tune_yaw_rp ) ) {
2022-03-31 17:52:40 -03:00
load_gain_set ( YAW , tune_yaw_rp , tune_yaw_rp * AUTOTUNE_YAW_PI_RATIO_FINAL , tune_yaw_rd , tune_yaw_rff , tune_yaw_sp , tune_yaw_accel , orig_yaw_fltt , orig_yaw_rLPF , orig_yaw_smax ) ;
// save rate yaw gains
2020-10-19 23:51:05 -03:00
attitude_control - > get_rate_yaw_pid ( ) . save_gains ( ) ;
2022-03-31 17:52:40 -03:00
// save stabilize yaw
2021-12-11 00:43:40 -04:00
attitude_control - > get_angle_yaw_p ( ) . save_gains ( ) ;
2020-10-19 23:51:05 -03:00
// resave pids to originals in case the autotune is run again
2021-12-11 00:43:40 -04:00
orig_yaw_rp = attitude_control - > get_rate_yaw_pid ( ) . kP ( ) ;
orig_yaw_ri = attitude_control - > get_rate_yaw_pid ( ) . kI ( ) ;
2020-10-19 23:51:05 -03:00
orig_yaw_rd = attitude_control - > get_rate_yaw_pid ( ) . kD ( ) ;
orig_yaw_rff = attitude_control - > get_rate_yaw_pid ( ) . ff ( ) ;
2021-12-11 00:43:40 -04:00
orig_yaw_rLPF = attitude_control - > get_rate_yaw_pid ( ) . filt_E_hz ( ) ;
orig_yaw_sp = attitude_control - > get_angle_yaw_p ( ) . kP ( ) ;
orig_yaw_accel = attitude_control - > get_accel_yaw_max_cdss ( ) ;
2020-10-19 23:51:05 -03:00
}
// update GCS and log save gains event
update_gcs ( AUTOTUNE_MESSAGE_SAVED_GAINS ) ;
2023-07-13 21:58:04 -03:00
LOGGER_WRITE_EVENT ( LogEvent : : AUTOTUNE_SAVEDGAINS ) ;
2020-10-19 23:51:05 -03:00
reset ( ) ;
}
2022-02-04 19:30:10 -04:00
// report final gains for a given axis to GCS
void AC_AutoTune_Heli : : report_final_gains ( AxisType test_axis ) const
{
switch ( test_axis ) {
case ROLL :
report_axis_gains ( " Roll " , tune_roll_rp , tune_roll_rff * AUTOTUNE_FFI_RATIO_FINAL , tune_roll_rd , tune_roll_rff , tune_roll_sp , tune_roll_accel ) ;
break ;
case PITCH :
report_axis_gains ( " Pitch " , tune_pitch_rp , tune_pitch_rff * AUTOTUNE_FFI_RATIO_FINAL , tune_pitch_rd , tune_pitch_rff , tune_pitch_sp , tune_pitch_accel ) ;
break ;
case YAW :
2023-03-06 05:39:47 -04:00
case YAW_D :
2022-02-04 19:30:10 -04:00
report_axis_gains ( " Yaw " , tune_yaw_rp , tune_yaw_rp * AUTOTUNE_YAW_PI_RATIO_FINAL , tune_yaw_rd , tune_yaw_rff , tune_yaw_sp , tune_yaw_accel ) ;
break ;
}
}
2023-10-11 04:41:49 -03:00
// report gain formatting helper
2022-02-04 19:30:10 -04:00
void AC_AutoTune_Heli : : report_axis_gains ( const char * axis_string , float rate_P , float rate_I , float rate_D , float rate_ff , float angle_P , float max_accel ) const
{
gcs ( ) . send_text ( MAV_SEVERITY_NOTICE , " AutoTune: %s complete " , axis_string ) ;
gcs ( ) . send_text ( MAV_SEVERITY_NOTICE , " AutoTune: %s Rate: P:%0.4f, I:%0.4f, D:%0.5f, FF:%0.4f " , axis_string , rate_P , rate_I , rate_D , rate_ff ) ;
gcs ( ) . send_text ( MAV_SEVERITY_NOTICE , " AutoTune: %s Angle P:%0.2f, Max Accel:%0.0f " , axis_string , angle_P , max_accel ) ;
}
2022-03-07 01:23:41 -04:00
void AC_AutoTune_Heli : : dwell_test_init ( float start_frq , float stop_frq , float filt_freq , DwellType dwell_type )
2021-12-13 00:02:40 -04:00
{
2022-03-05 23:16:50 -04:00
dwell_start_time_ms = 0.0f ;
settle_time = 200 ;
2021-12-13 00:02:40 -04:00
rotation_rate_filt . set_cutoff_frequency ( filt_freq ) ;
command_filt . set_cutoff_frequency ( filt_freq ) ;
target_rate_filt . set_cutoff_frequency ( filt_freq ) ;
2022-03-05 23:16:50 -04:00
2022-03-20 00:49:46 -03:00
rotation_rate_filt . reset ( 0 ) ;
command_filt . reset ( 0 ) ;
target_rate_filt . reset ( 0 ) ;
rotation_rate = 0.0f ;
command_out = 0.0f ;
filt_target_rate = 0.0f ;
2021-12-13 00:02:40 -04:00
2022-01-01 23:33:52 -04:00
// filter at lower frequency to remove steady state
filt_command_reading . set_cutoff_frequency ( 0.2f * start_frq ) ;
filt_gyro_reading . set_cutoff_frequency ( 0.2f * start_frq ) ;
filt_tgt_rate_reading . set_cutoff_frequency ( 0.2f * start_frq ) ;
filt_att_fdbk_from_velxy_cd . set_cutoff_frequency ( 0.2f * start_frq ) ;
2022-03-07 01:23:41 -04:00
if ( ! is_equal ( start_frq , stop_frq ) ) {
2022-03-05 23:16:50 -04:00
reset_sweep_variables ( ) ;
curr_test . gain = 0.0f ;
curr_test . phase = 0.0f ;
2021-12-13 00:02:40 -04:00
}
2022-03-07 01:23:41 -04:00
chirp_input . init ( sweep_time_ms * 0.001f , start_frq / M_2PI , stop_frq / M_2PI , 0.0f , 0.0001f * sweep_time_ms , 0.0f ) ;
2021-12-13 00:02:40 -04:00
}
2022-03-06 20:32:12 -04:00
void AC_AutoTune_Heli : : dwell_test_run ( uint8_t freq_resp_input , float start_frq , float stop_frq , float & dwell_gain , float & dwell_phase , DwellType dwell_type )
2021-12-13 00:02:40 -04:00
{
float gyro_reading = 0.0f ;
float command_reading = 0.0f ;
float tgt_rate_reading = 0.0f ;
2022-03-06 20:32:12 -04:00
float tgt_attitude ;
2021-12-13 00:02:40 -04:00
const uint32_t now = AP_HAL : : millis ( ) ;
2023-03-06 05:39:47 -04:00
float target_angle_cd = 0.0f ;
2022-03-06 20:32:12 -04:00
float dwell_freq = start_frq ;
2021-12-13 00:02:40 -04:00
float cycle_time_ms = 0 ;
if ( ! is_zero ( dwell_freq ) ) {
2022-03-06 20:32:12 -04:00
cycle_time_ms = 1000.0f * M_2PI / dwell_freq ;
}
2024-04-01 00:41:59 -03:00
//Determine target attitude magnitude limiting acceleration and rate
tgt_attitude = 5.0f * 0.01745f ;
2021-12-13 00:02:40 -04:00
2022-03-06 20:32:12 -04:00
// body frame calculation of velocity
2021-12-13 00:02:40 -04:00
Vector3f velocity_ned , velocity_bf ;
if ( ahrs_view - > get_velocity_NED ( velocity_ned ) ) {
velocity_bf . x = velocity_ned . x * ahrs_view - > cos_yaw ( ) + velocity_ned . y * ahrs_view - > sin_yaw ( ) ;
velocity_bf . y = - velocity_ned . x * ahrs_view - > sin_yaw ( ) + velocity_ned . y * ahrs_view - > cos_yaw ( ) ;
}
if ( settle_time = = 0 ) {
2024-04-01 00:41:59 -03:00
target_angle_cd = - chirp_input . update ( ( now - dwell_start_time_ms ) * 0.001 , tgt_attitude * 5730.0f ) ;
dwell_freq = chirp_input . get_frequency_rads ( ) ;
2022-03-06 20:32:12 -04:00
const Vector2f att_fdbk {
- 5730.0f * vel_hold_gain * velocity_bf . y ,
5730.0f * vel_hold_gain * velocity_bf . x
} ;
2022-01-01 23:33:52 -04:00
filt_att_fdbk_from_velxy_cd . apply ( att_fdbk , AP : : scheduler ( ) . get_loop_period_s ( ) ) ;
2021-12-13 00:02:40 -04:00
} else {
2024-04-01 00:41:59 -03:00
target_angle_cd = 0.0f ;
trim_yaw_tgt_reading = ( float ) attitude_control - > get_att_target_euler_cd ( ) . z ;
trim_yaw_heading_reading = ( float ) ahrs_view - > yaw_sensor ;
2021-12-13 00:02:40 -04:00
dwell_start_time_ms = now ;
2022-01-01 23:33:52 -04:00
filt_att_fdbk_from_velxy_cd . reset ( Vector2f ( 0.0f , 0.0f ) ) ;
2022-03-06 20:32:12 -04:00
settle_time - - ;
2021-12-13 00:02:40 -04:00
}
2024-04-01 00:41:59 -03:00
const Vector2f trim_angle_cd {
constrain_float ( filt_att_fdbk_from_velxy_cd . get ( ) . x , - 2000.0f , 2000.0f ) ,
constrain_float ( filt_att_fdbk_from_velxy_cd . get ( ) . y , - 2000.0f , 2000.0f )
} ;
switch ( axis ) {
case ROLL :
attitude_control - > input_euler_angle_roll_pitch_euler_rate_yaw ( target_angle_cd + trim_angle_cd . x , trim_angle_cd . y , 0.0f ) ;
command_reading = motors - > get_roll ( ) ;
if ( dwell_type = = DRB ) {
tgt_rate_reading = ( target_angle_cd ) / 5730.0f ;
gyro_reading = ( ( float ) ahrs_view - > roll_sensor + trim_angle_cd . x - target_angle_cd ) / 5730.0f ;
} else if ( dwell_type = = RATE ) {
2022-03-06 20:32:12 -04:00
tgt_rate_reading = attitude_control - > rate_bf_targets ( ) . x ;
2024-04-01 00:41:59 -03:00
gyro_reading = ahrs_view - > get_gyro ( ) . x ;
} else {
tgt_rate_reading = ( ( float ) attitude_control - > get_att_target_euler_cd ( ) . x ) / 5730.0f ;
gyro_reading = ( ( float ) ahrs_view - > roll_sensor ) / 5730.0f ;
}
break ;
case PITCH :
attitude_control - > input_euler_angle_roll_pitch_euler_rate_yaw ( trim_angle_cd . x , target_angle_cd + trim_angle_cd . y , 0.0f ) ;
command_reading = motors - > get_pitch ( ) ;
if ( dwell_type = = DRB ) {
tgt_rate_reading = ( target_angle_cd ) / 5730.0f ;
gyro_reading = ( ( float ) ahrs_view - > pitch_sensor + trim_angle_cd . y - target_angle_cd ) / 5730.0f ;
} else if ( dwell_type = = RATE ) {
2022-03-06 20:32:12 -04:00
tgt_rate_reading = attitude_control - > rate_bf_targets ( ) . y ;
2024-04-01 00:41:59 -03:00
gyro_reading = ahrs_view - > get_gyro ( ) . y ;
} else {
tgt_rate_reading = ( ( float ) attitude_control - > get_att_target_euler_cd ( ) . y ) / 5730.0f ;
gyro_reading = ( ( float ) ahrs_view - > pitch_sensor ) / 5730.0f ;
2021-12-13 00:02:40 -04:00
}
2024-04-01 00:41:59 -03:00
break ;
case YAW :
case YAW_D :
attitude_control - > input_euler_angle_roll_pitch_yaw ( trim_angle_cd . x , trim_angle_cd . y , wrap_180_cd ( trim_yaw_tgt_reading + target_angle_cd ) , false ) ;
command_reading = motors - > get_yaw ( ) ;
if ( dwell_type = = DRB ) {
tgt_rate_reading = ( target_angle_cd ) / 5730.0f ;
gyro_reading = ( wrap_180_cd ( ( float ) ahrs_view - > yaw_sensor - trim_yaw_heading_reading - target_angle_cd ) ) / 5730.0f ;
} else if ( dwell_type = = RATE ) {
tgt_rate_reading = attitude_control - > rate_bf_targets ( ) . z ;
gyro_reading = ahrs_view - > get_gyro ( ) . z ;
} else {
tgt_rate_reading = ( wrap_180_cd ( ( float ) attitude_control - > get_att_target_euler_cd ( ) . z - trim_yaw_tgt_reading ) ) / 5730.0f ;
gyro_reading = ( wrap_180_cd ( ( float ) ahrs_view - > yaw_sensor - trim_yaw_heading_reading ) ) / 5730.0f ;
2021-12-13 00:02:40 -04:00
}
2024-04-01 00:41:59 -03:00
break ;
2021-12-13 00:02:40 -04:00
}
if ( settle_time = = 0 ) {
2022-01-01 23:33:52 -04:00
filt_command_reading . apply ( command_reading , AP : : scheduler ( ) . get_loop_period_s ( ) ) ;
filt_gyro_reading . apply ( gyro_reading , AP : : scheduler ( ) . get_loop_period_s ( ) ) ;
filt_tgt_rate_reading . apply ( tgt_rate_reading , AP : : scheduler ( ) . get_loop_period_s ( ) ) ;
2021-12-13 00:02:40 -04:00
} else {
2022-01-01 23:33:52 -04:00
filt_command_reading . reset ( command_reading ) ;
filt_gyro_reading . reset ( gyro_reading ) ;
filt_tgt_rate_reading . reset ( tgt_rate_reading ) ;
2021-12-13 00:02:40 -04:00
}
// looks at gain and phase of input rate to output rate
2022-01-01 23:33:52 -04:00
rotation_rate = rotation_rate_filt . apply ( ( gyro_reading - filt_gyro_reading . get ( ) ) ,
2022-03-06 20:32:12 -04:00
AP : : scheduler ( ) . get_loop_period_s ( ) ) ;
2022-01-01 23:33:52 -04:00
filt_target_rate = target_rate_filt . apply ( ( tgt_rate_reading - filt_tgt_rate_reading . get ( ) ) ,
2022-03-06 20:32:12 -04:00
AP : : scheduler ( ) . get_loop_period_s ( ) ) ;
2022-01-01 23:33:52 -04:00
command_out = command_filt . apply ( ( command_reading - filt_command_reading . get ( ) ) ,
2022-03-06 20:32:12 -04:00
AP : : scheduler ( ) . get_loop_period_s ( ) ) ;
2021-12-13 00:02:40 -04:00
2022-03-06 20:32:12 -04:00
// wait for dwell to start before determining gain and phase
2021-12-13 00:02:40 -04:00
if ( ( float ) ( now - dwell_start_time_ms ) > 6.25f * cycle_time_ms | | ( ! is_equal ( start_frq , stop_frq ) & & settle_time = = 0 ) ) {
if ( freq_resp_input = = 1 ) {
2022-03-06 20:32:12 -04:00
freqresp . update ( command_out , filt_target_rate , rotation_rate , dwell_freq ) ;
2021-12-13 00:02:40 -04:00
} else {
2022-03-06 20:32:12 -04:00
freqresp . update ( command_out , command_out , rotation_rate , dwell_freq ) ;
2021-12-13 00:02:40 -04:00
}
2022-03-06 20:32:12 -04:00
if ( freqresp . is_cycle_complete ( ) ) {
2021-12-13 00:02:40 -04:00
if ( ! is_equal ( start_frq , stop_frq ) ) {
2022-03-06 20:32:12 -04:00
curr_test . freq = freqresp . get_freq ( ) ;
curr_test . gain = freqresp . get_gain ( ) ;
curr_test . phase = freqresp . get_phase ( ) ;
2022-05-08 00:28:50 -03:00
if ( dwell_type = = DRB ) { test_accel_max = freqresp . get_accel_max ( ) ; }
2021-12-13 00:02:40 -04:00
// reset cycle_complete to allow indication of next cycle
2022-03-06 20:32:12 -04:00
freqresp . reset_cycle_complete ( ) ;
2023-07-13 21:58:04 -03:00
# if HAL_LOGGING_ENABLED
2021-12-13 00:02:40 -04:00
// log sweep data
Log_AutoTuneSweep ( ) ;
2023-07-13 21:58:04 -03:00
# endif
2021-12-13 00:02:40 -04:00
} else {
2022-03-06 20:32:12 -04:00
dwell_gain = freqresp . get_gain ( ) ;
dwell_phase = freqresp . get_phase ( ) ;
2022-05-08 00:28:50 -03:00
if ( dwell_type = = DRB ) { test_accel_max = freqresp . get_accel_max ( ) ; }
2021-12-13 00:02:40 -04:00
}
}
}
// set sweep data if a frequency sweep is being conducted
if ( ! is_equal ( start_frq , stop_frq ) & & ( float ) ( now - dwell_start_time_ms ) > 2.5f * cycle_time_ms ) {
// track sweep phase to prevent capturing 180 deg and 270 deg data after phase has wrapped.
2022-02-08 19:31:51 -04:00
if ( curr_test . phase > 180.0f & & sweep . progress = = 0 ) {
2021-12-13 00:02:40 -04:00
sweep . progress = 1 ;
2022-02-08 19:31:51 -04:00
} else if ( curr_test . phase > 270.0f & & sweep . progress = = 1 ) {
2021-12-13 00:02:40 -04:00
sweep . progress = 2 ;
}
2022-02-08 19:31:51 -04:00
if ( curr_test . phase < = 160.0f & & curr_test . phase > = 150.0f & & sweep . progress = = 0 ) {
2022-02-08 19:42:14 -04:00
sweep . ph180 = curr_test ;
2021-12-13 00:02:40 -04:00
}
2022-03-06 20:32:12 -04:00
if ( curr_test . phase < = 250.0f & & curr_test . phase > = 240.0f & & sweep . progress = = 1 ) {
2022-02-08 19:42:14 -04:00
sweep . ph270 = curr_test ;
2021-12-13 00:02:40 -04:00
}
2022-02-08 19:31:51 -04:00
if ( curr_test . gain > sweep . maxgain . gain ) {
2022-02-08 19:42:14 -04:00
sweep . maxgain = curr_test ;
2021-12-13 00:02:40 -04:00
}
if ( now - step_start_time_ms > = sweep_time_ms + 200 ) {
// we have passed the maximum stop time
step = UPDATE_GAINS ;
}
} else {
2022-03-06 20:32:12 -04:00
if ( now - step_start_time_ms > = step_time_limit_ms | | freqresp . is_cycle_complete ( ) ) {
2021-12-13 00:02:40 -04:00
// we have passed the maximum stop time
step = UPDATE_GAINS ;
}
}
}
2021-01-17 22:28:20 -04:00
// update gains for the rate p up tune type
2020-10-19 23:51:05 -03:00
void AC_AutoTune_Heli : : updating_rate_p_up_all ( AxisType test_axis )
{
switch ( test_axis ) {
case ROLL :
2021-01-18 00:16:08 -04:00
updating_rate_p_up ( tune_roll_rp , test_freq , test_gain , test_phase , freq_cnt , max_rate_p ) ;
2020-10-19 23:51:05 -03:00
break ;
case PITCH :
2021-01-18 00:16:08 -04:00
updating_rate_p_up ( tune_pitch_rp , test_freq , test_gain , test_phase , freq_cnt , max_rate_p ) ;
2020-10-19 23:51:05 -03:00
break ;
case YAW :
2023-03-06 05:39:47 -04:00
case YAW_D :
2021-01-18 00:16:08 -04:00
updating_rate_p_up ( tune_yaw_rp , test_freq , test_gain , test_phase , freq_cnt , max_rate_p ) ;
2020-10-19 23:51:05 -03:00
break ;
}
}
2021-01-17 22:28:20 -04:00
// update gains for the rate d up tune type
2020-10-19 23:51:05 -03:00
void AC_AutoTune_Heli : : updating_rate_d_up_all ( AxisType test_axis )
{
switch ( test_axis ) {
case ROLL :
updating_rate_d_up ( tune_roll_rd , test_freq , test_gain , test_phase , freq_cnt , max_rate_d ) ;
break ;
case PITCH :
updating_rate_d_up ( tune_pitch_rd , test_freq , test_gain , test_phase , freq_cnt , max_rate_d ) ;
break ;
case YAW :
2023-03-06 05:39:47 -04:00
case YAW_D :
2020-10-19 23:51:05 -03:00
updating_rate_d_up ( tune_yaw_rd , test_freq , test_gain , test_phase , freq_cnt , max_rate_d ) ;
break ;
}
}
2021-01-17 22:28:20 -04:00
// update gains for the rate ff up tune type
2020-10-19 23:51:05 -03:00
void AC_AutoTune_Heli : : updating_rate_ff_up_all ( AxisType test_axis )
{
switch ( test_axis ) {
case ROLL :
2024-04-04 22:42:18 -03:00
updating_rate_ff_up ( tune_roll_rff , test_freq , test_gain , test_phase , freq_cnt ) ;
2020-10-19 23:51:05 -03:00
break ;
case PITCH :
2024-04-04 22:42:18 -03:00
updating_rate_ff_up ( tune_pitch_rff , test_freq , test_gain , test_phase , freq_cnt ) ;
2020-10-19 23:51:05 -03:00
break ;
case YAW :
2023-03-06 05:39:47 -04:00
case YAW_D :
2024-04-04 22:42:18 -03:00
updating_rate_ff_up ( tune_yaw_rff , test_freq , test_gain , test_phase , freq_cnt ) ;
2021-01-18 00:16:08 -04:00
// TODO make FF updating routine determine when to set rff gain to zero based on A/C response
if ( tune_yaw_rff < = AUTOTUNE_RFF_MIN & & counter = = AUTOTUNE_SUCCESS_COUNT ) {
tune_yaw_rff = 0.0f ;
}
2020-10-19 23:51:05 -03:00
break ;
}
}
2021-01-17 22:28:20 -04:00
// update gains for the angle p up tune type
2020-10-19 23:51:05 -03:00
void AC_AutoTune_Heli : : updating_angle_p_up_all ( AxisType test_axis )
{
2022-05-03 00:35:50 -03:00
attitude_control - > bf_feedforward ( orig_bf_feedforward ) ;
2020-10-19 23:51:05 -03:00
switch ( test_axis ) {
case ROLL :
updating_angle_p_up ( tune_roll_sp , test_freq , test_gain , test_phase , freq_cnt ) ;
break ;
case PITCH :
updating_angle_p_up ( tune_pitch_sp , test_freq , test_gain , test_phase , freq_cnt ) ;
break ;
case YAW :
2023-03-06 05:39:47 -04:00
case YAW_D :
2020-10-19 23:51:05 -03:00
updating_angle_p_up ( tune_yaw_sp , test_freq , test_gain , test_phase , freq_cnt ) ;
break ;
}
}
2021-01-17 22:28:20 -04:00
// update gains for the max gain tune type
2020-10-19 23:51:05 -03:00
void AC_AutoTune_Heli : : updating_max_gains_all ( AxisType test_axis )
{
switch ( test_axis ) {
case ROLL :
updating_max_gains ( & test_freq [ 0 ] , & test_gain [ 0 ] , & test_phase [ 0 ] , freq_cnt , max_rate_p , max_rate_d , tune_roll_rp , tune_roll_rd ) ;
break ;
case PITCH :
updating_max_gains ( & test_freq [ 0 ] , & test_gain [ 0 ] , & test_phase [ 0 ] , freq_cnt , max_rate_p , max_rate_d , tune_pitch_rp , tune_pitch_rd ) ;
break ;
case YAW :
2023-03-06 05:39:47 -04:00
case YAW_D :
2020-10-19 23:51:05 -03:00
updating_max_gains ( & test_freq [ 0 ] , & test_gain [ 0 ] , & test_phase [ 0 ] , freq_cnt , max_rate_p , max_rate_d , tune_yaw_rp , tune_yaw_rd ) ;
2021-01-18 00:16:08 -04:00
// rate P and rate D can be non zero for yaw and need to be included in the max allowed gain
if ( ! is_zero ( max_rate_p . max_allowed ) & & counter = = AUTOTUNE_SUCCESS_COUNT ) {
max_rate_p . max_allowed + = tune_yaw_rp ;
}
if ( ! is_zero ( max_rate_d . max_allowed ) & & counter = = AUTOTUNE_SUCCESS_COUNT ) {
max_rate_d . max_allowed + = tune_yaw_rd ;
}
2020-10-19 23:51:05 -03:00
break ;
}
}
2021-12-27 00:53:50 -04:00
// set gains post tune for the tune type
void AC_AutoTune_Heli : : set_gains_post_tune ( AxisType test_axis )
{
switch ( tune_type ) {
case RD_UP :
switch ( test_axis ) {
case ROLL :
tune_roll_rd = MAX ( 0.0f , tune_roll_rd * AUTOTUNE_RD_BACKOFF ) ;
break ;
case PITCH :
tune_pitch_rd = MAX ( 0.0f , tune_pitch_rd * AUTOTUNE_RD_BACKOFF ) ;
break ;
case YAW :
2023-03-06 05:39:47 -04:00
case YAW_D :
2022-01-07 14:37:02 -04:00
tune_yaw_rd = MAX ( 0.0f , tune_yaw_rd * AUTOTUNE_RD_BACKOFF ) ;
2021-12-27 00:53:50 -04:00
break ;
}
break ;
case RP_UP :
switch ( test_axis ) {
case ROLL :
2022-01-07 14:37:02 -04:00
tune_roll_rp = MAX ( 0.0f , tune_roll_rp * AUTOTUNE_RP_BACKOFF ) ;
2021-12-27 00:53:50 -04:00
break ;
case PITCH :
2022-01-07 14:37:02 -04:00
tune_pitch_rp = MAX ( 0.0f , tune_pitch_rp * AUTOTUNE_RP_BACKOFF ) ;
2021-12-27 00:53:50 -04:00
break ;
case YAW :
2023-03-06 05:39:47 -04:00
case YAW_D :
2021-12-27 00:53:50 -04:00
tune_yaw_rp = MAX ( AUTOTUNE_RP_MIN , tune_yaw_rp * AUTOTUNE_RP_BACKOFF ) ;
break ;
}
break ;
case SP_UP :
switch ( test_axis ) {
case ROLL :
tune_roll_sp = MAX ( AUTOTUNE_SP_MIN , tune_roll_sp * AUTOTUNE_SP_BACKOFF ) ;
break ;
case PITCH :
tune_pitch_sp = MAX ( AUTOTUNE_SP_MIN , tune_pitch_sp * AUTOTUNE_SP_BACKOFF ) ;
break ;
case YAW :
2023-03-06 05:39:47 -04:00
case YAW_D :
2021-12-27 00:53:50 -04:00
tune_yaw_sp = MAX ( AUTOTUNE_SP_MIN , tune_yaw_sp * AUTOTUNE_SP_BACKOFF ) ;
break ;
}
break ;
case RFF_UP :
2022-01-07 14:37:02 -04:00
break ;
default :
2021-12-27 00:53:50 -04:00
break ;
}
}
2020-10-19 23:51:05 -03:00
// updating_rate_ff_up - adjust FF to ensure the target is reached
2023-10-11 04:41:49 -03:00
// FF is adjusted until rate requested is achieved
2024-04-04 22:42:18 -03:00
void AC_AutoTune_Heli : : updating_rate_ff_up ( float & tune_ff , float * freq , float * gain , float * phase , uint8_t & frq_cnt )
2020-10-19 23:51:05 -03:00
{
2024-04-04 22:42:18 -03:00
float test_freq_incr = 0.05f * 3.14159f * 2.0f ;
if ( phase [ frq_cnt ] > 15.0f ) {
curr_test . freq = curr_test . freq - test_freq_incr ;
freq [ frq_cnt ] = curr_test . freq ;
} else if ( phase [ frq_cnt ] < 0.0f ) {
curr_test . freq = curr_test . freq + test_freq_incr ;
freq [ frq_cnt ] = curr_test . freq ;
} else {
if ( ( gain [ frq_cnt ] > 0.1 & & gain [ frq_cnt ] < 0.95 ) | | gain [ frq_cnt ] > 1.0 ) {
tune_ff = tune_ff / gain [ frq_cnt ] ;
} else if ( gain [ frq_cnt ] > = 0.95 & & gain [ frq_cnt ] < = 1.0 ) {
2021-01-18 00:16:08 -04:00
counter = AUTOTUNE_SUCCESS_COUNT ;
2024-04-04 22:42:18 -03:00
// reset curr_test.freq and frq_cnt for next test
curr_test . freq = freq [ 0 ] ;
frq_cnt = 0 ;
2021-01-18 00:16:08 -04:00
}
2024-04-04 22:42:18 -03:00
}
if ( counter = = AUTOTUNE_SUCCESS_COUNT ) {
start_freq = 0.0f ; //initializes next test that uses dwell test
2020-10-19 23:51:05 -03:00
} else {
2024-04-04 22:42:18 -03:00
start_freq = curr_test . freq ;
stop_freq = curr_test . freq ;
2020-10-19 23:51:05 -03:00
}
}
2021-12-06 00:27:42 -04:00
// updating_rate_p_up - uses maximum allowable gain determined from max_gain test to determine rate p gain that does not exceed exceed max response gain
2021-01-18 00:16:08 -04:00
void AC_AutoTune_Heli : : updating_rate_p_up ( float & tune_p , float * freq , float * gain , float * phase , uint8_t & frq_cnt , max_gain_data & max_gain_p )
2020-10-19 23:51:05 -03:00
{
2021-01-18 00:16:08 -04:00
float test_freq_incr = 0.25f * 3.14159f * 2.0f ;
if ( frq_cnt < 12 & & is_equal ( start_freq , stop_freq ) ) {
2022-01-16 19:13:59 -04:00
if ( phase [ frq_cnt ] < = 180.0f & & ! is_zero ( phase [ frq_cnt ] ) ) {
2021-10-06 00:50:02 -03:00
rp_prev_good_frq_cnt = frq_cnt ;
2021-01-18 00:16:08 -04:00
} else if ( frq_cnt > 1 & & phase [ frq_cnt ] > phase [ frq_cnt - 1 ] + 360.0f & & ! is_zero ( phase [ frq_cnt ] ) ) {
if ( phase [ frq_cnt ] - 360.0f < 180.0f ) {
2021-10-06 00:50:02 -03:00
rp_prev_good_frq_cnt = frq_cnt ;
2021-01-18 00:16:08 -04:00
}
2022-01-16 19:13:59 -04:00
} else if ( frq_cnt > 1 & & phase [ frq_cnt ] > 200.0f & & ! is_zero ( phase [ frq_cnt ] ) ) {
2021-01-18 00:16:08 -04:00
frq_cnt = 11 ;
}
frq_cnt + + ;
if ( frq_cnt = = 12 ) {
2021-10-06 00:50:02 -03:00
freq [ frq_cnt ] = freq [ rp_prev_good_frq_cnt ] ;
2022-02-08 19:31:51 -04:00
curr_test . freq = freq [ frq_cnt ] ;
2021-01-18 00:16:08 -04:00
} else {
freq [ frq_cnt ] = freq [ frq_cnt - 1 ] + test_freq_incr ;
2022-02-08 19:31:51 -04:00
curr_test . freq = freq [ frq_cnt ] ;
2021-01-18 00:16:08 -04:00
}
2022-01-16 19:13:59 -04:00
} else if ( is_equal ( start_freq , stop_freq ) ) {
if ( phase [ frq_cnt ] > 180.0f ) {
2022-02-08 19:31:51 -04:00
curr_test . freq = curr_test . freq - 0.5 * test_freq_incr ;
freq [ frq_cnt ] = curr_test . freq ;
2021-01-18 00:16:08 -04:00
} else if ( phase [ frq_cnt ] < 160.0f ) {
2022-02-08 19:31:51 -04:00
curr_test . freq = curr_test . freq + 0.5 * test_freq_incr ;
freq [ frq_cnt ] = curr_test . freq ;
2021-01-18 00:16:08 -04:00
} else if ( phase [ frq_cnt ] < = 180.0f & & phase [ frq_cnt ] > = 160.0f ) {
if ( gain [ frq_cnt ] < max_resp_gain & & tune_p < 0.6f * max_gain_p . max_allowed ) {
tune_p + = 0.05f * max_gain_p . max_allowed ;
} else {
counter = AUTOTUNE_SUCCESS_COUNT ;
2022-02-08 19:31:51 -04:00
// reset curr_test.freq and frq_cnt for next test
curr_test . freq = freq [ 0 ] ;
2021-01-18 00:16:08 -04:00
frq_cnt = 0 ;
tune_p - = 0.05f * max_gain_p . max_allowed ;
tune_p = constrain_float ( tune_p , 0.0f , 0.6f * max_gain_p . max_allowed ) ;
}
}
}
if ( counter = = AUTOTUNE_SUCCESS_COUNT ) {
start_freq = 0.0f ; //initializes next test that uses dwell test
} else {
2022-02-08 19:31:51 -04:00
start_freq = curr_test . freq ;
stop_freq = curr_test . freq ;
2021-01-18 00:16:08 -04:00
}
2020-10-19 23:51:05 -03:00
}
2021-12-06 00:27:42 -04:00
// updating_rate_d_up - uses maximum allowable gain determined from max_gain test to determine rate d gain where the response gain is at a minimum
2020-10-19 23:51:05 -03:00
void AC_AutoTune_Heli : : updating_rate_d_up ( float & tune_d , float * freq , float * gain , float * phase , uint8_t & frq_cnt , max_gain_data & max_gain_d )
{
2022-01-14 00:13:47 -04:00
float test_freq_incr = 0.25f * 3.14159f * 2.0f ; // set for 1/4 hz increments
2020-10-19 23:51:05 -03:00
2022-01-14 00:13:47 -04:00
// frequency sweep was conducted. check to see if freq for 180 deg phase was determined and start there if it was
2021-01-18 00:16:08 -04:00
if ( ! is_equal ( start_freq , stop_freq ) ) {
2022-02-08 19:31:51 -04:00
if ( ! is_zero ( sweep . ph180 . freq ) ) {
freq [ frq_cnt ] = sweep . ph180 . freq - 0.5f * test_freq_incr ;
2022-01-14 00:13:47 -04:00
frq_cnt = 12 ;
2021-01-18 00:16:08 -04:00
freq_cnt_max = frq_cnt ;
} else {
2022-01-14 00:13:47 -04:00
frq_cnt = 1 ;
freq [ frq_cnt ] = min_sweep_freq ;
2021-01-18 00:16:08 -04:00
}
2022-02-08 19:31:51 -04:00
curr_test . freq = freq [ frq_cnt ] ;
2021-01-18 00:16:08 -04:00
}
2022-01-14 00:13:47 -04:00
// if sweep failed to find frequency for 180 phase then use dwell to find frequency
2021-01-18 00:16:08 -04:00
if ( frq_cnt < 12 & & is_equal ( start_freq , stop_freq ) ) {
2022-01-14 00:13:47 -04:00
if ( phase [ frq_cnt ] < = 180.0f & & ! is_zero ( phase [ frq_cnt ] ) ) {
2021-10-06 00:50:02 -03:00
rd_prev_good_frq_cnt = frq_cnt ;
2020-10-19 23:51:05 -03:00
} else if ( frq_cnt > 1 & & phase [ frq_cnt ] > phase [ frq_cnt - 1 ] + 360.0f & & ! is_zero ( phase [ frq_cnt ] ) ) {
if ( phase [ frq_cnt ] - 360.0f < 180.0f ) {
2021-10-06 00:50:02 -03:00
rd_prev_good_frq_cnt = frq_cnt ;
2020-10-19 23:51:05 -03:00
}
2022-01-16 19:13:59 -04:00
} else if ( frq_cnt > 1 & & phase [ frq_cnt ] > 200.0f & & ! is_zero ( phase [ frq_cnt ] ) ) {
2020-10-19 23:51:05 -03:00
frq_cnt = 11 ;
}
frq_cnt + + ;
if ( frq_cnt = = 12 ) {
2021-10-06 00:50:02 -03:00
freq [ frq_cnt ] = freq [ rd_prev_good_frq_cnt ] ;
2022-02-08 19:31:51 -04:00
curr_test . freq = freq [ frq_cnt ] ;
2020-10-19 23:51:05 -03:00
} else {
freq [ frq_cnt ] = freq [ frq_cnt - 1 ] + test_freq_incr ;
2022-02-08 19:31:51 -04:00
curr_test . freq = freq [ frq_cnt ] ;
2020-10-19 23:51:05 -03:00
}
2022-01-14 00:13:47 -04:00
} else if ( is_equal ( start_freq , stop_freq ) ) {
2022-01-16 19:13:59 -04:00
if ( phase [ frq_cnt ] > 180.0f ) {
2022-02-08 19:31:51 -04:00
curr_test . freq = curr_test . freq - 0.5 * test_freq_incr ;
freq [ frq_cnt ] = curr_test . freq ;
2021-01-18 00:16:08 -04:00
} else if ( phase [ frq_cnt ] < 160.0f ) {
2022-02-08 19:31:51 -04:00
curr_test . freq = curr_test . freq + 0.5 * test_freq_incr ;
freq [ frq_cnt ] = curr_test . freq ;
2021-01-18 00:16:08 -04:00
} else if ( phase [ frq_cnt ] < = 180.0f & & phase [ frq_cnt ] > = 160.0f ) {
2021-10-06 00:50:02 -03:00
if ( ( gain [ frq_cnt ] < rd_prev_gain | | is_zero ( rd_prev_gain ) ) & & tune_d < 0.6f * max_gain_d . max_allowed ) {
2021-01-18 00:16:08 -04:00
tune_d + = 0.05f * max_gain_d . max_allowed ;
2021-10-06 00:50:02 -03:00
rd_prev_gain = gain [ frq_cnt ] ;
2021-01-18 00:16:08 -04:00
} else {
counter = AUTOTUNE_SUCCESS_COUNT ;
2022-02-08 19:31:51 -04:00
// reset curr_test.freq and frq_cnt for next test
curr_test . freq = freq [ 0 ] ;
2021-01-18 00:16:08 -04:00
frq_cnt = 0 ;
2021-10-06 00:50:02 -03:00
rd_prev_gain = 0.0f ;
2021-01-18 00:16:08 -04:00
tune_d - = 0.05f * max_gain_d . max_allowed ;
tune_d = constrain_float ( tune_d , 0.0f , 0.6f * max_gain_d . max_allowed ) ;
}
}
2020-10-19 23:51:05 -03:00
}
2021-01-18 00:16:08 -04:00
if ( counter = = AUTOTUNE_SUCCESS_COUNT ) {
start_freq = 0.0f ; //initializes next test that uses dwell test
2022-01-16 19:13:59 -04:00
reset_sweep_variables ( ) ;
2021-01-18 00:16:08 -04:00
} else {
2022-02-08 19:31:51 -04:00
start_freq = curr_test . freq ;
stop_freq = curr_test . freq ;
2021-01-18 00:16:08 -04:00
}
2020-10-19 23:51:05 -03:00
}
2021-12-06 00:27:42 -04:00
// updating_angle_p_up - determines maximum angle p gain for pitch and roll
2020-10-19 23:51:05 -03:00
void AC_AutoTune_Heli : : updating_angle_p_up ( float & tune_p , float * freq , float * gain , float * phase , uint8_t & frq_cnt )
2021-01-18 00:16:08 -04:00
{
float test_freq_incr = 0.5f * 3.14159f * 2.0f ;
float gain_incr = 0.5f ;
if ( ! is_equal ( start_freq , stop_freq ) ) {
2022-02-08 19:31:51 -04:00
if ( ! is_zero ( sweep . maxgain . freq ) ) {
2022-01-16 19:13:59 -04:00
frq_cnt = 12 ;
2022-02-08 19:31:51 -04:00
freq [ frq_cnt ] = sweep . maxgain . freq - 0.5f * test_freq_incr ;
2021-01-18 00:16:08 -04:00
freq_cnt_max = frq_cnt ;
} else {
2022-01-16 19:13:59 -04:00
frq_cnt = 1 ;
freq [ frq_cnt ] = min_sweep_freq ;
freq_cnt_max = 0 ;
2021-01-18 00:16:08 -04:00
}
2022-02-08 19:31:51 -04:00
curr_test . freq = freq [ frq_cnt ] ;
2021-01-18 00:16:08 -04:00
}
if ( freq_cnt < 12 & & is_equal ( start_freq , stop_freq ) ) {
2022-01-16 19:13:59 -04:00
if ( gain [ freq_cnt ] > max_resp_gain & & tune_p > AUTOTUNE_SP_MIN ) {
2021-01-18 00:16:08 -04:00
// exceeded max response gain already, reduce tuning gain to remain under max response gain
tune_p - = gain_incr ;
// force counter to stay on frequency
freq_cnt - = 1 ;
} else if ( gain [ freq_cnt ] > max_resp_gain & & tune_p < = AUTOTUNE_SP_MIN ) {
// exceeded max response gain at the minimum allowable tuning gain. terminate testing.
tune_p = AUTOTUNE_SP_MIN ;
counter = AUTOTUNE_SUCCESS_COUNT ;
2023-07-13 21:58:04 -03:00
LOGGER_WRITE_EVENT ( LogEvent : : AUTOTUNE_REACHED_LIMIT ) ;
2021-01-18 00:16:08 -04:00
} else if ( gain [ freq_cnt ] > gain [ freq_cnt_max ] ) {
freq_cnt_max = freq_cnt ;
phase_max = phase [ freq_cnt ] ;
2021-10-06 00:50:02 -03:00
sp_prev_gain = gain [ freq_cnt ] ;
2022-01-16 19:13:59 -04:00
} else if ( freq [ freq_cnt ] > max_sweep_freq | | ( gain [ freq_cnt ] > 0.0f & & gain [ freq_cnt ] < 0.5f ) ) {
2021-01-18 00:16:08 -04:00
// must be past peak, continue on to determine angle p
freq_cnt = 11 ;
}
freq_cnt + + ;
if ( freq_cnt = = 12 ) {
freq [ freq_cnt ] = freq [ freq_cnt_max ] ;
2022-02-08 19:31:51 -04:00
curr_test . freq = freq [ freq_cnt ] ;
2021-01-18 00:16:08 -04:00
} else {
freq [ freq_cnt ] = freq [ freq_cnt - 1 ] + test_freq_incr ;
2022-02-08 19:31:51 -04:00
curr_test . freq = freq [ freq_cnt ] ;
2021-01-18 00:16:08 -04:00
}
}
// once finished with sweep of frequencies, cnt = 12 is used to then tune for max response gain
if ( freq_cnt > = 12 & & is_equal ( start_freq , stop_freq ) ) {
if ( gain [ freq_cnt ] < max_resp_gain & & tune_p < AUTOTUNE_SP_MAX & & ! find_peak ) {
2023-10-11 04:41:49 -03:00
// keep increasing tuning gain unless phase changes or max response gain is achieved
2021-01-18 00:16:08 -04:00
if ( phase [ freq_cnt ] - phase_max > 20.0f & & phase [ freq_cnt ] < 210.0f ) {
freq [ freq_cnt ] + = 0.5 * test_freq_incr ;
find_peak = true ;
} else {
tune_p + = gain_incr ;
freq [ freq_cnt ] = freq [ freq_cnt_max ] ;
if ( tune_p > = AUTOTUNE_SP_MAX ) {
tune_p = AUTOTUNE_SP_MAX ;
counter = AUTOTUNE_SUCCESS_COUNT ;
2023-07-13 21:58:04 -03:00
LOGGER_WRITE_EVENT ( LogEvent : : AUTOTUNE_REACHED_LIMIT ) ;
2021-01-18 00:16:08 -04:00
}
}
2022-02-08 19:31:51 -04:00
curr_test . freq = freq [ freq_cnt ] ;
2021-10-06 00:50:02 -03:00
sp_prev_gain = gain [ freq_cnt ] ;
2021-01-18 00:16:08 -04:00
} else if ( gain [ freq_cnt ] > 1.1f * max_resp_gain & & tune_p > AUTOTUNE_SP_MIN & & ! find_peak ) {
2021-12-13 23:26:57 -04:00
tune_p - = gain_incr ;
2021-01-18 00:16:08 -04:00
} else if ( find_peak ) {
// find the frequency where the response gain is maximum
2021-10-06 00:50:02 -03:00
if ( gain [ freq_cnt ] > sp_prev_gain ) {
2021-01-18 00:16:08 -04:00
freq [ freq_cnt ] + = 0.5 * test_freq_incr ;
} else {
find_peak = false ;
phase_max = phase [ freq_cnt ] ;
}
2022-02-08 19:31:51 -04:00
curr_test . freq = freq [ freq_cnt ] ;
2021-10-06 00:50:02 -03:00
sp_prev_gain = gain [ freq_cnt ] ;
2021-01-18 00:16:08 -04:00
} else {
// adjust tuning gain so max response gain is not exceeded
2021-10-06 00:50:02 -03:00
if ( sp_prev_gain < max_resp_gain & & gain [ freq_cnt ] > max_resp_gain ) {
float adj_factor = ( max_resp_gain - gain [ freq_cnt ] ) / ( gain [ freq_cnt ] - sp_prev_gain ) ;
2021-01-18 00:16:08 -04:00
tune_p = tune_p + gain_incr * adj_factor ;
}
counter = AUTOTUNE_SUCCESS_COUNT ;
}
}
if ( counter = = AUTOTUNE_SUCCESS_COUNT ) {
start_freq = 0.0f ; //initializes next test that uses dwell test
2022-01-16 19:13:59 -04:00
reset_sweep_variables ( ) ;
2022-02-08 19:31:51 -04:00
curr_test . freq = freq [ 0 ] ;
2022-01-16 19:13:59 -04:00
freq_cnt = 0 ;
2021-01-18 00:16:08 -04:00
} else {
2022-02-08 19:31:51 -04:00
start_freq = curr_test . freq ;
stop_freq = curr_test . freq ;
2021-01-18 00:16:08 -04:00
}
}
2020-10-19 23:51:05 -03:00
// updating_max_gains: use dwells at increasing frequency to determine gain at which instability will occur
void AC_AutoTune_Heli : : updating_max_gains ( float * freq , float * gain , float * phase , uint8_t & frq_cnt , max_gain_data & max_gain_p , max_gain_data & max_gain_d , float & tune_p , float & tune_d )
{
2021-01-18 00:16:08 -04:00
float test_freq_incr = 1.0f * M_PI * 2.0f ;
if ( ! is_equal ( start_freq , stop_freq ) ) {
frq_cnt = 2 ;
2022-02-08 19:31:51 -04:00
if ( ! is_zero ( sweep . ph180 . freq ) ) {
freq [ frq_cnt ] = sweep . ph180 . freq - 0.5f * test_freq_incr ;
2021-01-18 00:16:08 -04:00
} else {
2022-01-21 14:44:36 -04:00
freq [ frq_cnt ] = min_sweep_freq ;
2021-01-18 00:16:08 -04:00
}
2022-02-08 19:31:51 -04:00
curr_test . freq = freq [ frq_cnt ] ;
start_freq = curr_test . freq ;
stop_freq = curr_test . freq ;
2021-01-18 00:16:08 -04:00
} else if ( frq_cnt < 12 & & is_equal ( start_freq , stop_freq ) ) {
if ( frq_cnt > 2 & & phase [ frq_cnt ] > 161.0f & & phase [ frq_cnt ] < 270.0f & &
! find_middle & & ! found_max_p ) {
find_middle = true ;
} else if ( find_middle & & ! found_max_p ) {
if ( phase [ frq_cnt ] > 161.0f ) {
// interpolate between frq_cnt-2 and frq_cnt
max_gain_p . freq = linear_interpolate ( freq [ frq_cnt - 2 ] , freq [ frq_cnt ] , 161.0f , phase [ frq_cnt - 2 ] , phase [ frq_cnt ] ) ;
max_gain_p . gain = linear_interpolate ( gain [ frq_cnt - 2 ] , gain [ frq_cnt ] , 161.0f , phase [ frq_cnt - 2 ] , phase [ frq_cnt ] ) ;
} else {
// interpolate between frq_cnt-1 and frq_cnt
max_gain_p . freq = linear_interpolate ( freq [ frq_cnt ] , freq [ frq_cnt - 1 ] , 161.0f , phase [ frq_cnt ] , phase [ frq_cnt - 1 ] ) ;
max_gain_p . gain = linear_interpolate ( gain [ frq_cnt ] , gain [ frq_cnt - 1 ] , 161.0f , phase [ frq_cnt ] , phase [ frq_cnt - 1 ] ) ;
}
2020-10-19 23:51:05 -03:00
max_gain_p . phase = 161.0f ;
max_gain_p . max_allowed = powf ( 10.0f , - 1 * ( log10f ( max_gain_p . gain ) * 20.0f + 2.42 ) / 20.0f ) ;
2021-01-18 00:16:08 -04:00
// limit max gain allowed to be no more than 2x the max p gain limit to keep initial gains bounded
max_gain_p . max_allowed = constrain_float ( max_gain_p . max_allowed , 0.0f , 2.0f * AUTOTUNE_RP_MAX ) ;
found_max_p = true ;
find_middle = false ;
2022-02-08 19:31:51 -04:00
if ( ! is_zero ( sweep . ph270 . freq ) ) {
2021-01-18 00:16:08 -04:00
// set freq cnt back to reinitialize process
frq_cnt = 1 ; // set to 1 because it will be incremented
// set frequency back at least one test_freq_incr as it will be added
2022-02-08 19:31:51 -04:00
freq [ 1 ] = sweep . ph270 . freq - 1.5f * test_freq_incr ;
2021-01-18 00:16:08 -04:00
}
}
if ( frq_cnt > 2 & & phase [ frq_cnt ] > 251.0f & & phase [ frq_cnt ] < 360.0f & &
! find_middle & & ! found_max_d & & found_max_p ) {
find_middle = true ;
} else if ( find_middle & & found_max_p & & ! found_max_d ) {
if ( phase [ frq_cnt ] > 251.0f ) {
// interpolate between frq_cnt-2 and frq_cnt
max_gain_d . freq = linear_interpolate ( freq [ frq_cnt - 2 ] , freq [ frq_cnt ] , 251.0f , phase [ frq_cnt - 2 ] , phase [ frq_cnt ] ) ;
max_gain_d . gain = linear_interpolate ( gain [ frq_cnt - 2 ] , gain [ frq_cnt ] , 251.0f , phase [ frq_cnt - 2 ] , phase [ frq_cnt ] ) ;
} else {
// interpolate between frq_cnt-1 and frq_cnt
max_gain_d . freq = linear_interpolate ( freq [ frq_cnt ] , freq [ frq_cnt - 1 ] , 251.0f , phase [ frq_cnt ] , phase [ frq_cnt - 1 ] ) ;
max_gain_d . gain = linear_interpolate ( gain [ frq_cnt ] , gain [ frq_cnt - 1 ] , 251.0f , phase [ frq_cnt ] , phase [ frq_cnt - 1 ] ) ;
}
2020-10-19 23:51:05 -03:00
max_gain_d . phase = 251.0f ;
max_gain_d . max_allowed = powf ( 10.0f , - 1 * ( log10f ( max_gain_d . freq * max_gain_d . gain ) * 20.0f + 2.42 ) / 20.0f ) ;
2021-01-18 00:16:08 -04:00
// limit max gain allowed to be no more than 2x the max d gain limit to keep initial gains bounded
max_gain_d . max_allowed = constrain_float ( max_gain_d . max_allowed , 0.0f , 2.0f * AUTOTUNE_RD_MAX ) ;
found_max_d = true ;
find_middle = false ;
2020-10-19 23:51:05 -03:00
}
2021-01-18 00:16:08 -04:00
// stop progression in frequency.
if ( ( frq_cnt > 1 & & phase [ frq_cnt ] > 330.0f & & ! is_zero ( phase [ frq_cnt ] ) ) | | found_max_d ) {
2020-10-19 23:51:05 -03:00
frq_cnt = 11 ;
}
frq_cnt + + ;
if ( frq_cnt = = 12 ) {
counter = AUTOTUNE_SUCCESS_COUNT ;
2021-01-18 00:16:08 -04:00
// reset variables for next test
2022-02-08 19:31:51 -04:00
curr_test . freq = freq [ 0 ] ;
2020-10-19 23:51:05 -03:00
frq_cnt = 0 ;
2021-01-18 00:16:08 -04:00
start_freq = 0.0f ; //initializes next test that uses dwell test
2022-01-16 19:13:59 -04:00
reset_sweep_variables ( ) ;
2020-10-19 23:51:05 -03:00
} else {
2021-01-18 00:16:08 -04:00
if ( frq_cnt = = 3 & & phase [ 2 ] > = 161.0f & & ! found_max_p ) {
// phase greater than 161 deg won't allow max p to be found
// reset freq cnt to 2 and lower dwell freq to push phase below 161 deg
frq_cnt = 2 ;
freq [ frq_cnt ] = freq [ frq_cnt ] - 0.5f * test_freq_incr ;
} else if ( frq_cnt = = 3 & & phase [ 2 ] > = 251.0f & & ! found_max_d ) {
// phase greater than 161 deg won't allow max p to be found
// reset freq cnt to 2 and lower dwell freq to push phase below 161 deg
frq_cnt = 2 ;
freq [ frq_cnt ] = freq [ frq_cnt ] - 0.5f * test_freq_incr ;
} else if ( find_middle ) {
freq [ frq_cnt ] = freq [ frq_cnt - 1 ] - 0.5f * test_freq_incr ;
} else {
freq [ frq_cnt ] = freq [ frq_cnt - 1 ] + test_freq_incr ;
}
2022-02-08 19:31:51 -04:00
curr_test . freq = freq [ frq_cnt ] ;
start_freq = curr_test . freq ;
stop_freq = curr_test . freq ;
2020-10-19 23:51:05 -03:00
}
}
2022-01-07 14:37:02 -04:00
if ( found_max_p & & found_max_d ) {
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " AutoTune: Max rate P freq=%f gain=%f " , ( double ) ( max_rate_p . freq ) , ( double ) ( max_rate_p . gain ) ) ;
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " AutoTune: ph=%f rate_p=%f " , ( double ) ( max_rate_p . phase ) , ( double ) ( max_rate_p . max_allowed ) ) ;
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " AutoTune: Max Rate D freq=%f gain=%f " , ( double ) ( max_rate_d . freq ) , ( double ) ( max_rate_d . gain ) ) ;
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " AutoTune: ph=%f rate_d=%f " , ( double ) ( max_rate_d . phase ) , ( double ) ( max_rate_d . max_allowed ) ) ;
}
2020-10-19 23:51:05 -03:00
}
2024-02-29 02:42:39 -04:00
float AC_AutoTune_Heli : : target_angle_max_rp_cd ( ) const
{
return AUTOTUNE_ANGLE_TARGET_MAX_RP_CD ;
}
float AC_AutoTune_Heli : : target_angle_max_y_cd ( ) const
{
return AUTOTUNE_ANGLE_TARGET_MAX_Y_CD ;
}
float AC_AutoTune_Heli : : target_angle_min_rp_cd ( ) const
{
return AUTOTUNE_ANGLE_TARGET_MIN_RP_CD ;
}
float AC_AutoTune_Heli : : target_angle_min_y_cd ( ) const
{
return AUTOTUNE_ANGLE_TARGET_MIN_Y_CD ;
}
float AC_AutoTune_Heli : : angle_lim_max_rp_cd ( ) const
{
return AUTOTUNE_ANGLE_MAX_RP_CD ;
}
float AC_AutoTune_Heli : : angle_lim_neg_rpy_cd ( ) const
{
return AUTOTUNE_ANGLE_NEG_RPY_CD ;
}
2023-07-13 21:58:04 -03:00
# if HAL_LOGGING_ENABLED
2021-12-06 00:27:42 -04:00
// log autotune summary data
2020-10-19 23:51:05 -03:00
void AC_AutoTune_Heli : : Log_AutoTune ( )
{
2022-01-07 14:37:02 -04:00
switch ( axis ) {
case ROLL :
Log_Write_AutoTune ( axis , tune_type , test_freq [ freq_cnt ] , test_gain [ freq_cnt ] , test_phase [ freq_cnt ] , tune_roll_rff , tune_roll_rp , tune_roll_rd , tune_roll_sp , test_accel_max ) ;
break ;
case PITCH :
Log_Write_AutoTune ( axis , tune_type , test_freq [ freq_cnt ] , test_gain [ freq_cnt ] , test_phase [ freq_cnt ] , tune_pitch_rff , tune_pitch_rp , tune_pitch_rd , tune_pitch_sp , test_accel_max ) ;
break ;
case YAW :
2023-03-06 05:39:47 -04:00
case YAW_D :
2022-01-07 14:37:02 -04:00
Log_Write_AutoTune ( axis , tune_type , test_freq [ freq_cnt ] , test_gain [ freq_cnt ] , test_phase [ freq_cnt ] , tune_yaw_rff , tune_yaw_rp , tune_yaw_rd , tune_yaw_sp , test_accel_max ) ;
break ;
}
2021-01-18 00:16:08 -04:00
}
2021-12-06 00:27:42 -04:00
// log autotune time history results for command, angular rate, and attitude
2021-01-18 00:16:08 -04:00
void AC_AutoTune_Heli : : Log_AutoTuneDetails ( )
{
if ( tune_type = = SP_UP ) {
Log_Write_AutoTuneDetails ( command_out , 0.0f , 0.0f , filt_target_rate , rotation_rate ) ;
2020-10-19 23:51:05 -03:00
} else {
2021-01-18 00:16:08 -04:00
Log_Write_AutoTuneDetails ( command_out , filt_target_rate , rotation_rate , 0.0f , 0.0f ) ;
2020-10-19 23:51:05 -03:00
}
}
2021-12-06 00:27:42 -04:00
// log autotune frequency response data
2021-01-18 00:16:08 -04:00
void AC_AutoTune_Heli : : Log_AutoTuneSweep ( )
2020-10-19 23:51:05 -03:00
{
2022-02-08 19:31:51 -04:00
Log_Write_AutoTuneSweep ( curr_test . freq , curr_test . gain , curr_test . phase ) ;
2020-10-19 23:51:05 -03:00
}
2021-01-18 00:16:08 -04:00
// @LoggerMessage: ATNH
// @Description: Heli AutoTune
// @Vehicles: Copter
2020-10-19 23:51:05 -03:00
// @Field: TimeUS: Time since system startup
// @Field: Axis: which axis is currently being tuned
// @Field: TuneStep: step in autotune process
// @Field: Freq: target dwell frequency
// @Field: Gain: measured gain of dwell
// @Field: Phase: measured phase of dwell
// @Field: RFF: new rate gain FF term
// @Field: RP: new rate gain P term
// @Field: RD: new rate gain D term
// @Field: SP: new angle P term
2021-01-18 00:16:08 -04:00
// @Field: ACC: new max accel term
2020-10-19 23:51:05 -03:00
2021-12-06 00:27:42 -04:00
// Write an Autotune summary data packet
2021-01-18 00:16:08 -04:00
void AC_AutoTune_Heli : : Log_Write_AutoTune ( uint8_t _axis , uint8_t tune_step , float dwell_freq , float meas_gain , float meas_phase , float new_gain_rff , float new_gain_rp , float new_gain_rd , float new_gain_sp , float max_accel )
2020-10-19 23:51:05 -03:00
{
AP : : logger ( ) . Write (
2021-01-18 00:16:08 -04:00
" ATNH " ,
" TimeUS,Axis,TuneStep,Freq,Gain,Phase,RFF,RP,RD,SP,ACC " ,
" s--E-d----- " ,
" F--000----- " ,
" QBBffffffff " ,
2020-10-19 23:51:05 -03:00
AP_HAL : : micros64 ( ) ,
axis ,
tune_step ,
dwell_freq ,
meas_gain ,
meas_phase ,
new_gain_rff ,
new_gain_rp ,
new_gain_rd ,
2021-01-18 00:16:08 -04:00
new_gain_sp ,
max_accel ) ;
2020-10-19 23:51:05 -03:00
}
2021-12-06 00:27:42 -04:00
// Write an Autotune detailed data packet
2021-01-18 00:16:08 -04:00
void AC_AutoTune_Heli : : Log_Write_AutoTuneDetails ( float motor_cmd , float tgt_rate_rads , float rate_rads , float tgt_ang_rad , float ang_rad )
2020-10-19 23:51:05 -03:00
{
2021-01-18 00:16:08 -04:00
// @LoggerMessage: ATDH
// @Description: Heli AutoTune data packet
// @Vehicles: Copter
2020-10-19 23:51:05 -03:00
// @Field: TimeUS: Time since system startup
// @Field: Cmd: current motor command
// @Field: TRate: current target angular rate
// @Field: Rate: current angular rate
2021-01-18 00:16:08 -04:00
// @Field: TAng: current target angle
// @Field: Ang: current angle
2020-10-19 23:51:05 -03:00
AP : : logger ( ) . WriteStreaming (
2021-01-18 00:16:08 -04:00
" ATDH " ,
" TimeUS,Cmd,TRate,Rate,TAng,Ang " ,
" s-kkdd " ,
" F00000 " ,
" Qfffff " ,
AP_HAL : : micros64 ( ) ,
motor_cmd ,
tgt_rate_rads * 57.3 ,
rate_rads * 57.3f ,
tgt_ang_rad * 57.3 ,
ang_rad * 57.3f ) ;
}
2021-12-06 00:27:42 -04:00
// Write an Autotune frequency response data packet
2021-01-18 00:16:08 -04:00
void AC_AutoTune_Heli : : Log_Write_AutoTuneSweep ( float freq , float gain , float phase )
{
// @LoggerMessage: ATSH
// @Description: Heli AutoTune Sweep packet
// @Vehicles: Copter
// @Field: TimeUS: Time since system startup
// @Field: freq: current frequency
// @Field: gain: current response gain
// @Field: phase: current response phase
AP : : logger ( ) . WriteStreaming (
" ATSH " ,
" TimeUS,freq,gain,phase " ,
" sE-d " ,
2020-10-19 23:51:05 -03:00
" F000 " ,
" Qfff " ,
AP_HAL : : micros64 ( ) ,
2021-01-18 00:16:08 -04:00
freq ,
gain ,
phase ) ;
2020-10-19 23:51:05 -03:00
}
2023-07-13 21:58:04 -03:00
# endif // HAL_LOGGING_ENABLED
2020-10-19 23:51:05 -03:00
2023-10-11 04:41:49 -03:00
// reset the test variables for each vehicle
2021-12-22 23:37:31 -04:00
void AC_AutoTune_Heli : : reset_vehicle_test_variables ( )
{
// reset dwell test variables if sweep was interrupted in order to restart sweep
if ( ! is_equal ( start_freq , stop_freq ) ) {
freq_cnt = 0 ;
start_freq = 0.0f ;
stop_freq = 0.0f ;
}
}
2022-01-07 14:37:02 -04:00
// reset the update gain variables for heli
void AC_AutoTune_Heli : : reset_update_gain_variables ( )
{
// reset feedforward update gain variables
ff_up_first_iter = true ;
first_dir_complete = false ;
// reset max gain variables
2022-01-16 19:13:59 -04:00
reset_maxgains_update_gain_variables ( ) ;
// reset rd_up variables
rd_prev_good_frq_cnt = 0 ;
rd_prev_gain = 0.0f ;
// reset rp_up variables
rp_prev_good_frq_cnt = 0 ;
// reset sp_up variables
phase_max = 0.0f ;
sp_prev_gain = 0.0f ;
find_peak = false ;
}
// reset the max_gains update gain variables
void AC_AutoTune_Heli : : reset_maxgains_update_gain_variables ( )
{
2022-02-08 19:46:44 -04:00
max_rate_p = { } ;
max_rate_d = { } ;
2022-01-07 14:37:02 -04:00
found_max_p = false ;
found_max_d = false ;
find_middle = false ;
}
2022-01-16 19:13:59 -04:00
// reset the max_gains update gain variables
void AC_AutoTune_Heli : : reset_sweep_variables ( )
{
2022-02-08 19:46:44 -04:00
sweep . ph180 = { } ;
sweep . ph270 = { } ;
sweep . maxgain = { } ;
2022-02-08 19:31:51 -04:00
2022-01-16 19:13:59 -04:00
sweep . progress = 0 ;
}
2021-12-06 00:27:42 -04:00
// set the tuning test sequence
2021-01-18 00:16:08 -04:00
void AC_AutoTune_Heli : : set_tune_sequence ( )
{
uint8_t seq_cnt = 0 ;
if ( seq_bitmask & AUTOTUNE_SEQ_BITMASK_VFF ) {
tune_seq [ seq_cnt ] = RFF_UP ;
seq_cnt + + ;
}
if ( seq_bitmask & AUTOTUNE_SEQ_BITMASK_RATE_D ) {
tune_seq [ seq_cnt ] = MAX_GAINS ;
seq_cnt + + ;
tune_seq [ seq_cnt ] = RD_UP ;
seq_cnt + + ;
tune_seq [ seq_cnt ] = RP_UP ;
seq_cnt + + ;
}
if ( seq_bitmask & AUTOTUNE_SEQ_BITMASK_ANGLE_P ) {
tune_seq [ seq_cnt ] = SP_UP ;
seq_cnt + + ;
}
if ( seq_bitmask & AUTOTUNE_SEQ_BITMASK_MAX_GAIN & & ! ( seq_bitmask & AUTOTUNE_SEQ_BITMASK_RATE_D ) ) {
tune_seq [ seq_cnt ] = MAX_GAINS ;
seq_cnt + + ;
}
2022-05-08 00:28:50 -03:00
if ( seq_bitmask & AUTOTUNE_SEQ_BITMASK_TUNE_CHECK ) {
tune_seq [ seq_cnt ] = TUNE_CHECK ;
seq_cnt + + ;
}
2021-01-18 00:16:08 -04:00
tune_seq [ seq_cnt ] = TUNE_COMPLETE ;
}
2021-12-28 00:23:19 -04:00
// get_testing_step_timeout_ms accessor
uint32_t AC_AutoTune_Heli : : get_testing_step_timeout_ms ( ) const
{
return AUTOTUNE_TESTING_STEP_TIMEOUT_MS ;
}
2022-01-21 14:44:36 -04:00
// exceeded_freq_range - ensures tuning remains inside frequency range
bool AC_AutoTune_Heli : : exceeded_freq_range ( float frequency )
{
bool ret = false ;
if ( frequency < min_sweep_freq | | frequency > max_sweep_freq ) {
ret = true ;
}
return ret ;
}
2023-12-07 20:03:15 -04:00
# endif // AC_AUTOTUNE_ENABLED