|
|
|
@ -61,7 +61,7 @@ void Copter::AutoTune::run()
@@ -61,7 +61,7 @@ void Copter::AutoTune::run()
|
|
|
|
|
copter.attitude_control->reset_rate_controller_I_terms(); |
|
|
|
|
copter.attitude_control->set_yaw_target_to_current_heading(); |
|
|
|
|
|
|
|
|
|
int32_t target_roll, target_pitch, target_yaw_rate; |
|
|
|
|
float target_roll, target_pitch, target_yaw_rate; |
|
|
|
|
get_pilot_desired_rp_yrate_cd(target_roll, target_pitch, target_yaw_rate); |
|
|
|
|
|
|
|
|
|
copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); |
|
|
|
@ -90,17 +90,11 @@ float Copter::AutoTune::get_pilot_desired_climb_rate_cms(void) const
@@ -90,17 +90,11 @@ float Copter::AutoTune::get_pilot_desired_climb_rate_cms(void) const
|
|
|
|
|
/*
|
|
|
|
|
get stick roll, pitch and yaw rate |
|
|
|
|
*/ |
|
|
|
|
void Copter::AutoTune::get_pilot_desired_rp_yrate_cd(int32_t &des_roll_cd, int32_t &des_pitch_cd, int32_t &yaw_rate_cds) |
|
|
|
|
void Copter::AutoTune::get_pilot_desired_rp_yrate_cd(float &des_roll_cd, float &des_pitch_cd, float &yaw_rate_cds) |
|
|
|
|
{ |
|
|
|
|
// map from int32_t to float
|
|
|
|
|
float des_roll, des_pitch; |
|
|
|
|
|
|
|
|
|
copter.mode_autotune.get_pilot_desired_lean_angles(des_roll, des_pitch, copter.aparm.angle_max, |
|
|
|
|
copter.mode_autotune.get_pilot_desired_lean_angles(des_roll_cd, des_pitch_cd, copter.aparm.angle_max, |
|
|
|
|
copter.attitude_control->get_althold_lean_angle_max()); |
|
|
|
|
yaw_rate_cds = copter.mode_autotune.get_pilot_desired_yaw_rate(copter.channel_yaw->get_control_in()); |
|
|
|
|
|
|
|
|
|
des_roll_cd = des_roll; |
|
|
|
|
des_pitch_cd = des_pitch; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
|