zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AC_AutoTune: set FLTT to zero while twitching 

add pilot testing message
c415-sdk
Andy Piper 5 years ago committed by Peter Barker
parent
c707303336
commit
fd96cdf6e0
2 changed files with 23 additions and 3 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 20
      libraries/AC_AutoTune/AC_AutoTune.cpp
  2. 6
      libraries/AC_AutoTune/AC_AutoTune.h

20
libraries/AC_AutoTune/AC_AutoTune.cpp
Unescape View File

@ -90,6 +90,7 @@ @@ -90,6 +90,7 @@
#define AUTOTUNE_MESSAGE_SUCCESS 2
#define AUTOTUNE_MESSAGE_FAILED 3
#define AUTOTUNE_MESSAGE_SAVED_GAINS 4
#define AUTOTUNE_MESSAGE_TESTING 5
#define AUTOTUNE_ANNOUNCE_INTERVAL_MS 2000
@ -177,6 +178,7 @@ bool AC_AutoTune::init_internals(bool _use_poshold, @@ -177,6 +178,7 @@ bool AC_AutoTune::init_internals(bool _use_poshold,
// we have completed a tune and the pilot wishes to test the new gains in the current flight mode
// so simply apply tuning gains (i.e. do not change flight mode)
load_gains(GAIN_TUNED);
update_gcs(AUTOTUNE_MESSAGE_TESTING);
AP::logger().Write_Event(LogEvent::AUTOTUNE_PILOT_TESTING);
break;
}
@ -925,6 +927,7 @@ void AC_AutoTune::backup_gains_and_initialise() @@ -925,6 +927,7 @@ void AC_AutoTune::backup_gains_and_initialise()
orig_roll_ri = attitude_control->get_rate_roll_pid().kI();
orig_roll_rd = attitude_control->get_rate_roll_pid().kD();
orig_roll_rff = attitude_control->get_rate_roll_pid().ff();
orig_roll_fltt = attitude_control->get_rate_roll_pid().filt_T_hz();
orig_roll_sp = attitude_control->get_angle_roll_p().kP();
orig_roll_accel = attitude_control->get_accel_roll_max();
tune_roll_rp = attitude_control->get_rate_roll_pid().kP();
@ -936,6 +939,7 @@ void AC_AutoTune::backup_gains_and_initialise() @@ -936,6 +939,7 @@ void AC_AutoTune::backup_gains_and_initialise()
orig_pitch_ri = attitude_control->get_rate_pitch_pid().kI();
orig_pitch_rd = attitude_control->get_rate_pitch_pid().kD();
orig_pitch_rff = attitude_control->get_rate_pitch_pid().ff();
orig_pitch_fltt = attitude_control->get_rate_pitch_pid().filt_T_hz();
orig_pitch_sp = attitude_control->get_angle_pitch_p().kP();
orig_pitch_accel = attitude_control->get_accel_pitch_max();
tune_pitch_rp = attitude_control->get_rate_pitch_pid().kP();
@ -947,6 +951,7 @@ void AC_AutoTune::backup_gains_and_initialise() @@ -947,6 +951,7 @@ void AC_AutoTune::backup_gains_and_initialise()
orig_yaw_ri = attitude_control->get_rate_yaw_pid().kI();
orig_yaw_rd = attitude_control->get_rate_yaw_pid().kD();
orig_yaw_rff = attitude_control->get_rate_yaw_pid().ff();
orig_yaw_fltt = attitude_control->get_rate_yaw_pid().filt_T_hz();
orig_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_E_hz();
orig_yaw_accel = attitude_control->get_accel_yaw_max();
orig_yaw_sp = attitude_control->get_angle_yaw_p().kP();
@ -969,6 +974,7 @@ void AC_AutoTune::load_orig_gains() @@ -969,6 +974,7 @@ void AC_AutoTune::load_orig_gains()
attitude_control->get_rate_roll_pid().kI(orig_roll_ri);
attitude_control->get_rate_roll_pid().kD(orig_roll_rd);
attitude_control->get_rate_roll_pid().ff(orig_roll_rff);
attitude_control->get_rate_roll_pid().filt_T_hz(orig_roll_fltt);
attitude_control->get_angle_roll_p().kP(orig_roll_sp);
attitude_control->set_accel_roll_max(orig_roll_accel);
}
@ -979,6 +985,7 @@ void AC_AutoTune::load_orig_gains() @@ -979,6 +985,7 @@ void AC_AutoTune::load_orig_gains()
attitude_control->get_rate_pitch_pid().kI(orig_pitch_ri);
attitude_control->get_rate_pitch_pid().kD(orig_pitch_rd);
attitude_control->get_rate_pitch_pid().ff(orig_pitch_rff);
attitude_control->get_rate_pitch_pid().filt_T_hz(orig_pitch_fltt);
attitude_control->get_angle_pitch_p().kP(orig_pitch_sp);
attitude_control->set_accel_pitch_max(orig_pitch_accel);
}
@ -990,6 +997,7 @@ void AC_AutoTune::load_orig_gains() @@ -990,6 +997,7 @@ void AC_AutoTune::load_orig_gains()
attitude_control->get_rate_yaw_pid().kD(orig_yaw_rd);
attitude_control->get_rate_yaw_pid().ff(orig_yaw_rff);
attitude_control->get_rate_yaw_pid().filt_E_hz(orig_yaw_rLPF);
attitude_control->get_rate_yaw_pid().filt_T_hz(orig_yaw_fltt);
attitude_control->get_angle_yaw_p().kP(orig_yaw_sp);
attitude_control->set_accel_yaw_max(orig_yaw_accel);
}
@ -1049,6 +1057,7 @@ void AC_AutoTune::load_intra_test_gains() @@ -1049,6 +1057,7 @@ void AC_AutoTune::load_intra_test_gains()
attitude_control->get_rate_roll_pid().kI(orig_roll_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
attitude_control->get_rate_roll_pid().kD(orig_roll_rd);
attitude_control->get_rate_roll_pid().ff(orig_roll_rff);
attitude_control->get_rate_roll_pid().filt_T_hz(orig_roll_fltt);
attitude_control->get_angle_roll_p().kP(orig_roll_sp);
}
if (pitch_enabled()) {
@ -1056,6 +1065,7 @@ void AC_AutoTune::load_intra_test_gains() @@ -1056,6 +1065,7 @@ void AC_AutoTune::load_intra_test_gains()
attitude_control->get_rate_pitch_pid().kI(orig_pitch_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
attitude_control->get_rate_pitch_pid().kD(orig_pitch_rd);
attitude_control->get_rate_pitch_pid().ff(orig_pitch_rff);
attitude_control->get_rate_pitch_pid().filt_T_hz(orig_pitch_fltt);
attitude_control->get_angle_pitch_p().kP(orig_pitch_sp);
}
if (yaw_enabled()) {
@ -1063,6 +1073,7 @@ void AC_AutoTune::load_intra_test_gains() @@ -1063,6 +1073,7 @@ void AC_AutoTune::load_intra_test_gains()
attitude_control->get_rate_yaw_pid().kI(orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
attitude_control->get_rate_yaw_pid().kD(orig_yaw_rd);
attitude_control->get_rate_yaw_pid().ff(orig_yaw_rff);
attitude_control->get_rate_yaw_pid().filt_T_hz(orig_yaw_fltt);
attitude_control->get_rate_yaw_pid().filt_E_hz(orig_yaw_rLPF);
attitude_control->get_angle_yaw_p().kP(orig_yaw_sp);
}
@ -1078,6 +1089,7 @@ void AC_AutoTune::load_twitch_gains() @@ -1078,6 +1089,7 @@ void AC_AutoTune::load_twitch_gains()
attitude_control->get_rate_roll_pid().kI(tune_roll_rp*0.01f);
attitude_control->get_rate_roll_pid().kD(tune_roll_rd);
attitude_control->get_rate_roll_pid().ff(0.0f);
attitude_control->get_rate_roll_pid().filt_T_hz(0.0f);
attitude_control->get_angle_roll_p().kP(tune_roll_sp);
break;
case PITCH:
@ -1085,6 +1097,7 @@ void AC_AutoTune::load_twitch_gains() @@ -1085,6 +1097,7 @@ void AC_AutoTune::load_twitch_gains()
attitude_control->get_rate_pitch_pid().kI(tune_pitch_rp*0.01f);
attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd);
attitude_control->get_rate_pitch_pid().ff(0.0f);
attitude_control->get_rate_pitch_pid().filt_T_hz(0.0f);
attitude_control->get_angle_pitch_p().kP(tune_pitch_sp);
break;
case YAW:
@ -1093,6 +1106,7 @@ void AC_AutoTune::load_twitch_gains() @@ -1093,6 +1106,7 @@ void AC_AutoTune::load_twitch_gains()
attitude_control->get_rate_yaw_pid().kD(0.0f);
attitude_control->get_rate_yaw_pid().ff(0.0f);
attitude_control->get_rate_yaw_pid().filt_E_hz(tune_yaw_rLPF);
attitude_control->get_rate_yaw_pid().filt_T_hz(0.0f);
attitude_control->get_angle_yaw_p().kP(tune_yaw_sp);
break;
}
@ -1141,6 +1155,7 @@ void AC_AutoTune::save_tuning_gains() @@ -1141,6 +1155,7 @@ void AC_AutoTune::save_tuning_gains()
attitude_control->get_rate_roll_pid().kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL);
attitude_control->get_rate_roll_pid().kD(tune_roll_rd);
attitude_control->get_rate_roll_pid().ff(orig_roll_rff);
attitude_control->get_rate_roll_pid().filt_T_hz(orig_roll_fltt);
attitude_control->get_rate_roll_pid().save_gains();
// stabilize roll
@ -1165,6 +1180,7 @@ void AC_AutoTune::save_tuning_gains() @@ -1165,6 +1180,7 @@ void AC_AutoTune::save_tuning_gains()
attitude_control->get_rate_pitch_pid().kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL);
attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd);
attitude_control->get_rate_pitch_pid().ff(orig_pitch_rff);
attitude_control->get_rate_pitch_pid().filt_T_hz(orig_pitch_fltt);
attitude_control->get_rate_pitch_pid().save_gains();
// stabilize pitch
@ -1189,6 +1205,7 @@ void AC_AutoTune::save_tuning_gains() @@ -1189,6 +1205,7 @@ void AC_AutoTune::save_tuning_gains()
attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL);
attitude_control->get_rate_yaw_pid().kD(0.0f);
attitude_control->get_rate_yaw_pid().ff(orig_yaw_rff);
attitude_control->get_rate_yaw_pid().filt_T_hz(orig_yaw_fltt);
attitude_control->get_rate_yaw_pid().filt_E_hz(tune_yaw_rLPF);
attitude_control->get_rate_yaw_pid().save_gains();
@ -1232,6 +1249,9 @@ void AC_AutoTune::update_gcs(uint8_t message_id) @@ -1232,6 +1249,9 @@ void AC_AutoTune::update_gcs(uint8_t message_id)
case AUTOTUNE_MESSAGE_FAILED:
gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: Failed");
break;
case AUTOTUNE_MESSAGE_TESTING:
gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: Pilot Testing");
break;
case AUTOTUNE_MESSAGE_SAVED_GAINS:
gcs().send_text(MAV_SEVERITY_NOTICE,"AutoTune: Saved gains for %s%s%s",
(axes_completed&AUTOTUNE_AXIS_BITMASK_ROLL)?"Roll ":"",

6
libraries/AC_AutoTune/AC_AutoTune.h
Unescape View File

@ -191,9 +191,9 @@ private: @@ -191,9 +191,9 @@ private:
LowPassFilterFloat rotation_rate_filt; // filtered rotation rate in radians/second
// backup of currently being tuned parameter values
float orig_roll_rp, orig_roll_ri, orig_roll_rd, orig_roll_rff, orig_roll_sp, orig_roll_accel;
float orig_pitch_rp, orig_pitch_ri, orig_pitch_rd, orig_pitch_rff, orig_pitch_sp, orig_pitch_accel;
float orig_yaw_rp, orig_yaw_ri, orig_yaw_rd, orig_yaw_rff, orig_yaw_rLPF, orig_yaw_sp, orig_yaw_accel;
float orig_roll_rp, orig_roll_ri, orig_roll_rd, orig_roll_rff, orig_roll_fltt, orig_roll_sp, orig_roll_accel;
float orig_pitch_rp, orig_pitch_ri, orig_pitch_rd, orig_pitch_rff, orig_pitch_fltt, orig_pitch_sp, orig_pitch_accel;
float orig_yaw_rp, orig_yaw_ri, orig_yaw_rd, orig_yaw_rff, orig_yaw_fltt, orig_yaw_rLPF, orig_yaw_sp, orig_yaw_accel;
bool orig_bf_feedforward;
// currently being tuned parameter values

Write Preview
Loading…
Cancel
Save