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

AP_Vehicle: allow HarmonicNotches to be compiled out of the code

Browse Source
This commit is contained in:
Peter Barker 2024-02-13 11:26:07 +11:00 committed by Andrew Tridgell
parent ffb1fe0a1a
commit bafd73ba14
2 changed files with 14 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_Vehicle/AP_Vehicle.cpp
View File

@ -595,7 +595,9 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = {
SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update, 400, 50, 205), SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update, 400, 50, 205),
SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update_parameters, 1, 50, 210), SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update_parameters, 1, 50, 210),
#endif #endif
#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
SCHED_TASK(update_dynamic_notch_at_specified_rate, LOOP_RATE, 200, 215), SCHED_TASK(update_dynamic_notch_at_specified_rate, LOOP_RATE, 200, 215),
#endif
#if AP_VIDEOTX_ENABLED #if AP_VIDEOTX_ENABLED
SCHED_TASK_CLASS(AP_VideoTX, &vehicle.vtx, update, 2, 100, 220), SCHED_TASK_CLASS(AP_VideoTX, &vehicle.vtx, update, 2, 100, 220),
#endif #endif
@ -745,6 +747,7 @@ bool AP_Vehicle::is_crashed() const
#endif #endif
} }
#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
// update the harmonic notch filter for throttle based notch // update the harmonic notch filter for throttle based notch
void AP_Vehicle::update_throttle_notch(AP_InertialSensor::HarmonicNotch &notch) void AP_Vehicle::update_throttle_notch(AP_InertialSensor::HarmonicNotch &notch)
{ {
@ -862,6 +865,7 @@ void AP_Vehicle::update_dynamic_notch_at_specified_rate()
} }
} }
} }
#endif // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
void AP_Vehicle::notify_no_such_mode(uint8_t mode_number) void AP_Vehicle::notify_no_such_mode(uint8_t mode_number)
{ {
@ -1028,9 +1032,14 @@ void AP_Vehicle::one_Hz_update(void)
void AP_Vehicle::check_motor_noise() void AP_Vehicle::check_motor_noise()
{ {
#if HAL_GYROFFT_ENABLED && HAL_WITH_ESC_TELEM #if HAL_GYROFFT_ENABLED && HAL_WITH_ESC_TELEM
if (!hal.util->get_soft_armed() || !gyro_fft.check_esc_noise() || !gyro_fft.using_post_filter_samples() || ins.has_fft_notch()) { if (!hal.util->get_soft_armed() || !gyro_fft.check_esc_noise() || !gyro_fft.using_post_filter_samples()) {
return; return;
} }
#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
if (ins.has_fft_notch()) {
return;
}
#endif
float esc_data[ESC_TELEM_MAX_ESCS]; float esc_data[ESC_TELEM_MAX_ESCS];
const uint8_t numf = AP::esc_telem().get_motor_frequencies_hz(ESC_TELEM_MAX_ESCS, esc_data); const uint8_t numf = AP::esc_telem().get_motor_frequencies_hz(ESC_TELEM_MAX_ESCS, esc_data);

6
libraries/AP_Vehicle/AP_Vehicle.h
View File

@ -497,7 +497,7 @@ private:
// statustext: // statustext:
void send_watchdog_reset_statustext(); void send_watchdog_reset_statustext();
#if AP_INERTIALSENSOR_ENABLED #if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
// update the harmonic notch for throttle based notch // update the harmonic notch for throttle based notch
void update_throttle_notch(AP_InertialSensor::HarmonicNotch &notch); void update_throttle_notch(AP_InertialSensor::HarmonicNotch &notch);
@ -506,7 +506,7 @@ private:
// run notch update at either loop rate or 200Hz // run notch update at either loop rate or 200Hz
void update_dynamic_notch_at_specified_rate(); void update_dynamic_notch_at_specified_rate();
#endif #endif // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
// decimation for 1Hz update // decimation for 1Hz update
uint8_t one_Hz_counter; uint8_t one_Hz_counter;
@ -514,7 +514,9 @@ private:
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
#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
uint32_t _last_notch_update_ms[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS]; // last time update_dynamic_notch() was run uint32_t _last_notch_update_ms[HAL_INS_NUM_HARMONIC_NOTCH_FILTERS]; // last time update_dynamic_notch() was run
#endif
static AP_Vehicle *_singleton; static AP_Vehicle *_singleton;