mirror of https://github.com/ArduPilot/ardupilot
Sub: update PID notch centers at 1Hz with average loop rate
This commit is contained in:
parent
0d10f74bf7
commit
2442bca978
|
@ -291,6 +291,9 @@ void Sub::one_hz_loop()
|
|||
// need to set "likely flying" when armed to allow for compass
|
||||
// learning to run
|
||||
set_likely_flying(hal.util->get_soft_armed());
|
||||
|
||||
attitude_control.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
|
||||
pos_control.get_accel_z_pid().set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
|
||||
}
|
||||
|
||||
void Sub::read_AHRS()
|
||||
|
|
Loading…
Reference in New Issue