mirror of https://github.com/ArduPilot/ardupilot
24 lines
596 B
C++
24 lines
596 B
C++
|
#include <AP_gtest.h>
|
||
|
|
#include <AP_HAL/HAL.h>
|
||
|
|
#include <AP_HAL/Util.h>
|
||
|
|
#include <AP_GyroFFT/AP_GyroFFT.h>
|
||
|
|
#include <stdio.h>
|
||
|
|
|
||
|
|
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
||
|
|
|
||
|
|
TEST(harmonic_fit_Test, Basic)
|
||
|
|
{
|
||
|
|
#if HAL_GYROFFT_ENABLED
|
||
|
|
float freqs[] { 176.9, 57.2, 228.7 };
|
||
|
|
uint8_t harmonics;
|
||
|
|
float harmonic = AP_GyroFFT::calculate_notch_frequency(freqs, 3, 10, harmonics);
|
||
|
|
|
||
|
|
printf("FFT: Found peaks at %.1f/%.1f/%.1fHz\n", freqs[0], freqs[1], freqs[2]);
|
||
|
|
printf("FFT: Selected %.1fHz %d\n", harmonic, harmonics);
|
||
|
|
|
||
|
|
EXPECT_TRUE(is_equal(harmonic, 57.2f));
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
|
||
|
|
AP_GTEST_MAIN()
|