zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_HAL_ChibiOS: add harmonics to DSP 

add vector_mean_float() to DSP
allow fft_start() to use ObjectBuffer<float> for lock-free access
c415-sdk
Andy Piper 5 years ago committed by Andrew Tridgell
parent
ee87ef7013
commit
33c1523905
2 changed files with 34 additions and 34 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 44
      libraries/AP_HAL_ChibiOS/DSP.cpp
  2. 24
      libraries/AP_HAL_ChibiOS/DSP.h

44
libraries/AP_HAL_ChibiOS/DSP.cpp
Unescape View File

@ -49,10 +49,10 @@ extern const AP_HAL::HAL& hal;
// important as frequency resolution. Referred to as [Heinz] throughout the code. // important as frequency resolution. Referred to as [Heinz] throughout the code.
// initialize the FFT state machine // initialize the FFT state machine
AP_HAL::DSP::FFTWindowState* DSP::fft_init(uint16_t window_size, uint16_t sample_rate) AP_HAL::DSP::FFTWindowState* DSP::fft_init(uint16_t window_size, uint16_t sample_rate, uint8_t harmonics)
{ {
DSP::FFTWindowStateARM* fft = new DSP::FFTWindowStateARM(window_size, sample_rate); DSP::FFTWindowStateARM* fft = new DSP::FFTWindowStateARM(window_size, sample_rate, harmonics);
if (fft->_hanning_window == nullptr || fft->_rfft_data == nullptr || fft->_freq_bins == nullptr) { if (fft == nullptr || fft->_hanning_window == nullptr || fft->_rfft_data == nullptr || fft->_freq_bins == nullptr || fft->_derivative_freq_bins == nullptr) {
delete fft; delete fft;
return nullptr; return nullptr;
} }
@ -60,28 +60,28 @@ AP_HAL::DSP::FFTWindowState* DSP::fft_init(uint16_t window_size, uint16_t sample
} }
// start an FFT analysis // start an FFT analysis
void DSP::fft_start(AP_HAL::DSP::FFTWindowState* state, const float* samples, uint16_t buffer_index, uint16_t buffer_size) void DSP::fft_start(FFTWindowState* state, FloatBuffer& samples, uint16_t advance)
{ {
step_hanning((FFTWindowStateARM*)state, samples, buffer_index, buffer_size); step_hanning((FFTWindowStateARM*)state, samples, advance);
} }
// perform remaining steps of an FFT analysis // perform remaining steps of an FFT analysis
uint16_t DSP::fft_analyse(AP_HAL::DSP::FFTWindowState* state, uint16_t start_bin, uint16_t end_bin, uint8_t harmonics, float noise_att_cutoff) uint16_t DSP::fft_analyse(AP_HAL::DSP::FFTWindowState* state, uint16_t start_bin, uint16_t end_bin, float noise_att_cutoff)
{ {
FFTWindowStateARM* fft = (FFTWindowStateARM*)state; FFTWindowStateARM* fft = (FFTWindowStateARM*)state;
step_arm_cfft_f32(fft); step_arm_cfft_f32(fft);
step_bitreversal(fft); step_bitreversal(fft);
step_stage_rfft_f32(fft); step_stage_rfft_f32(fft);
step_arm_cmplx_mag_f32(fft, start_bin, end_bin, harmonics, noise_att_cutoff); step_arm_cmplx_mag_f32(fft, start_bin, end_bin, noise_att_cutoff);
return step_calc_frequencies_f32(fft, start_bin, end_bin); return step_calc_frequencies_f32(fft, start_bin, end_bin);
} }
// create an instance of the FFT state machine // create an instance of the FFT state machine
DSP::FFTWindowStateARM::FFTWindowStateARM(uint16_t window_size, uint16_t sample_rate) DSP::FFTWindowStateARM::FFTWindowStateARM(uint16_t window_size, uint16_t sample_rate, uint8_t harmonics)
: AP_HAL::DSP::FFTWindowState::FFTWindowState(window_size, sample_rate) : AP_HAL::DSP::FFTWindowState::FFTWindowState(window_size, sample_rate, harmonics)
{ {
if (_freq_bins == nullptr || _hanning_window == nullptr || _rfft_data == nullptr) { if (_freq_bins == nullptr || _hanning_window == nullptr || _rfft_data == nullptr || _derivative_freq_bins == nullptr) {
gcs().send_text(MAV_SEVERITY_WARNING, "Failed to allocate %u bytes for window %u for DSP", GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Failed to allocate %u bytes for window %u for DSP",
unsigned(sizeof(float) * (window_size * 3 + 2)), unsigned(window_size)); unsigned(sizeof(float) * (window_size * 3 + 2)), unsigned(window_size));
return; return;
} }
@ -115,9 +115,7 @@ DSP::FFTWindowStateARM::FFTWindowStateARM(uint16_t window_size, uint16_t sample_
} }
} }
DSP::FFTWindowStateARM::~FFTWindowStateARM() DSP::FFTWindowStateARM::~FFTWindowStateARM() {}
{
}
extern "C" { extern "C" {
void stage_rfft_f32(arm_rfft_fast_instance_f32 *S, float32_t *p, float32_t *pOut); void stage_rfft_f32(arm_rfft_fast_instance_f32 *S, float32_t *p, float32_t *pOut);
@ -128,17 +126,15 @@ extern "C" {
} }
// step 1: filter the incoming samples through a Hanning window // step 1: filter the incoming samples through a Hanning window
void DSP::step_hanning(FFTWindowStateARM* fft, const float* samples, uint16_t buffer_index, uint16_t buffer_size) void DSP::step_hanning(FFTWindowStateARM* fft, FloatBuffer& samples, uint16_t advance)
{ {
TIMER_START(_hanning_timer); TIMER_START(_hanning_timer);
// 5us // 5us
// apply hanning window to gyro samples and store result in _freq_bins // apply hanning window to gyro samples and store result in _freq_bins
// hanning starts and ends with 0, could be skipped for minor speed improvement // hanning starts and ends with 0, could be skipped for minor speed improvement
const uint16_t ring_buf_idx = MIN(buffer_size - buffer_index, fft->_window_size); samples.peek(&fft->_freq_bins[0], fft->_window_size); // the caller ensures we get a full buffer of samples
arm_mult_f32(&samples[buffer_index], &fft->_hanning_window[0], &fft->_freq_bins[0], ring_buf_idx); samples.advance(advance);
if (buffer_index > 0) { arm_mult_f32(&fft->_freq_bins[0], &fft->_hanning_window[0], &fft->_freq_bins[0], fft->_window_size);
arm_mult_f32(&samples[0], &fft->_hanning_window[ring_buf_idx], &fft->_freq_bins[ring_buf_idx], fft->_window_size - ring_buf_idx);
}
TIMER_END(_hanning_timer); TIMER_END(_hanning_timer);
} }
@ -212,7 +208,7 @@ void DSP::step_stage_rfft_f32(FFTWindowStateARM* fft)
} }
// step 5: find the magnitudes of the complex data // step 5: find the magnitudes of the complex data
void DSP::step_arm_cmplx_mag_f32(FFTWindowStateARM* fft, uint16_t start_bin, uint16_t end_bin, uint8_t harmonics, float noise_att_cutoff) void DSP::step_arm_cmplx_mag_f32(FFTWindowStateARM* fft, uint16_t start_bin, uint16_t end_bin, float noise_att_cutoff)
{ {
TIMER_START(_arm_cmplx_mag_f32_timer); TIMER_START(_arm_cmplx_mag_f32_timer);
// 8us (BF) // 8us (BF)
@ -231,7 +227,7 @@ void DSP::step_arm_cmplx_mag_f32(FFTWindowStateARM* fft, uint16_t start_bin, uin
fft->_rfft_data[fft->_window_size] = fft->_rfft_data[1]; // Nyquist for the interpolator fft->_rfft_data[fft->_window_size] = fft->_rfft_data[1]; // Nyquist for the interpolator
fft->_rfft_data[fft->_window_size + 1] = 0; fft->_rfft_data[fft->_window_size + 1] = 0;
step_cmplx_mag(fft, start_bin, end_bin, harmonics, noise_att_cutoff); step_cmplx_mag(fft, start_bin, end_bin, noise_att_cutoff);
TIMER_END(_arm_cmplx_mag_f32_timer); TIMER_END(_arm_cmplx_mag_f32_timer);
} }
@ -250,12 +246,12 @@ uint16_t DSP::step_calc_frequencies_f32(FFTWindowStateARM* fft, uint16_t start_b
_output_count++; _output_count++;
// outputs at approx 1hz // outputs at approx 1hz
if (_output_count % 400 == 0) { if (_output_count % 400 == 0) {
gcs().send_text(MAV_SEVERITY_WARNING, "FFT(us): t1:%lu,t2:%lu,t3:%lu,t4:%lu,t5:%lu,t6:%lu", GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "FFT(us): t1:%lu,t2:%lu,t3:%lu,t4:%lu,t5:%lu,t6:%lu",
_hanning_timer._timer_avg, _arm_cfft_f32_timer._timer_avg, _bitreversal_timer._timer_avg, _stage_rfft_f32_timer._timer_avg, _arm_cmplx_mag_f32_timer._timer_avg, _step_calc_frequencies._timer_avg); _hanning_timer._timer_avg, _arm_cfft_f32_timer._timer_avg, _bitreversal_timer._timer_avg, _stage_rfft_f32_timer._timer_avg, _arm_cmplx_mag_f32_timer._timer_avg, _step_calc_frequencies._timer_avg);
} }
#endif #endif
return fft->_max_energy_bin; return fft->_peak_data[CENTER]._bin;
} }
static const float PI_N = M_PI / 32.0f; static const float PI_N = M_PI / 32.0f;

