Skip to content

Commit 1d6d112

Browse files
andyp1pertridge
authored andcommitted
AP_HAL: protect against invalid values when calculating Jain's estimator
1 parent ec215c3 commit 1d6d112

File tree

1 file changed

+5
-0
lines changed

1 file changed

+5
-0
lines changed

libraries/AP_HAL/DSP.cpp

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -286,6 +286,11 @@ float DSP::calculate_jains_estimator(const FFTWindowState* fft, const float* rea
286286
float y1 = real_fft[k_max-1];
287287
float y2 = real_fft[k_max];
288288
float y3 = real_fft[k_max+1];
289+
290+
if (is_zero(y2) || is_zero(y1)) {
291+
return 0.0f;
292+
}
293+
289294
float d = 0.0f;
290295

291296
if (y1 > y3) {

0 commit comments

Comments
 (0)