2016-01-14 15:30:56 -04:00
# include "Sub.h"
2016-11-25 23:55:35 -04:00
# if CH6_TUNE_ENABLED == ENABLED
2015-12-30 18:57:56 -04:00
/*
* tuning . pde - function to update various parameters in flight using the ch6 tuning knob
*/
// tuning - updates parameters based on the ch6 tuning knob's position
// should be called at 3.3hz
2017-02-03 17:33:27 -04:00
void Sub : : tuning ( )
{
2015-12-30 18:57:56 -04:00
// exit immediately if not tuning of when radio failsafe is invoked so tuning values are not set to zero
2017-01-30 13:49:39 -04:00
if ( ( g . radio_tuning < = 0 ) | | failsafe . manual_control ) {
2015-12-30 18:57:56 -04:00
return ;
}
2017-02-03 00:18:27 -04:00
float tuning_value = ( float ) g . rc_6 . get_control_in ( ) / 1000.0f ;
2015-12-30 18:57:56 -04:00
g . rc_6 . set_range ( g . radio_tuning_low , g . radio_tuning_high ) ;
2017-02-03 00:18:27 -04:00
Log_Write_Parameter_Tuning ( g . radio_tuning , tuning_value , g . rc_6 . get_control_in ( ) , g . radio_tuning_low , g . radio_tuning_high ) ;
2015-12-30 18:57:56 -04:00
2017-02-03 17:33:27 -04:00
switch ( g . radio_tuning ) {
2015-12-30 18:57:56 -04:00
2017-02-03 17:33:27 -04:00
// Roll, Pitch tuning
2015-12-30 18:57:56 -04:00
case TUNING_STABILIZE_ROLL_PITCH_KP :
2016-04-05 00:17:39 -03:00
attitude_control . get_angle_roll_p ( ) . kP ( tuning_value ) ;
attitude_control . get_angle_pitch_p ( ) . kP ( tuning_value ) ;
2015-12-30 18:57:56 -04:00
break ;
case TUNING_RATE_ROLL_PITCH_KP :
2016-04-05 00:17:39 -03:00
attitude_control . get_rate_roll_pid ( ) . kP ( tuning_value ) ;
attitude_control . get_rate_pitch_pid ( ) . kP ( tuning_value ) ;
2015-12-30 18:57:56 -04:00
break ;
case TUNING_RATE_ROLL_PITCH_KI :
2016-04-05 00:17:39 -03:00
attitude_control . get_rate_roll_pid ( ) . kI ( tuning_value ) ;
attitude_control . get_rate_pitch_pid ( ) . kI ( tuning_value ) ;
2015-12-30 18:57:56 -04:00
break ;
case TUNING_RATE_ROLL_PITCH_KD :
2016-04-05 00:17:39 -03:00
attitude_control . get_rate_roll_pid ( ) . kD ( tuning_value ) ;
attitude_control . get_rate_pitch_pid ( ) . kD ( tuning_value ) ;
2015-12-30 18:57:56 -04:00
break ;
2017-02-03 17:33:27 -04:00
// Yaw tuning
2015-12-30 18:57:56 -04:00
case TUNING_STABILIZE_YAW_KP :
2016-04-05 00:17:39 -03:00
attitude_control . get_angle_yaw_p ( ) . kP ( tuning_value ) ;
2015-12-30 18:57:56 -04:00
break ;
case TUNING_YAW_RATE_KP :
2016-04-05 00:17:39 -03:00
attitude_control . get_rate_yaw_pid ( ) . kP ( tuning_value ) ;
2015-12-30 18:57:56 -04:00
break ;
case TUNING_YAW_RATE_KD :
2016-04-05 00:17:39 -03:00
attitude_control . get_rate_yaw_pid ( ) . kD ( tuning_value ) ;
2015-12-30 18:57:56 -04:00
break ;
2017-02-03 17:33:27 -04:00
// Altitude and throttle tuning
2015-12-30 18:57:56 -04:00
case TUNING_ALTITUDE_HOLD_KP :
g . p_alt_hold . kP ( tuning_value ) ;
break ;
case TUNING_THROTTLE_RATE_KP :
g . p_vel_z . kP ( tuning_value ) ;
break ;
case TUNING_ACCEL_Z_KP :
g . pid_accel_z . kP ( tuning_value ) ;
break ;
case TUNING_ACCEL_Z_KI :
g . pid_accel_z . kI ( tuning_value ) ;
break ;
case TUNING_ACCEL_Z_KD :
g . pid_accel_z . kD ( tuning_value ) ;
break ;
2017-02-03 17:33:27 -04:00
// Loiter and navigation tuning
2015-12-30 18:57:56 -04:00
case TUNING_LOITER_POSITION_KP :
g . p_pos_xy . kP ( tuning_value ) ;
break ;
case TUNING_VEL_XY_KP :
g . pi_vel_xy . kP ( tuning_value ) ;
break ;
case TUNING_VEL_XY_KI :
g . pi_vel_xy . kI ( tuning_value ) ;
break ;
case TUNING_WP_SPEED :
// set waypoint navigation horizontal speed to 0 ~ 1000 cm/s
2017-02-03 00:18:27 -04:00
wp_nav . set_speed_xy ( g . rc_6 . get_control_in ( ) ) ;
2015-12-30 18:57:56 -04:00
break ;
2017-02-03 17:33:27 -04:00
// Acro roll pitch gain
2015-12-30 18:57:56 -04:00
case TUNING_ACRO_RP_KP :
g . acro_rp_p = tuning_value ;
break ;
2017-02-03 17:33:27 -04:00
// Acro yaw gain
2015-12-30 18:57:56 -04:00
case TUNING_ACRO_YAW_KP :
g . acro_yaw_p = tuning_value ;
break ;
case TUNING_DECLINATION :
// set declination to +-20degrees
2017-02-03 00:18:27 -04:00
compass . set_declination ( ToRad ( ( 2.0f * g . rc_6 . get_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
2015-12-30 18:57:56 -04:00
break ;
case TUNING_CIRCLE_RATE :
// set circle rate up to approximately 45 deg/sec in either direction
2017-02-03 00:18:27 -04:00
circle_nav . set_rate ( ( float ) g . rc_6 . get_control_in ( ) / 25.0f - 20.0f ) ;
2015-12-30 18:57:56 -04:00
break ;
2016-05-22 21:50:44 -03:00
case TUNING_RANGEFINDER_GAIN :
// set rangefinder gain
g . rangefinder_gain . set ( tuning_value ) ;
2015-12-30 18:57:56 -04:00
break ;
#if 0
// disabled for now - we need accessor functions
case TUNING_EKF_VERTICAL_POS :
// Tune the EKF that is being used
// EKF's baro vs accel (higher rely on accels more, baro impact is reduced)
if ( ! ahrs . get_NavEKF2 ( ) . enabled ( ) ) {
ahrs . get_NavEKF ( ) . _gpsVertPosNoise = tuning_value ;
} else {
ahrs . get_NavEKF2 ( ) . _gpsVertPosNoise = tuning_value ;
}
break ;
case TUNING_EKF_HORIZONTAL_POS :
// EKF's gps vs accel (higher rely on accels more, gps impact is reduced)
if ( ! ahrs . get_NavEKF2 ( ) . enabled ( ) ) {
ahrs . get_NavEKF ( ) . _gpsHorizPosNoise = tuning_value ;
} else {
ahrs . get_NavEKF2 ( ) . _gpsHorizPosNoise = tuning_value ;
}
break ;
case TUNING_EKF_ACCEL_NOISE :
// EKF's accel noise (lower means trust accels more, gps & baro less)
if ( ! ahrs . get_NavEKF2 ( ) . enabled ( ) ) {
ahrs . get_NavEKF ( ) . _accNoise = tuning_value ;
} else {
ahrs . get_NavEKF2 ( ) . _accNoise = tuning_value ;
}
break ;
# endif
case TUNING_RC_FEEL_RP :
// roll-pitch input smoothing
2017-02-03 00:18:27 -04:00
g . rc_feel_rp = g . rc_6 . get_control_in ( ) / 10 ;
2015-12-30 18:57:56 -04:00
break ;
case TUNING_RATE_PITCH_KP :
2016-04-05 00:17:39 -03:00
attitude_control . get_rate_pitch_pid ( ) . kP ( tuning_value ) ;
2015-12-30 18:57:56 -04:00
break ;
case TUNING_RATE_PITCH_KI :
2016-04-05 00:17:39 -03:00
attitude_control . get_rate_pitch_pid ( ) . kI ( tuning_value ) ;
2015-12-30 18:57:56 -04:00
break ;
case TUNING_RATE_PITCH_KD :
2016-04-05 00:17:39 -03:00
attitude_control . get_rate_pitch_pid ( ) . kD ( tuning_value ) ;
2015-12-30 18:57:56 -04:00
break ;
case TUNING_RATE_ROLL_KP :
2016-04-05 00:17:39 -03:00
attitude_control . get_rate_roll_pid ( ) . kP ( tuning_value ) ;
2015-12-30 18:57:56 -04:00
break ;
case TUNING_RATE_ROLL_KI :
2016-04-05 00:17:39 -03:00
attitude_control . get_rate_roll_pid ( ) . kI ( tuning_value ) ;
2015-12-30 18:57:56 -04:00
break ;
case TUNING_RATE_ROLL_KD :
2016-04-05 00:17:39 -03:00
attitude_control . get_rate_roll_pid ( ) . kD ( tuning_value ) ;
2015-12-30 18:57:56 -04:00
break ;
case TUNING_RATE_MOT_YAW_HEADROOM :
motors . set_yaw_headroom ( tuning_value * 1000 ) ;
break ;
2017-02-03 17:33:27 -04:00
case TUNING_RATE_YAW_FILT :
attitude_control . get_rate_yaw_pid ( ) . filt_hz ( tuning_value ) ;
break ;
2015-12-30 18:57:56 -04:00
}
}
2016-11-25 23:55:35 -04:00
# endif