Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Vehicle: optionally run the harmonic notch update at the loop rate

Browse Source
This commit is contained in:
Andy Piper 2021-03-08 21:05:24 +00:00 committed by Andrew Tridgell
parent 8a3a609e3b
commit 9bc9873e38
2 changed files with 17 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_Vehicle/AP_Vehicle.cpp
View File

@ -219,7 +219,7 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = {
SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update, 400, 50), SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update, 400, 50),
SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update_parameters, 1, 50), SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update_parameters, 1, 50),
#endif #endif
SCHED_TASK(update_dynamic_notch, 200, 200), SCHED_TASK(update_dynamic_notch, LOOP_RATE, 200),
SCHED_TASK_CLASS(AP_VideoTX, &vehicle.vtx, update, 2, 100), SCHED_TASK_CLASS(AP_VideoTX, &vehicle.vtx, update, 2, 100),
SCHED_TASK(send_watchdog_reset_statustext, 0.1, 20), SCHED_TASK(send_watchdog_reset_statustext, 0.1, 20),
#if HAL_WITH_ESC_TELEM #if HAL_WITH_ESC_TELEM
@ -332,6 +332,18 @@ void AP_Vehicle::write_notch_log_messages() const
notches[0], notches[1], notches[2], notches[3]); notches[0], notches[1], notches[2], notches[3]);
} }
// run notch update at either loop rate or 200Hz
void AP_Vehicle::update_dynamic_notch_at_specified_rate()
{
const uint32_t now = AP_HAL::millis();
if (ins.has_harmonic_option(HarmonicNotchFilterParams::Options::LoopRateUpdate)
|| now - _last_notch_update_ms > 5) {
update_dynamic_notch();
_last_notch_update_ms = now;
}
}
// reboot the vehicle in an orderly manner, doing various cleanups and // reboot the vehicle in an orderly manner, doing various cleanups and
// flashing LEDs as appropriate // flashing LEDs as appropriate
void AP_Vehicle::reboot(bool hold_in_bootloader) void AP_Vehicle::reboot(bool hold_in_bootloader)

4
libraries/AP_Vehicle/AP_Vehicle.h
View File

@ -345,8 +345,12 @@ private:
// statustext: // statustext:
void send_watchdog_reset_statustext(); void send_watchdog_reset_statustext();
// run notch update at either loop rate or 200Hz
void update_dynamic_notch_at_specified_rate();
bool likely_flying; // true if vehicle is probably flying bool likely_flying; // true if vehicle is probably flying
uint32_t _last_flying_ms; // time when likely_flying last went true uint32_t _last_flying_ms; // time when likely_flying last went true
uint32_t _last_notch_update_ms; // last time update_dynamic_notch() was run
static AP_Vehicle *_singleton; static AP_Vehicle *_singleton;