2015-04-16 01:27:06 -03:00
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
2015-05-29 23:12:49 -03:00
# include "Copter.h"
2015-04-16 01:27:06 -03:00
/*
* tuning . pde - 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 . pde
*/
// tuning - updates parameters based on the ch6 tuning knob's position
// should be called at 3.3hz
2015-05-29 23:12:49 -03:00
void Copter : : tuning ( ) {
2015-04-16 01:27:06 -03:00
2015-12-28 20:06:11 -04:00
// exit immediately if not using tuning function, or when radio failsafe is invoked, so tuning values are not set to zero
if ( ( g . radio_tuning < = 0 ) | | failsafe . radio | | failsafe . radio_counter ! = 0 | | g . rc_6 . radio_in = = 0 ) {
2015-04-16 01:27:06 -03:00
return ;
}
2015-12-29 18:39:57 -04:00
// set tuning range and then get new value
2015-12-06 18:11:01 -04:00
g . rc_6 . set_range_in ( g . radio_tuning_low , g . radio_tuning_high ) ;
2015-12-29 18:39:57 -04:00
float tuning_value = ( float ) g . rc_6 . control_in / 1000.0f ;
// Tuning Value should never be outside the bounds of the specified low and high value
tuning_value = constrain_float ( tuning_value , g . radio_tuning_low / 1000.0f , g . radio_tuning_high / 1000.0f ) ;
2015-04-16 01:27:06 -03:00
2015-04-26 21:29:04 -03:00
Log_Write_Parameter_Tuning ( g . radio_tuning , tuning_value , g . rc_6 . control_in , g . radio_tuning_low , g . radio_tuning_high ) ;
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 :
2015-04-16 01:27:06 -03:00
g . p_stabilize_roll . kP ( tuning_value ) ;
g . p_stabilize_pitch . kP ( tuning_value ) ;
break ;
2015-03-24 17:43:58 -03:00
case TUNING_RATE_ROLL_PITCH_KP :
2015-04-16 01:27:06 -03:00
g . pid_rate_roll . kP ( tuning_value ) ;
g . pid_rate_pitch . kP ( tuning_value ) ;
break ;
2015-03-24 17:43:58 -03:00
case TUNING_RATE_ROLL_PITCH_KI :
2015-04-16 01:27:06 -03:00
g . pid_rate_roll . kI ( tuning_value ) ;
g . pid_rate_pitch . kI ( tuning_value ) ;
break ;
2015-03-24 17:43:58 -03:00
case TUNING_RATE_ROLL_PITCH_KD :
2015-04-16 01:27:06 -03:00
g . pid_rate_roll . kD ( tuning_value ) ;
g . pid_rate_pitch . kD ( tuning_value ) ;
break ;
// Yaw tuning
2015-03-24 17:43:58 -03:00
case TUNING_STABILIZE_YAW_KP :
2015-04-16 01:27:06 -03:00
g . p_stabilize_yaw . kP ( tuning_value ) ;
break ;
2015-03-24 17:43:58 -03:00
case TUNING_YAW_RATE_KP :
2015-04-16 01:27:06 -03:00
g . pid_rate_yaw . kP ( tuning_value ) ;
break ;
2015-03-24 17:43:58 -03:00
case TUNING_YAW_RATE_KD :
2015-04-16 01:27:06 -03:00
g . pid_rate_yaw . kD ( tuning_value ) ;
break ;
// Altitude and throttle tuning
2015-03-24 17:43:58 -03:00
case TUNING_ALTITUDE_HOLD_KP :
2015-04-16 01:27:06 -03:00
g . p_alt_hold . kP ( tuning_value ) ;
break ;
2015-03-24 17:43:58 -03:00
case TUNING_THROTTLE_RATE_KP :
2015-04-16 01:27:06 -03:00
g . p_vel_z . kP ( tuning_value ) ;
break ;
2015-03-24 17:43:58 -03:00
case TUNING_ACCEL_Z_KP :
2015-04-16 01:27:06 -03:00
g . pid_accel_z . kP ( tuning_value ) ;
break ;
2015-03-24 17:43:58 -03:00
case TUNING_ACCEL_Z_KI :
2015-04-16 01:27:06 -03:00
g . pid_accel_z . kI ( tuning_value ) ;
break ;
2015-03-24 17:43:58 -03:00
case TUNING_ACCEL_Z_KD :
2015-04-16 01:27:06 -03:00
g . pid_accel_z . kD ( tuning_value ) ;
break ;
// Loiter and navigation tuning
2015-03-24 17:43:58 -03:00
case TUNING_LOITER_POSITION_KP :
2015-04-16 01:27:06 -03:00
g . p_pos_xy . kP ( tuning_value ) ;
break ;
2015-03-24 17:43:58 -03:00
case TUNING_VEL_XY_KP :
2015-04-16 01:27:06 -03:00
g . pi_vel_xy . kP ( tuning_value ) ;
break ;
2015-03-24 17:43:58 -03:00
case TUNING_VEL_XY_KI :
2015-04-16 01:27:06 -03:00
g . pi_vel_xy . kI ( tuning_value ) ;
break ;
2015-03-24 17:43:58 -03:00
case TUNING_WP_SPEED :
2015-04-16 01:27:06 -03:00
// set waypoint navigation horizontal speed to 0 ~ 1000 cm/s
wp_nav . set_speed_xy ( g . rc_6 . control_in ) ;
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 :
2015-04-16 01:27:06 -03:00
motors . ext_gyro_gain ( g . rc_6 . control_in ) ;
break ;
2015-03-24 17:43:58 -03:00
case TUNING_RATE_PITCH_FF :
2015-04-16 01:27:06 -03:00
g . pid_rate_pitch . ff ( tuning_value ) ;
break ;
2015-03-24 17:43:58 -03:00
case TUNING_RATE_ROLL_FF :
2015-04-16 01:27:06 -03:00
g . pid_rate_roll . ff ( tuning_value ) ;
break ;
2015-03-24 17:43:58 -03:00
case TUNING_RATE_YAW_FF :
2015-04-16 01:27:06 -03:00
g . pid_rate_yaw . ff ( tuning_value ) ;
break ;
# endif
2015-03-24 17:43:58 -03:00
case TUNING_DECLINATION :
2015-04-16 01:27:06 -03:00
// set declination to +-20degrees
compass . set_declination ( ToRad ( ( 2.0f * g . rc_6 . control_in - g . radio_tuning_high ) / 100.0f ) , false ) ; // 2nd parameter is false because we do not want to save to eeprom because this would have a performance impact
break ;
2015-03-24 17:43:58 -03:00
case TUNING_CIRCLE_RATE :
2015-07-22 09:42:02 -03:00
// set circle rate up to approximately 45 deg/sec in either direction
circle_nav . set_rate ( ( float ) g . rc_6 . control_in / 25.0f - 20.0f ) ;
2015-04-16 01:27:06 -03:00
break ;
2015-03-24 17:43:58 -03:00
case TUNING_SONAR_GAIN :
2015-04-16 01:27:06 -03:00
// set sonar gain
g . sonar_gain . set ( tuning_value ) ;
break ;
#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 :
2015-04-16 01:27:06 -03:00
// roll-pitch input smoothing
g . rc_feel_rp = g . rc_6 . control_in / 10 ;
break ;
2015-03-24 17:43:58 -03:00
case TUNING_RATE_PITCH_KP :
2015-04-16 01:27:06 -03:00
g . pid_rate_pitch . kP ( tuning_value ) ;
break ;
2015-03-24 17:43:58 -03:00
case TUNING_RATE_PITCH_KI :
2015-04-16 01:27:06 -03:00
g . pid_rate_pitch . kI ( tuning_value ) ;
break ;
2015-03-24 17:43:58 -03:00
case TUNING_RATE_PITCH_KD :
2015-04-16 01:27:06 -03:00
g . pid_rate_pitch . kD ( tuning_value ) ;
break ;
2015-03-24 17:43:58 -03:00
case TUNING_RATE_ROLL_KP :
2015-04-16 01:27:06 -03:00
g . pid_rate_roll . kP ( tuning_value ) ;
break ;
2015-03-24 17:43:58 -03:00
case TUNING_RATE_ROLL_KI :
2015-04-16 01:27:06 -03:00
g . pid_rate_roll . kI ( tuning_value ) ;
break ;
2015-03-24 17:43:58 -03:00
case TUNING_RATE_ROLL_KD :
2015-04-16 01:27:06 -03:00
g . pid_rate_roll . kD ( tuning_value ) ;
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 :
2015-04-16 01:27:06 -03:00
motors . set_yaw_headroom ( tuning_value * 1000 ) ;
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 :
2015-04-16 01:27:06 -03:00
g . pid_rate_yaw . filt_hz ( tuning_value ) ;
break ;
}
}