2015-05-29 23:12:49 -03:00
# include "Copter.h"
2015-04-16 01:27:06 -03:00
/*
2016-07-25 15:45:29 -03:00
* Function to update various parameters in flight using the ch6 tuning knob
* This should not be confused with the AutoTune feature which can bve found in control_autotune . cpp
2015-04-16 01:27:06 -03:00
*/
// tuning - updates parameters based on the ch6 tuning knob's position
// should be called at 3.3hz
2019-04-03 09:32:26 -03:00
void Copter : : tuning ( )
{
const RC_Channel * rc6 = rc ( ) . channel ( CH_6 ) ;
2015-04-16 01:27:06 -03:00
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
if ( failsafe . radio | | failsafe . radio_counter ! = 0 | | rc6 - > get_radio_in ( ) = = 0 ) {
return ;
}
2015-04-16 01:27:06 -03:00
2019-04-03 09:32:26 -03:00
// exit immediately if a function is assigned to channel 6
if ( ( RC_Channel : : aux_func_t ) rc6 - > option . get ( ) ! = RC_Channel : : AUX_FUNC : : DO_NOTHING ) {
return ;
}
const uint16_t radio_in = rc6 - > get_radio_in ( ) ;
float tuning_value = linear_interpolate ( g2 . tuning_min , g2 . tuning_max , radio_in , rc6 - > get_radio_min ( ) , rc6 - > get_radio_max ( ) ) ;
Log_Write_Parameter_Tuning ( g . radio_tuning , tuning_value , g2 . tuning_min , g2 . tuning_max ) ;
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 :
2017-01-09 03:31:26 -04:00
attitude_control - > get_angle_roll_p ( ) . kP ( tuning_value ) ;
attitude_control - > get_angle_pitch_p ( ) . 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 :
2017-01-09 03:31:26 -04:00
attitude_control - > get_rate_roll_pid ( ) . kP ( tuning_value ) ;
attitude_control - > get_rate_pitch_pid ( ) . 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 :
2017-01-09 03:31:26 -04:00
attitude_control - > get_rate_roll_pid ( ) . kI ( tuning_value ) ;
attitude_control - > get_rate_pitch_pid ( ) . 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 :
2017-01-09 03:31:26 -04:00
attitude_control - > get_rate_roll_pid ( ) . kD ( tuning_value ) ;
attitude_control - > get_rate_pitch_pid ( ) . 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 :
2017-01-09 03:31:26 -04:00
attitude_control - > get_angle_yaw_p ( ) . 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 :
2017-01-09 03:31:26 -04:00
attitude_control - > get_rate_yaw_pid ( ) . 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 :
2017-01-09 03:31:26 -04:00
attitude_control - > get_rate_yaw_pid ( ) . 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 :
2017-11-20 21:49:11 -04:00
pos_control - > get_pos_z_p ( ) . 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 :
2017-11-20 21:49:11 -04:00
pos_control - > get_vel_z_p ( ) . 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 :
2017-11-20 21:49:11 -04:00
pos_control - > get_accel_z_pid ( ) . 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 :
2017-11-20 21:49:11 -04:00
pos_control - > get_accel_z_pid ( ) . 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 :
2017-11-20 21:49:11 -04:00
pos_control - > get_accel_z_pid ( ) . 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 :
2017-11-20 21:49:11 -04:00
pos_control - > get_pos_xy_p ( ) . 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 :
2017-11-20 07:26:32 -04:00
pos_control - > get_vel_xy_pid ( ) . 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 :
2017-11-20 07:26:32 -04:00
pos_control - > get_vel_xy_pid ( ) . 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 ;
// Acro roll pitch gain
2015-03-24 17:43:58 -03:00
case TUNING_ACRO_RP_KP :
2015-04-16 01:27:06 -03:00
g . acro_rp_p = tuning_value ;
break ;
// Acro yaw gain
2015-03-24 17:43:58 -03:00
case TUNING_ACRO_YAW_KP :
2015-04-16 01:27:06 -03:00
g . acro_yaw_p = tuning_value ;
break ;
# 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 :
2017-01-09 03:50:09 -04:00
attitude_control - > get_rate_pitch_pid ( ) . 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 :
2017-01-09 03:50:09 -04:00
attitude_control - > get_rate_roll_pid ( ) . 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 :
2017-01-09 03:50:09 -04:00
attitude_control - > get_rate_yaw_pid ( ) . 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
2018-03-15 09:39:26 -03:00
# if RANGEFINDER_ENABLED == ENABLED
2016-04-27 08:37:04 -03:00
case TUNING_RANGEFINDER_GAIN :
// set rangefinder gain
g . rangefinder_gain . set ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2018-03-15 09:39:26 -03:00
# endif
2015-04-16 01:27:06 -03:00
#if 0
// disabled for now - we need accessor functions
2015-03-24 17:43:58 -03:00
case TUNING_EKF_VERTICAL_POS :
2015-10-17 03:25:06 -03:00
// Tune the EKF that is being used
2015-04-16 01:27:06 -03:00
// EKF's baro vs accel (higher rely on accels more, baro impact is reduced)
2015-10-17 03:25:06 -03:00
if ( ! ahrs . get_NavEKF2 ( ) . enabled ( ) ) {
ahrs . get_NavEKF ( ) . _gpsVertPosNoise = tuning_value ;
} else {
ahrs . get_NavEKF2 ( ) . _gpsVertPosNoise = tuning_value ;
}
2015-04-16 01:27:06 -03:00
break ;
2015-03-24 17:43:58 -03:00
case TUNING_EKF_HORIZONTAL_POS :
2015-04-16 01:27:06 -03:00
// EKF's gps vs accel (higher rely on accels more, gps impact is reduced)
2015-10-17 03:25:06 -03:00
if ( ! ahrs . get_NavEKF2 ( ) . enabled ( ) ) {
ahrs . get_NavEKF ( ) . _gpsHorizPosNoise = tuning_value ;
} else {
ahrs . get_NavEKF2 ( ) . _gpsHorizPosNoise = tuning_value ;
}
2015-04-16 01:27:06 -03:00
break ;
2015-03-24 17:43:58 -03:00
case TUNING_EKF_ACCEL_NOISE :
2015-04-16 01:27:06 -03:00
// EKF's accel noise (lower means trust accels more, gps & baro less)
2015-10-17 03:25:06 -03:00
if ( ! ahrs . get_NavEKF2 ( ) . enabled ( ) ) {
ahrs . get_NavEKF ( ) . _accNoise = tuning_value ;
} else {
ahrs . get_NavEKF2 ( ) . _accNoise = tuning_value ;
}
2015-04-16 01:27:06 -03:00
break ;
# endif
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 :
2017-01-09 03:31:26 -04:00
attitude_control - > get_rate_pitch_pid ( ) . 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 :
2017-01-09 03:31:26 -04:00
attitude_control - > get_rate_pitch_pid ( ) . 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 :
2017-01-09 03:31:26 -04:00
attitude_control - > get_rate_pitch_pid ( ) . 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 :
2017-01-09 03:31:26 -04:00
attitude_control - > get_rate_roll_pid ( ) . 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 :
2017-01-09 03:31:26 -04:00
attitude_control - > get_rate_roll_pid ( ) . 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 :
2017-01-09 03:31:26 -04:00
attitude_control - > get_rate_roll_pid ( ) . 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
2015-03-24 17:43:58 -03:00
case TUNING_RATE_YAW_FILT :
2019-06-27 06:36:47 -03:00
attitude_control - > get_rate_yaw_pid ( ) . filt_E_hz ( tuning_value ) ;
2015-04-16 01:27:06 -03:00
break ;
2017-10-04 23:21:23 -03:00
2018-02-10 10:23:06 -04:00
# if WINCH_ENABLED == ENABLED
2019-04-03 09:32:26 -03:00
case TUNING_WINCH :
// add small deadzone
if ( fabsf ( tuning_value ) < 0.05f ) {
tuning_value = 0 ;
2017-10-04 23:21:23 -03:00
}
2019-04-03 09:32:26 -03:00
g2 . winch . set_desired_rate ( tuning_value ) ;
2017-10-04 23:21:23 -03:00
break ;
2018-02-10 10:23:06 -04:00
# endif
2019-07-29 04:55:40 -03:00
# if MODE_SYSTEMID_ENABLED == ENABLED
2019-10-15 02:38:24 -03:00
case TUNING_SYSTEM_ID_MAGNITUDE :
2019-07-29 04:55:40 -03:00
copter . mode_systemid . set_magnitude ( tuning_value ) ;
break ;
2017-10-04 23:21:23 -03:00
}
2019-07-29 04:55:40 -03:00
# endif
2015-04-16 01:27:06 -03:00
}