mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 02:18:29 -04:00
Copter: disable autotune if pilot moves yaw stick
This commit is contained in:
parent
94f301181e
commit
aaf642c223
@ -1741,7 +1741,7 @@ void update_roll_pitch_mode(void)
|
|||||||
get_stabilize_pitch(control_pitch);
|
get_stabilize_pitch(control_pitch);
|
||||||
|
|
||||||
// copy user input for reporting purposes
|
// copy user input for reporting purposes
|
||||||
get_autotune_roll_pitch_controller(g.rc_1.control_in, g.rc_2.control_in);
|
get_autotune_roll_pitch_controller(g.rc_1.control_in, g.rc_2.control_in, g.rc_4.control_in);
|
||||||
break;
|
break;
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
@ -279,7 +279,7 @@ void auto_tune_update_gcs(uint8_t message_id)
|
|||||||
|
|
||||||
// Auto tuning roll-pitch controller
|
// Auto tuning roll-pitch controller
|
||||||
static void
|
static void
|
||||||
get_autotune_roll_pitch_controller(int32_t pilot_roll_angle, int32_t pilot_pitch_angle)
|
get_autotune_roll_pitch_controller(int16_t pilot_roll_angle, int16_t pilot_pitch_angle, int16_t pilot_yaw_command)
|
||||||
{
|
{
|
||||||
int32_t target_roll_rate, target_pitch_rate;
|
int32_t target_roll_rate, target_pitch_rate;
|
||||||
float rotation_rate; // rotation rate in radians/second
|
float rotation_rate; // rotation rate in radians/second
|
||||||
@ -291,7 +291,7 @@ get_autotune_roll_pitch_controller(int32_t pilot_roll_angle, int32_t pilot_pitch
|
|||||||
}
|
}
|
||||||
|
|
||||||
// check for pilot override
|
// check for pilot override
|
||||||
if (pilot_roll_angle != 0 || pilot_pitch_angle != 0 ) {
|
if (pilot_roll_angle != 0 || pilot_pitch_angle != 0 || pilot_yaw_command != 0) {
|
||||||
if (!auto_tune_state.pilot_override) {
|
if (!auto_tune_state.pilot_override) {
|
||||||
// restore pids to their original values
|
// restore pids to their original values
|
||||||
auto_tune_restore_orig_gains();
|
auto_tune_restore_orig_gains();
|
||||||
|
Loading…
Reference in New Issue
Block a user