24
libraries/AP_HAL_ChibiOS/DSP.h
Unescape View File

@ -29,19 +29,18 @@
class ChibiOS::DSP : public AP_HAL::DSP { class ChibiOS::DSP : public AP_HAL::DSP {
public: public:
// initialise an FFT instance // initialise an FFT instance
virtual FFTWindowState* fft_init(uint16_t window_size, uint16_t sample_rate) override; virtual FFTWindowState* fft_init(uint16_t window_size, uint16_t sample_rate, uint8_t harmonics) override;
// start an FFT analysis // start an FFT analysis with an ObjectBuffer
virtual void fft_start(FFTWindowState* state, const float* samples, uint16_t buffer_index, uint16_t buffer_size) override; virtual void fft_start(FFTWindowState* state, FloatBuffer& samples, uint16_t advance) override;
// perform remaining steps of an FFT analysis // perform remaining steps of an FFT analysis
virtual uint16_t fft_analyse(FFTWindowState* state, uint16_t start_bin, uint16_t end_bin, uint8_t harmonics, float noise_att_cutoff) override; virtual uint16_t fft_analyse(FFTWindowState* state, uint16_t start_bin, uint16_t end_bin, float noise_att_cutoff) override;
// STM32-based FFT state // STM32-based FFT state
class FFTWindowStateARM : public AP_HAL::DSP::FFTWindowState { class FFTWindowStateARM : public AP_HAL::DSP::FFTWindowState {
friend class ChibiOS::DSP; friend class ChibiOS::DSP;
public:
protected: FFTWindowStateARM(uint16_t window_size, uint16_t sample_rate, uint8_t harmonics);
FFTWindowStateARM(uint16_t window_size, uint16_t sample_rate); virtual ~FFTWindowStateARM();
~FFTWindowStateARM();
private: private:
// underlying CMSIS data structure for FFT analysis // underlying CMSIS data structure for FFT analysis
@ -57,14 +56,19 @@ protected:
void vector_scale_float(const float* vin, float scale, float* vout, uint16_t len) const override { void vector_scale_float(const float* vin, float scale, float* vout, uint16_t len) const override {
arm_scale_f32(vin, scale, vout, len); arm_scale_f32(vin, scale, vout, len);
} }
float vector_mean_float(const float* vin, uint16_t len) const override {
float mean_value;
arm_mean_f32(vin, len, &mean_value);
return mean_value;
}
private: private:
// following are the six independent steps for calculating an FFT // following are the six independent steps for calculating an FFT
void step_hanning(FFTWindowStateARM* fft, const float* samples, uint16_t buffer_index, uint16_t buffer_size); void step_hanning(FFTWindowStateARM* fft, FloatBuffer& samples, uint16_t advance);
void step_arm_cfft_f32(FFTWindowStateARM* fft); void step_arm_cfft_f32(FFTWindowStateARM* fft);
void step_bitreversal(FFTWindowStateARM* fft); void step_bitreversal(FFTWindowStateARM* fft);
void step_stage_rfft_f32(FFTWindowStateARM* fft); void step_stage_rfft_f32(FFTWindowStateARM* fft);
void step_arm_cmplx_mag_f32(FFTWindowStateARM* fft, uint16_t start_bin, uint16_t end_bin, uint8_t harmonics, float noise_att_cutoff); void step_arm_cmplx_mag_f32(FFTWindowStateARM* fft, uint16_t start_bin, uint16_t end_bin, float noise_att_cutoff);
uint16_t step_calc_frequencies_f32(FFTWindowStateARM* fft, uint16_t start_bin, uint16_t end_bin); uint16_t step_calc_frequencies_f32(FFTWindowStateARM* fft, uint16_t start_bin, uint16_t end_bin);
// candan's frequency interpolator // candan's frequency interpolator
float calculate_candans_estimator(const FFTWindowStateARM* fft, uint16_t k) const; float calculate_candans_estimator(const FFTWindowStateARM* fft, uint16_t k) const;

Write Preview
Loading…
Cancel
Save