Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GyroFFT: ensure we got float division when there are needed instead of integer divisions

Browse Source
This commit is contained in:
Pierre Kancir 2021-08-13 10:31:06 +02:00 committed by Peter Barker
parent 23a67a9e60
commit cc39eafe02
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_GyroFFT/AP_GyroFFT.cpp
View File

@ -283,7 +283,7 @@ void AP_GyroFFT::init(uint32_t target_looptime_us)
// per-axis frame time
_frame_time_ms = _samples_per_frame * 1000 / _fft_sampling_rate_hz;
// The update rate for the output, defaults are 1Khz / (1 - 0.5) * 32 == 62hz
const float output_rate = _fft_sampling_rate_hz / _samples_per_frame;
const float output_rate = static_cast<float>(_fft_sampling_rate_hz) / static_cast<float>(_samples_per_frame);
// establish suitable defaults for the detected values
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
_thread_state._center_freq_hz[axis] = _fft_min_hz;
@ -655,7 +655,7 @@ float AP_GyroFFT::get_slewed_noise_center_freq_hz(FrequencyPeak peak, uint8_t ax
{
uint32_t now = AP_HAL::micros();
const float slew = MIN(1.0f, (now - _global_state._last_output_us[axis])
* (_fft_sampling_rate_hz / _samples_per_frame) / 1e6f);
* (static_cast<float>(_fft_sampling_rate_hz) / static_cast<float>(_samples_per_frame)) * 1e-6f);
return (_global_state._prev_center_freq_hz_filtered[peak][axis]
+ (_global_state._center_freq_hz_filtered[peak][axis] - _global_state._prev_center_freq_hz_filtered[peak][axis]) * slew);
}
@ -1133,7 +1133,7 @@ void AP_GyroFFT::update_ref_energy(uint16_t max_bin)
}
if (--_noise_calibration_cycles[_update_axis] == 0) {
for (uint16_t i = 1; i < _state->_bin_count; i++) {
const float cycles = (_window_size / _samples_per_frame) * 2;
const float cycles = (static_cast<float>(_window_size) / static_cast<float>(_samples_per_frame)) * 2;
// overall random noise is reduced by sqrt(N) when averaging periodigrams so adjust for that
_ref_energy[_update_axis][i] = (_ref_energy[_update_axis][i] / cycles) * sqrtf(cycles);
}