2015-05-29 23:12:49 -03:00
# include "Copter.h"
2015-04-16 01:27:06 -03:00
/*
2023-10-07 07:51:05 -03:00
* Function to update various parameters in flight using the TRANSMITTER_TUNING channel knob
2021-10-22 06:02:44 -03:00
* This should not be confused with the AutoTune feature which can be found in control_autotune . cpp
2015-04-16 01:27:06 -03:00
*/
2023-10-07 07:51:05 -03:00
// tuning - updates parameters based on the ch6 TRANSMITTER_TUNING channel knob's position
2015-04-16 01:27:06 -03:00
// should be called at 3.3hz
2019-04-03 09:32:26 -03:00
void Copter : : tuning ( )
{
2023-10-07 07:51:05 -03:00
const RC_Channel * rc_tuning = rc ( ) . find_channel_for_option ( RC_Channel : : AUX_FUNC : : TRANSMITTER_TUNING ) ;
2015-04-16 01:27:06 -03:00
2023-10-07 07:51:05 -03:00
// exit immediately if tuning channel is not set
if ( rc_tuning = = nullptr ) {
return ;
}
2019-04-03 09:32:26 -03:00
// exit immediately if the tuning function is not set or min and max are both zero
if ( ( g . radio_tuning < = 0 ) | | ( is_zero ( g2 . tuning_min . get ( ) ) & & is_zero ( g2 . tuning_max . get ( ) ) ) ) {
2015-04-16 01:27:06 -03:00
return ;
}
2019-04-03 09:32:26 -03:00
// exit immediately when radio failsafe is invoked or transmitter has not been turned on
2023-10-07 07:51:05 -03:00
if ( failsafe . radio | | failsafe . radio_counter ! = 0 | | rc_tuning - > get_radio_in ( ) = = 0 ) {
2019-04-03 09:32:26 -03:00
return ;
}
2023-10-07 07:51:05 -03:00
const uint16_t radio_in = rc_tuning - > get_radio_in ( ) ;
float tuning_value = linear_interpolate ( g2 . tuning_min , g2 . tuning_max , radio_in , rc_tuning - > get_radio_min ( ) , rc_tuning - > get_radio_max ( ) ) ;
2023-07-13 21:58:09 -03:00
# if HAL_LOGGING_ENABLED
2019-04-03 09:32:26 -03:00
Log_Write_Parameter_Tuning ( g . radio_tuning , tuning_value , g2 . tuning_min , g2 . tuning_max ) ;
2023-07-13 21:58:09 -03:00
# endif
2015-04-26 21:29:04 -03:00
2015-04-16 01:27:06 -03:00
switch ( g . radio_tuning ) {
// Roll, Pitch tuning
2015-03-24 17:43:58 -03:00
case TUNING_STABILIZE_ROLL_PITCH_KP :
2024-07-30 08:37:51 -03:00
attitude_control - > get_angle_roll_p ( ) . set_kP ( tuning_value ) ;
attitude_control - > get_angle_pitch_p ( ) . set_kP ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2015-03-24 17:43:58 -03:00
case TUNING_RATE_ROLL_PITCH_KP :
2024-07-30 08:37:51 -03:00
attitude_control - > get_rate_roll_pid ( ) . set_kP ( tuning_value ) ;
attitude_control - > get_rate_pitch_pid ( ) . set_kP ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2015-03-24 17:43:58 -03:00
case TUNING_RATE_ROLL_PITCH_KI :
2024-07-30 08:37:51 -03:00
attitude_control - > get_rate_roll_pid ( ) . set_kI ( tuning_value ) ;
attitude_control - > get_rate_pitch_pid ( ) . set_kI ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2015-03-24 17:43:58 -03:00
case TUNING_RATE_ROLL_PITCH_KD :
2024-07-30 08:37:51 -03:00
attitude_control - > get_rate_roll_pid ( ) . set_kD ( tuning_value ) ;
attitude_control - > get_rate_pitch_pid ( ) . set_kD ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
// Yaw tuning
2015-03-24 17:43:58 -03:00
case TUNING_STABILIZE_YAW_KP :
2024-07-30 08:37:51 -03:00
attitude_control - > get_angle_yaw_p ( ) . set_kP ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2015-03-24 17:43:58 -03:00
case TUNING_YAW_RATE_KP :
2024-07-30 08:37:51 -03:00
attitude_control - > get_rate_yaw_pid ( ) . set_kP ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2015-03-24 17:43:58 -03:00
case TUNING_YAW_RATE_KD :
2024-07-30 08:37:51 -03:00
attitude_control - > get_rate_yaw_pid ( ) . set_kD ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
// Altitude and throttle tuning
2015-03-24 17:43:58 -03:00
case TUNING_ALTITUDE_HOLD_KP :
2024-07-30 08:37:51 -03:00
pos_control - > get_pos_z_p ( ) . set_kP ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2015-03-24 17:43:58 -03:00
case TUNING_THROTTLE_RATE_KP :
2024-07-30 08:37:51 -03:00
pos_control - > get_vel_z_pid ( ) . set_kP ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2015-03-24 17:43:58 -03:00
case TUNING_ACCEL_Z_KP :
2024-07-30 08:37:51 -03:00
pos_control - > get_accel_z_pid ( ) . set_kP ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2015-03-24 17:43:58 -03:00
case TUNING_ACCEL_Z_KI :
2024-07-30 08:37:51 -03:00
pos_control - > get_accel_z_pid ( ) . set_kI ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2015-03-24 17:43:58 -03:00
case TUNING_ACCEL_Z_KD :
2024-07-30 08:37:51 -03:00
pos_control - > get_accel_z_pid ( ) . set_kD ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
// Loiter and navigation tuning
2015-03-24 17:43:58 -03:00
case TUNING_LOITER_POSITION_KP :
2024-07-30 08:37:51 -03:00
pos_control - > get_pos_xy_p ( ) . set_kP ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2015-03-24 17:43:58 -03:00
case TUNING_VEL_XY_KP :
2024-07-30 08:37:51 -03:00
pos_control - > get_vel_xy_pid ( ) . set_kP ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2015-03-24 17:43:58 -03:00
case TUNING_VEL_XY_KI :
2024-07-30 08:37:51 -03:00
pos_control - > get_vel_xy_pid ( ) . set_kI ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2015-03-24 17:43:58 -03:00
case TUNING_WP_SPEED :
2019-04-03 09:32:26 -03:00
wp_nav - > set_speed_xy ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2021-10-11 03:19:53 -03:00
# if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
2021-09-17 02:54:19 -03:00
// Acro roll pitch rates
case TUNING_ACRO_RP_RATE :
2022-06-15 00:08:53 -03:00
g2 . command_model_acro_rp . set_rate ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2021-10-11 03:19:53 -03:00
# endif
2015-04-16 01:27:06 -03:00
2021-10-11 03:19:53 -03:00
# if MODE_ACRO_ENABLED == ENABLED || MODE_DRIFT_ENABLED == ENABLED
2021-09-17 02:54:19 -03:00
// Acro yaw rate
case TUNING_ACRO_YAW_RATE :
2022-06-15 00:08:53 -03:00
g2 . command_model_acro_y . set_rate ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2021-10-11 03:19:53 -03:00
# endif
2015-04-16 01:27:06 -03:00
# if FRAME_CONFIG == HELI_FRAME
2015-03-24 17:43:58 -03:00
case TUNING_HELI_EXTERNAL_GYRO :
2019-04-03 09:32:26 -03:00
motors - > ext_gyro_gain ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2015-03-24 17:43:58 -03:00
case TUNING_RATE_PITCH_FF :
2024-07-30 08:37:51 -03:00
attitude_control - > get_rate_pitch_pid ( ) . set_ff ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2015-03-24 17:43:58 -03:00
case TUNING_RATE_ROLL_FF :
2024-07-30 08:37:51 -03:00
attitude_control - > get_rate_roll_pid ( ) . set_ff ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2015-03-24 17:43:58 -03:00
case TUNING_RATE_YAW_FF :
2024-07-30 08:37:51 -03:00
attitude_control - > get_rate_yaw_pid ( ) . set_ff ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
# endif
2015-03-24 17:43:58 -03:00
case TUNING_DECLINATION :
2019-04-03 09:32:26 -03:00
compass . set_declination ( ToRad ( tuning_value ) , false ) ; // 2nd parameter is false because we do not want to save to eeprom because this would have a performance impact
2015-04-16 01:27:06 -03:00
break ;
2018-02-23 05:51:17 -04:00
# if MODE_CIRCLE_ENABLED == ENABLED
2015-03-24 17:43:58 -03:00
case TUNING_CIRCLE_RATE :
2019-04-03 09:32:26 -03:00
circle_nav - > set_rate ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2018-02-23 05:51:17 -04:00
# endif
2015-04-16 01:27:06 -03:00
2015-03-24 17:43:58 -03:00
case TUNING_RC_FEEL_RP :
2019-04-03 09:32:26 -03:00
attitude_control - > set_input_tc ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2015-03-24 17:43:58 -03:00
case TUNING_RATE_PITCH_KP :
2024-07-30 08:37:51 -03:00
attitude_control - > get_rate_pitch_pid ( ) . set_kP ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2015-03-24 17:43:58 -03:00
case TUNING_RATE_PITCH_KI :
2024-07-30 08:37:51 -03:00
attitude_control - > get_rate_pitch_pid ( ) . set_kI ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2015-03-24 17:43:58 -03:00
case TUNING_RATE_PITCH_KD :
2024-07-30 08:37:51 -03:00
attitude_control - > get_rate_pitch_pid ( ) . set_kD ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2015-03-24 17:43:58 -03:00
case TUNING_RATE_ROLL_KP :
2024-07-30 08:37:51 -03:00
attitude_control - > get_rate_roll_pid ( ) . set_kP ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2015-03-24 17:43:58 -03:00
case TUNING_RATE_ROLL_KI :
2024-07-30 08:37:51 -03:00
attitude_control - > get_rate_roll_pid ( ) . set_kI ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2015-03-24 17:43:58 -03:00
case TUNING_RATE_ROLL_KD :
2024-07-30 08:37:51 -03:00
attitude_control - > get_rate_roll_pid ( ) . set_kD ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2015-07-02 18:16:47 -03:00
# if FRAME_CONFIG != HELI_FRAME
2015-03-24 17:43:58 -03:00
case TUNING_RATE_MOT_YAW_HEADROOM :
2019-04-03 09:32:26 -03:00
motors - > set_yaw_headroom ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2015-07-02 18:16:47 -03:00
# endif
2015-04-16 01:27:06 -03:00
2022-03-07 11:48:20 -04:00
case TUNING_RATE_YAW_FILT :
2024-07-30 08:37:51 -03:00
attitude_control - > get_rate_yaw_pid ( ) . set_filt_E_hz ( tuning_value ) ;
2022-03-07 11:48:20 -04:00
break ;
2017-10-04 23:21:23 -03:00
2022-03-07 11:48:20 -04:00
case TUNING_SYSTEM_ID_MAGNITUDE :
2019-10-16 00:38:26 -03:00
# if MODE_SYSTEMID_ENABLED == ENABLED
2022-03-07 11:48:20 -04:00
copter . mode_systemid . set_magnitude ( tuning_value ) ;
2019-07-29 04:55:40 -03:00
# endif
2022-03-07 11:48:20 -04:00
break ;
2022-12-17 08:55:09 -04:00
case TUNING_POS_CONTROL_ANGLE_MAX :
pos_control - > set_lean_angle_max_cd ( tuning_value * 100.0 ) ;
break ;
2019-10-16 00:38:26 -03:00
}
2015-04-16 01:27:06 -03:00
}