ardupilot/ArduCopter/tuning.cpp

194 lines
5.6 KiB
C++
Raw Normal View History

#include "Copter.h"
2015-04-16 01:27:06 -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
void Copter::tuning()
{
const RC_Channel *rc6 = rc().channel(CH_6);
2015-04-16 01:27:06 -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;
}
// 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
// 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-16 01:27:06 -03:00
switch(g.radio_tuning) {
// Roll, Pitch tuning
case TUNING_STABILIZE_ROLL_PITCH_KP:
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;
case TUNING_RATE_ROLL_PITCH_KP:
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;
case TUNING_RATE_ROLL_PITCH_KI:
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;
case TUNING_RATE_ROLL_PITCH_KD:
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
case TUNING_STABILIZE_YAW_KP:
attitude_control->get_angle_yaw_p().kP(tuning_value);
2015-04-16 01:27:06 -03:00
break;
case TUNING_YAW_RATE_KP:
attitude_control->get_rate_yaw_pid().kP(tuning_value);
2015-04-16 01:27:06 -03:00
break;
case TUNING_YAW_RATE_KD:
attitude_control->get_rate_yaw_pid().kD(tuning_value);
2015-04-16 01:27:06 -03:00
break;
// Altitude and throttle tuning
case TUNING_ALTITUDE_HOLD_KP:
pos_control->get_pos_z_p().kP(tuning_value);
2015-04-16 01:27:06 -03:00
break;
case TUNING_THROTTLE_RATE_KP:
pos_control->get_vel_z_pid().kP(tuning_value);
2015-04-16 01:27:06 -03:00
break;
case TUNING_ACCEL_Z_KP:
pos_control->get_accel_z_pid().kP(tuning_value);
2015-04-16 01:27:06 -03:00
break;
case TUNING_ACCEL_Z_KI:
pos_control->get_accel_z_pid().kI(tuning_value);
2015-04-16 01:27:06 -03:00
break;
case TUNING_ACCEL_Z_KD:
pos_control->get_accel_z_pid().kD(tuning_value);
2015-04-16 01:27:06 -03:00
break;
// Loiter and navigation tuning
case TUNING_LOITER_POSITION_KP:
pos_control->get_pos_xy_p().kP(tuning_value);
2015-04-16 01:27:06 -03:00
break;
case TUNING_VEL_XY_KP:
pos_control->get_vel_xy_pid().kP(tuning_value);
2015-04-16 01:27:06 -03:00
break;
case TUNING_VEL_XY_KI:
pos_control->get_vel_xy_pid().kI(tuning_value);
2015-04-16 01:27:06 -03:00
break;
case TUNING_WP_SPEED:
wp_nav->set_speed_xy(tuning_value);
2015-04-16 01:27:06 -03:00
break;
#if MODE_ACRO_ENABLED == ENABLED || MODE_SPORT_ENABLED == ENABLED
// Acro roll pitch rates
case TUNING_ACRO_RP_RATE:
g2.acro_rp_rate = tuning_value;
2015-04-16 01:27:06 -03:00
break;
#endif
2015-04-16 01:27:06 -03:00
#if MODE_ACRO_ENABLED == ENABLED || MODE_DRIFT_ENABLED == ENABLED
// Acro yaw rate
case TUNING_ACRO_YAW_RATE:
g2.acro_y_rate = tuning_value;
2015-04-16 01:27:06 -03:00
break;
#endif
2015-04-16 01:27:06 -03:00
#if FRAME_CONFIG == HELI_FRAME
case TUNING_HELI_EXTERNAL_GYRO:
motors->ext_gyro_gain(tuning_value);
2015-04-16 01:27:06 -03:00
break;
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;
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;
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
case TUNING_DECLINATION:
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;
#if MODE_CIRCLE_ENABLED == ENABLED
case TUNING_CIRCLE_RATE:
circle_nav->set_rate(tuning_value);
2015-04-16 01:27:06 -03:00
break;
#endif
2015-04-16 01:27:06 -03:00
case TUNING_RC_FEEL_RP:
attitude_control->set_input_tc(tuning_value);
2015-04-16 01:27:06 -03:00
break;
case TUNING_RATE_PITCH_KP:
attitude_control->get_rate_pitch_pid().kP(tuning_value);
2015-04-16 01:27:06 -03:00
break;
case TUNING_RATE_PITCH_KI:
attitude_control->get_rate_pitch_pid().kI(tuning_value);
2015-04-16 01:27:06 -03:00
break;
case TUNING_RATE_PITCH_KD:
attitude_control->get_rate_pitch_pid().kD(tuning_value);
2015-04-16 01:27:06 -03:00
break;
case TUNING_RATE_ROLL_KP:
attitude_control->get_rate_roll_pid().kP(tuning_value);
2015-04-16 01:27:06 -03:00
break;
case TUNING_RATE_ROLL_KI:
attitude_control->get_rate_roll_pid().kI(tuning_value);
2015-04-16 01:27:06 -03:00
break;
case TUNING_RATE_ROLL_KD:
attitude_control->get_rate_roll_pid().kD(tuning_value);
2015-04-16 01:27:06 -03:00
break;
#if FRAME_CONFIG != HELI_FRAME
case TUNING_RATE_MOT_YAW_HEADROOM:
motors->set_yaw_headroom(tuning_value);
2015-04-16 01:27:06 -03:00
break;
#endif
2015-04-16 01:27:06 -03:00
case TUNING_RATE_YAW_FILT:
attitude_control->get_rate_yaw_pid().filt_E_hz(tuning_value);
2015-04-16 01:27:06 -03:00
break;
case TUNING_SYSTEM_ID_MAGNITUDE:
#if MODE_SYSTEMID_ENABLED == ENABLED
2019-07-29 04:55:40 -03:00
copter.mode_systemid.set_magnitude(tuning_value);
#endif
break;
}
2015-04-16 01:27:06 -03:00
}