AP_GyroFFT: add FFT_OPTIONS to allow post-filter sampling of IMUs
provide method to determine noise at a particular frequency add ability to record per-peak SnR correct bad array indexing leading to free memory read track all three axes for health and peaks remove slewed frequency values, since slewing is now done in the filters ReplayGyroFFT initial implementation allow IMU data to be read and written from a file only build Replay on SITL correctly calibrate FFT in Replay better noise simulation in Replay FTN3 logging allow FFT peaks to swap indefinitely as long as they both still exist. Leads to much smoother frequency transitions increase the energy gap required to switch the tracked peak use exit on stop for Replay filter noise tracking more aggressively for post-filter samples remove message and use appropriate gyro window when using post-filter do not fallback to throttle-based estimate AP_GyroFFT: default SnR to 10 when using post-filter samples
This commit is contained in:
Andy Piper
Andrew Tridgell
parent
cbecb20614
commit
449d9814ab
@ -49,11 +49,12 @@ extern const AP_HAL::HAL& hal;
|
||||
#endif
|
||||
#define FFT_THR_REF_DEFAULT 0.35f // the estimated throttle reference, 0 ~ 1
|
||||
#define FFT_SNR_DEFAULT 25.0f // a higher SNR is safer and this works quite well on a Pixracer
|
||||
#define FFT_SNR_PFILT_DEFAULT 10.0f // post-filter there is much less noise so default should be lower
|
||||
#define FFT_STACK_SIZE 1024
|
||||
#define FFT_MIN_SAMPLES_PER_FRAME 16
|
||||
#define FFT_HARMONIC_FIT_DEFAULT 10
|
||||
#define FFT_HARMONIC_FIT_FILTER_HZ 15.0f
|
||||
#define FFT_HARMONIC_FIT_MULT 200.0f
|
||||
#define FFT_HARMONIC_FIT_MULT 50.0f
|
||||
#define FFT_HARMONIC_FIT_TRACK_ROLL 4
|
||||
#define FFT_HARMONIC_FIT_TRACK_PITCH 5
|
||||
|
||||
@ -166,6 +167,14 @@ const AP_Param::GroupInfo AP_GyroFFT::var_info[] = {
|
||||
// @RebootRequired: True
|
||||
AP_GROUPINFO("NUM_FRAMES", 14, AP_GyroFFT, _num_frames, 0),
|
||||
|
||||
// @Param: OPTIONS
|
||||
// @DisplayName: FFT options
|
||||
// @Description: FFT configuration options. Values: 1:Apply the FFT *after* the filter bank,2:Check noise at the motor frequencies using ESC data as a reference
|
||||
// @Bitmask: 0:Enable post-filter FFT,1:Check motor noise
|
||||
// @User: Advanced
|
||||
// @RebootRequired: True
|
||||
AP_GROUPINFO("OPTIONS", 15, AP_GyroFFT, _options, 0),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
@ -325,13 +334,20 @@ void AP_GyroFFT::init(uint16_t loop_rate_hz)
|
||||
}
|
||||
|
||||
// configure a filter for frequency, bandwidth and energy for each of the three tracked noise peaks
|
||||
// filter more aggressively post-filter since the noise is harder to detect
|
||||
const float scale_factor = using_post_filter_samples() ? 0.1f : 1.0f;
|
||||
for (uint8_t peak = 0; peak < FrequencyPeak::MAX_TRACKED_PEAKS; peak++) {
|
||||
// calculate low-pass filter characteristics based on window size and overlap
|
||||
_center_freq_filter[peak].set_cutoff_frequency(output_rate, output_rate * 0.48f);
|
||||
_center_freq_filter[peak].set_cutoff_frequency(output_rate, output_rate * 0.48f * scale_factor);
|
||||
// the bin energy jumps around a lot so requires more filtering
|
||||
_center_freq_energy_filter[peak].set_cutoff_frequency(output_rate, output_rate * 0.25f);
|
||||
_center_freq_energy_filter[peak].set_cutoff_frequency(output_rate, output_rate * 0.25f * scale_factor);
|
||||
// smooth the bandwidth output more aggressively
|
||||
_center_bandwidth_filter[peak].set_cutoff_frequency(output_rate, output_rate * 0.25f);
|
||||
_center_bandwidth_filter[peak].set_cutoff_frequency(output_rate, output_rate * 0.25f * scale_factor);
|
||||
}
|
||||
|
||||
// turn down the SNR threshold if examining post-filter
|
||||
if (using_post_filter_samples()) {
|
||||
_snr_threshold_db.set_default(FFT_SNR_PFILT_DEFAULT);
|
||||
}
|
||||
|
||||
// the number of cycles required to have a proper noise reference
|
||||
@ -357,7 +373,7 @@ void AP_GyroFFT::sample_gyros()
|
||||
// update counters for gyro window
|
||||
if (_current_sample_mode > 0) {
|
||||
// for loop rate sampling accumulate and average gyro samples
|
||||
_oversampled_gyro_accum += _ins->get_raw_gyro();
|
||||
_oversampled_gyro_accum += _ins->get_gyro_for_fft();
|
||||
_oversampled_gyro_count++;
|
||||
|
||||
if ((_oversampled_gyro_count % _current_sample_mode) == 0) {
|
||||
@ -398,16 +414,15 @@ void AP_GyroFFT::update()
|
||||
_rpy_health.y = (now - _global_state._health_ms.y <= output_delay);
|
||||
_rpy_health.z = (now - _global_state._health_ms.z <= output_delay);
|
||||
|
||||
if (!_rpy_health.x && !_rpy_health.y) {
|
||||
_health = 0;
|
||||
} else {
|
||||
uint8_t num_notches = 1;
|
||||
for (auto ¬ch : _ins->harmonic_notches) {
|
||||
if (notch.params.enabled()) {
|
||||
num_notches = MAX(num_notches, notch.num_dynamic_notches);
|
||||
_health = _global_state._health;
|
||||
if (!_rpy_health.x) {
|
||||
_health.x = 0;
|
||||
}
|
||||
if (!_rpy_health.y) {
|
||||
_health.y = 0;
|
||||
}
|
||||
_health = MIN(_global_state._health, num_notches);
|
||||
if (!_rpy_health.z) {
|
||||
_health.z = 0;
|
||||
}
|
||||
}
|
||||
|
||||
@ -458,6 +473,27 @@ uint16_t AP_GyroFFT::run_cycle()
|
||||
_thread_state._last_output_us[_update_axis] = AP_HAL::micros();
|
||||
_output_cycle_micros = _thread_state._last_output_us[_update_axis] - now;
|
||||
|
||||
#if AP_SIM_ENABLED
|
||||
// extra logging when running simulations
|
||||
AP::logger().WriteStreaming(
|
||||
"FTN3",
|
||||
"TimeUS,Id,Pk1,Pk2,Pk3,Bw1,Bw2,Bw3,En1,En2,En3",
|
||||
"s#zzzzzz---",
|
||||
"F----------",
|
||||
"QBfffffffff",
|
||||
AP_HAL::micros64(),
|
||||
_update_axis,
|
||||
_state->_peak_data[0]._freq_hz,
|
||||
_state->_peak_data[1]._freq_hz,
|
||||
_state->_peak_data[2]._freq_hz,
|
||||
_state->_peak_data[0]._noise_width_hz,
|
||||
_state->_peak_data[1]._noise_width_hz,
|
||||
_state->_peak_data[2]._noise_width_hz,
|
||||
_state->_freq_bins[_state->_peak_data[0]._bin],
|
||||
_state->_freq_bins[_state->_peak_data[1]._bin],
|
||||
_state->_freq_bins[_state->_peak_data[2]._bin]);
|
||||
#endif
|
||||
|
||||
// move onto the next axis
|
||||
_update_axis = (_update_axis + 1) % XYZ_AXIS_COUNT;
|
||||
|
||||
@ -575,7 +611,7 @@ bool AP_GyroFFT::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len)
|
||||
return false;
|
||||
}
|
||||
|
||||
// make sure the frequency maxium is below Nyquist
|
||||
// make sure the frequency maximum is below Nyquist
|
||||
if (_fft_max_hz > _fft_sampling_rate_hz * 0.5f) {
|
||||
hal.util->snprintf(failure_msg, failure_msg_len, "FFT config MAXHZ %dHz > %dHz", _fft_max_hz.get(), _fft_sampling_rate_hz / 2);
|
||||
return false;
|
||||
@ -754,27 +790,16 @@ AP_GyroFFT::FrequencyPeak AP_GyroFFT::get_tracked_noise_peak() const
|
||||
return FrequencyPeak::CENTER;
|
||||
}
|
||||
|
||||
// center frequency slewed from previous to current value at the output rate
|
||||
float AP_GyroFFT::get_slewed_noise_center_freq_hz(FrequencyPeak peak, uint8_t axis) const
|
||||
{
|
||||
uint32_t now = AP_HAL::micros();
|
||||
const float slew = MIN(1.0f, (now - _global_state._last_output_us[axis])
|
||||
* (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);
|
||||
}
|
||||
|
||||
// weighted center frequency slewed from previous to current value at the output rate
|
||||
float AP_GyroFFT::get_slewed_weighted_freq_hz(FrequencyPeak peak) const
|
||||
// weighted center frequency
|
||||
float AP_GyroFFT::get_weighted_freq_hz(FrequencyPeak peak) const
|
||||
{
|
||||
const Vector3f& energy = get_center_freq_energy(peak);
|
||||
const float freq_x = get_slewed_noise_center_freq_hz(peak, 0);
|
||||
const float freq_y = get_slewed_noise_center_freq_hz(peak, 1);
|
||||
const Vector3f& freq = get_noise_center_freq_hz(peak);
|
||||
|
||||
if (!energy.is_nan() && !is_zero(energy.x) && !is_zero(energy.y)) {
|
||||
return (freq_x * energy.x + freq_y * energy.y) / (energy.x + energy.y);
|
||||
return (freq.x * energy.x + freq.y * energy.y) / (energy.x + energy.y);
|
||||
} else {
|
||||
return (freq_x + freq_y) * 0.5f;
|
||||
return (freq.x + freq.y) * 0.5f;
|
||||
}
|
||||
}
|
||||
|
||||
@ -786,11 +811,15 @@ float AP_GyroFFT::get_weighted_noise_center_freq_hz() const
|
||||
return _fft_min_hz;
|
||||
}
|
||||
|
||||
if (!_health) {
|
||||
if (_health.is_zero()) {
|
||||
#if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)
|
||||
// if we are post-filter sampling then throttle estimate will be useless
|
||||
if (using_post_filter_samples()) {
|
||||
return 0.0f;
|
||||
}
|
||||
AP_Motors* motors = AP::motors();
|
||||
if (motors != nullptr && !is_zero(_throttle_ref)) {
|
||||
// FFT is not healthy, fallback to throttle-based estimate
|
||||
// FFT is not healthy, fallback to FFT's throttle-based estimate
|
||||
return constrain_float(_fft_min_hz * MAX(1.0f, sqrtf(motors->get_throttle_out() / _throttle_ref)), _fft_min_hz, _fft_max_hz);
|
||||
}
|
||||
#endif
|
||||
@ -798,15 +827,15 @@ float AP_GyroFFT::get_weighted_noise_center_freq_hz() const
|
||||
|
||||
const FrequencyPeak peak = get_tracked_noise_peak();
|
||||
// pitch was good or required, roll was not, use pitch only
|
||||
if (!_rpy_health.x || _harmonic_peak == FFT_HARMONIC_FIT_TRACK_PITCH) {
|
||||
return get_slewed_noise_center_freq_hz(peak, 1); // Y-axis
|
||||
if (!_health.x || _harmonic_peak == FFT_HARMONIC_FIT_TRACK_PITCH) {
|
||||
return get_noise_center_freq_hz(peak).y; // Y-axis
|
||||
}
|
||||
// roll was good or required, pitch was not, use roll only
|
||||
if (!_rpy_health.y || _harmonic_peak == FFT_HARMONIC_FIT_TRACK_ROLL) {
|
||||
return get_slewed_noise_center_freq_hz(peak, 0); // X-axis
|
||||
if (!_health.y || _harmonic_peak == FFT_HARMONIC_FIT_TRACK_ROLL) {
|
||||
return get_noise_center_freq_hz(peak).x; // X-axis
|
||||
}
|
||||
|
||||
return get_slewed_weighted_freq_hz(peak);
|
||||
return get_weighted_freq_hz(peak);
|
||||
}
|
||||
|
||||
// return all the center frequencies weighted by bin energy
|
||||
@ -818,39 +847,73 @@ uint8_t AP_GyroFFT::get_weighted_noise_center_frequencies_hz(uint8_t num_freqs,
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (!_health) {
|
||||
if (_health.is_zero()) {
|
||||
#if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)
|
||||
// if we are post-filter sampling then throttle estimate will be useless
|
||||
if (using_post_filter_samples()) {
|
||||
return 0;
|
||||
}
|
||||
AP_Motors* motors = AP::motors();
|
||||
if (motors != nullptr) {
|
||||
// FFT is not healthy, fallback to throttle-based estimate
|
||||
// FFT is not healthy, fallback to FFT's throttle-based estimate
|
||||
freqs[0] = constrain_float(_fft_min_hz * MAX(1.0f, sqrtf(motors->get_throttle_out() / _throttle_ref)), _fft_min_hz, _fft_max_hz);
|
||||
return 1;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
const uint8_t tracked_peaks = MIN(_health, num_freqs);
|
||||
// pitch was good or required, roll was not, use pitch only
|
||||
if (!_rpy_health.x || _harmonic_peak == FFT_HARMONIC_FIT_TRACK_PITCH) {
|
||||
if (!_health.x || _harmonic_peak == FFT_HARMONIC_FIT_TRACK_PITCH) {
|
||||
const uint8_t tracked_peaks = MIN(_health.y, num_freqs);
|
||||
for (uint8_t i = 0; i < tracked_peaks; i++) {
|
||||
freqs[i] = get_slewed_noise_center_freq_hz(FrequencyPeak(i), 1); // Y-axis
|
||||
freqs[i] = get_noise_center_freq_hz(FrequencyPeak(i)).y; // Y-axis
|
||||
}
|
||||
return tracked_peaks;
|
||||
}
|
||||
// roll was good or required, pitch was not, use roll only
|
||||
if (!_rpy_health.y || _harmonic_peak == FFT_HARMONIC_FIT_TRACK_ROLL) {
|
||||
if (!_health.y || _harmonic_peak == FFT_HARMONIC_FIT_TRACK_ROLL) {
|
||||
const uint8_t tracked_peaks = MIN(_health.x, num_freqs);
|
||||
for (uint8_t i = 0; i < tracked_peaks; i++) {
|
||||
freqs[i] = get_slewed_noise_center_freq_hz(FrequencyPeak(i), 0); // X-axis
|
||||
freqs[i] = get_noise_center_freq_hz(FrequencyPeak(i)).x; // X-axis
|
||||
}
|
||||
return tracked_peaks;
|
||||
}
|
||||
|
||||
const uint8_t tracked_peaks = MIN(MAX(_health.x, _health.y), num_freqs);
|
||||
for (uint8_t i = 0; i < tracked_peaks; i++) {
|
||||
freqs[i] = get_slewed_weighted_freq_hz(FrequencyPeak(i));
|
||||
freqs[i] = get_weighted_freq_hz(FrequencyPeak(i));
|
||||
}
|
||||
return tracked_peaks;
|
||||
}
|
||||
|
||||
// return noise energy at the requested frequency
|
||||
float AP_GyroFFT::has_noise_at_frequency_hz(float freq) const
|
||||
{
|
||||
if (!analysis_enabled()) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
float max_energy = 0.0f;
|
||||
|
||||
// check each axis of each peak to see if it contains the pass frequency
|
||||
for (uint8_t i = 0; i < _tracked_peaks; i++) {
|
||||
const Vector3f& noise = get_noise_center_freq_hz(FrequencyPeak(i));
|
||||
const Vector3f& snr = get_noise_signal_to_noise_db(FrequencyPeak(i));
|
||||
|
||||
for (uint8_t j = 0; j < XYZ_AXIS_COUNT; j++) {
|
||||
if (!_rpy_health[j]) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// only check one bin either side of the frequency
|
||||
if ((noise[j] - _state->_bin_resolution) < freq && (noise[j] + _state->_bin_resolution) > freq) {
|
||||
max_energy = MAX(snr[j], max_energy);
|
||||
}
|
||||
}
|
||||
}
|
||||
return max_energy;
|
||||
}
|
||||
|
||||
float AP_GyroFFT::calculate_weighted_freq_hz(const Vector3f& energy, const Vector3f& freq) const
|
||||
{
|
||||
// there is generally a lot of high-energy, slightly lower frequency noise on yaw, however this
|
||||
@ -868,14 +931,16 @@ float AP_GyroFFT::calculate_weighted_freq_hz(const Vector3f& energy, const Vecto
|
||||
// @Description: FFT Filter Tuning
|
||||
// @Field: TimeUS: microseconds since system startup
|
||||
// @Field: PkAvg: peak noise frequency as an energy-weighted average of roll and pitch peak frequencies
|
||||
// @Field: BwAvg: bandwidth of weighted peak freqency where edges are determined by FFT_ATT_REF
|
||||
// @Field: BwAvg: bandwidth of weighted peak frequency where edges are determined by FFT_ATT_REF
|
||||
// @Field: SnX: signal-to-noise ratio on the roll axis
|
||||
// @Field: SnY: signal-to-noise ratio on the pitch axis
|
||||
// @Field: SnZ: signal-to-noise ratio on the yaw axis
|
||||
// @Field: FtX: harmonic fit on roll of the highest noise peak to the second highest noise peak
|
||||
// @Field: FtY: harmonic fit on pitch of the highest noise peak to the second highest noise peak
|
||||
// @Field: FtZ: harmonic fit on yaw of the highest noise peak to the second highest noise peak
|
||||
// @Field: FH: FFT health
|
||||
// @Field: FHX: FFT health, X-axis
|
||||
// @Field: FHY: FFT health, Y-axis
|
||||
// @Field: FHZ: FFT health, Z-axis
|
||||
// @Field: Tc: FFT cycle time
|
||||
|
||||
// log gyro fft messages
|
||||
@ -887,10 +952,10 @@ void AP_GyroFFT::write_log_messages()
|
||||
|
||||
AP::logger().WriteStreaming(
|
||||
"FTN1",
|
||||
"TimeUS,PkAvg,BwAvg,SnX,SnY,SnZ,FtX,FtY,FtZ,FH,Tc",
|
||||
"szz---%%%-s",
|
||||
"F---------F",
|
||||
"QffffffffBI",
|
||||
"TimeUS,PkAvg,BwAvg,SnX,SnY,SnZ,FtX,FtY,FtZ,FHX,FHY,FHZ,Tc",
|
||||
"szz---%%%---s",
|
||||
"F-----------F",
|
||||
"QffffffffBBBI",
|
||||
AP_HAL::micros64(),
|
||||
get_weighted_noise_center_freq_hz(),
|
||||
get_weighted_noise_center_bandwidth_hz(),
|
||||
@ -900,7 +965,7 @@ void AP_GyroFFT::write_log_messages()
|
||||
get_raw_noise_harmonic_fit().x,
|
||||
get_raw_noise_harmonic_fit().y,
|
||||
get_raw_noise_harmonic_fit().z,
|
||||
_health, _output_cycle_micros);
|
||||
_health.x, _health.y, _health.z, _output_cycle_micros);
|
||||
|
||||
log_noise_peak(0, FrequencyPeak::CENTER);
|
||||
if (_tracked_peaks> 1) {
|
||||
@ -917,7 +982,7 @@ void AP_GyroFFT::write_log_messages()
|
||||
gcs().send_text(MAV_SEVERITY_WARNING, "FFT: f:%.1f, fr:%.1f, b:%u, fd:%.1f",
|
||||
_debug_state._center_freq_hz_filtered[FrequencyPeak::CENTER][_update_axis], _debug_state._center_freq_hz[_update_axis], _debug_max_bin, _debug_max_bin_freq);
|
||||
gcs().send_text(MAV_SEVERITY_WARNING, "FFT: bw:%.1f, e:%.1f, r:%.1f, snr:%.1f",
|
||||
_debug_state._center_bandwidth_hz_filtered[FrequencyPeak::CENTER][_update_axis], _debug_max_freq_bin, _ref_energy[_update_axis][_debug_max_bin], _debug_snr);
|
||||
_debug_state._center_bandwidth_hz_filtered[FrequencyPeak::CENTER][_update_axis], _debug_max_freq_bin, _ref_energy[_debug_max_bin][_update_axis], _debug_snr);
|
||||
_last_output_ms = now;
|
||||
}
|
||||
#endif
|
||||
@ -930,9 +995,12 @@ void AP_GyroFFT::write_log_messages()
|
||||
// @Field: PkX: noise frequency of the peak on roll
|
||||
// @Field: PkY: noise frequency of the peak on pitch
|
||||
// @Field: PkZ: noise frequency of the peak on yaw
|
||||
// @Field: BwX: bandwidth of the peak freqency on roll where edges are determined by FFT_ATT_REF
|
||||
// @Field: BwY: bandwidth of the peak freqency on pitch where edges are determined by FFT_ATT_REF
|
||||
// @Field: BwZ: bandwidth of the peak freqency on yaw where edges are determined by FFT_ATT_REF
|
||||
// @Field: BwX: bandwidth of the peak frequency on roll where edges are determined by FFT_ATT_REF
|
||||
// @Field: BwY: bandwidth of the peak frequency on pitch where edges are determined by FFT_ATT_REF
|
||||
// @Field: BwZ: bandwidth of the peak frequency on yaw where edges are determined by FFT_ATT_REF
|
||||
// @Field: SnX: signal-to-noise ratio on the roll axis
|
||||
// @Field: SnY: signal-to-noise ratio on the pitch axis
|
||||
// @Field: SnZ: signal-to-noise ratio on the yaw axis
|
||||
// @Field: EnX: power spectral density bin energy of the peak on roll
|
||||
// @Field: EnY: power spectral density bin energy of the peak on roll
|
||||
// @Field: EnZ: power spectral density bin energy of the peak on roll
|
||||
@ -940,7 +1008,7 @@ void AP_GyroFFT::write_log_messages()
|
||||
// write a single log message
|
||||
void AP_GyroFFT::log_noise_peak(uint8_t id, FrequencyPeak peak) const
|
||||
{
|
||||
AP::logger().WriteStreaming("FTN2", "TimeUS,Id,PkX,PkY,PkZ,BwX,BwY,BwZ,EnX,EnY,EnZ", "s#zzzzzz---", "F----------", "QBfffffffff",
|
||||
AP::logger().WriteStreaming("FTN2", "TimeUS,Id,PkX,PkY,PkZ,BwX,BwY,BwZ,SnX,SnY,SnZ,EnX,EnY,EnZ", "s#zzzzzz------", "F-------------", "QBffffffffffff",
|
||||
AP_HAL::micros64(),
|
||||
id,
|
||||
get_noise_center_freq_hz(peak).x,
|
||||
@ -949,6 +1017,9 @@ void AP_GyroFFT::log_noise_peak(uint8_t id, FrequencyPeak peak) const
|
||||
get_noise_center_bandwidth_hz(peak).x,
|
||||
get_noise_center_bandwidth_hz(peak).y,
|
||||
get_noise_center_bandwidth_hz(peak).z,
|
||||
get_noise_signal_to_noise_db(peak).x,
|
||||
get_noise_signal_to_noise_db(peak).y,
|
||||
get_noise_signal_to_noise_db(peak).z,
|
||||
get_center_freq_energy(peak).x,
|
||||
get_center_freq_energy(peak).y,
|
||||
get_center_freq_energy(peak).z);
|
||||
@ -973,23 +1044,22 @@ void AP_GyroFFT::calculate_noise(bool calibrating, const EngineConfig& config)
|
||||
{
|
||||
// calculate the SNR and center frequency energy
|
||||
float weighted_center_freq_hz = 0.0f;
|
||||
float snr = 0.0f;
|
||||
|
||||
uint8_t num_peaks = calculate_tracking_peaks(weighted_center_freq_hz, snr, calibrating, config);
|
||||
uint8_t num_peaks = calculate_tracking_peaks(weighted_center_freq_hz, calibrating, config);
|
||||
|
||||
_thread_state._center_freq_bin[_update_axis] = _state->_peak_data[FrequencyPeak::CENTER]._bin;
|
||||
_thread_state._center_freq_bin[_update_axis] = _state->_peak_data[_thread_state._center_peak[_update_axis]]._bin;
|
||||
_thread_state._center_freq_hz[_update_axis] = weighted_center_freq_hz;
|
||||
_thread_state._center_snr[_update_axis] = snr;
|
||||
// record the last time we had a good signal on this axis
|
||||
if (num_peaks > 0) {
|
||||
_thread_state._health_ms[_update_axis] = AP_HAL::millis();
|
||||
} else {
|
||||
_thread_state._health_ms[_update_axis] = 0;
|
||||
}
|
||||
_thread_state._health = num_peaks;
|
||||
_thread_state._health[_update_axis] = num_peaks;
|
||||
FrequencyPeak tracked_peak = FrequencyPeak::CENTER;
|
||||
|
||||
// record the tracked peak for harmonic fit, but only if we have more than one noise peak
|
||||
// this checks filtered energies and so can allow energies to be closer together
|
||||
if (num_peaks > 1 && _tracked_peaks > 1 && !is_zero(get_tl_noise_center_freq_hz(FrequencyPeak::CENTER, _update_axis))) {
|
||||
if (get_tl_noise_center_freq_hz(FrequencyPeak::CENTER, _update_axis) > get_tl_noise_center_freq_hz(FrequencyPeak::LOWER_SHOULDER, _update_axis)
|
||||
// ignore the fit if there is too big a discrepancy between the energies
|
||||
@ -1007,6 +1077,7 @@ void AP_GyroFFT::calculate_noise(bool calibrating, const EngineConfig& config)
|
||||
// if targetting more than one harmonic then make sure we get the fundamental
|
||||
// on larger copters the second harmonic often has more energy
|
||||
// if the highest peak is above the second highest then check for harmonic fit
|
||||
// comparisons are made using filter, normalised data
|
||||
if (_thread_state._tracked_peak[_update_axis] != FrequencyPeak::CENTER) {
|
||||
// calculate the fit and filter at 10hz
|
||||
const float harmonic_fit = 100.0f * fabsf(get_tl_noise_center_freq_hz(FrequencyPeak::CENTER, _update_axis)
|
||||
@ -1032,7 +1103,7 @@ void AP_GyroFFT::calculate_noise(bool calibrating, const EngineConfig& config)
|
||||
|
||||
|
||||
// calculate noise peaks based on the frequencies closest to the recent historical average, switching peaks around as necessary
|
||||
uint8_t AP_GyroFFT::calculate_tracking_peaks(float& weighted_center_freq_hz, float& snr, bool calibrating, const EngineConfig& config)
|
||||
uint8_t AP_GyroFFT::calculate_tracking_peaks(float& weighted_center_freq_hz, bool calibrating, const EngineConfig& config)
|
||||
{
|
||||
uint8_t num_peaks = 0;
|
||||
FrequencyData freqs(*this, config);
|
||||
@ -1048,14 +1119,9 @@ uint8_t AP_GyroFFT::calculate_tracking_peaks(float& weighted_center_freq_hz, flo
|
||||
FrequencyPeak lower = find_closest_peak(FrequencyPeak::LOWER_SHOULDER, distance_matrix, 1 << center);
|
||||
FrequencyPeak upper = find_closest_peak(FrequencyPeak::UPPER_SHOULDER, distance_matrix, 1 << center | 1 << lower);
|
||||
|
||||
// if we have had the maximum number of swapped cycles, force a full calculation
|
||||
if (calibrating || _distorted_cycles[_update_axis] == 0) {
|
||||
calculate_filtered_noise(FrequencyPeak::LOWER_SHOULDER, FrequencyPeak::LOWER_SHOULDER, freqs, config) && num_peaks++;
|
||||
calculate_filtered_noise(FrequencyPeak::UPPER_SHOULDER, FrequencyPeak::UPPER_SHOULDER, freqs, config) && num_peaks++;
|
||||
calculate_filtered_noise(FrequencyPeak::CENTER, FrequencyPeak::CENTER, freqs, config) && num_peaks++;
|
||||
_distorted_cycles[_update_axis] = constrain_int16(_distorted_cycles[_update_axis] + 1, 0, FFT_MAX_MISSED_UPDATES);
|
||||
_thread_state._tracked_peak[_update_axis] = FrequencyPeak::CENTER;
|
||||
weighted_center_freq_hz = freqs.get_weighted_frequency(FrequencyPeak::CENTER);
|
||||
snr = freqs.get_signal_to_noise(FrequencyPeak::CENTER);
|
||||
num_peaks = calculate_tracking_peaks(weighted_center_freq_hz, freqs, config);
|
||||
#if DEBUG_FFT
|
||||
printf("Skipped update, order would have been is %d/%.1f(%.1f) %d/%.1f(%.1f) %d/%.1f(%.1f) n = %d\n",
|
||||
center, _state->_peak_data[center]._freq_hz, get_tl_noise_center_freq_hz(FrequencyPeak::CENTER, _update_axis),
|
||||
@ -1067,29 +1133,57 @@ uint8_t AP_GyroFFT::calculate_tracking_peaks(float& weighted_center_freq_hz, flo
|
||||
|
||||
// another peak is closer to what is currently considered the center frequency
|
||||
if (center != FrequencyPeak::CENTER || lower != FrequencyPeak::LOWER_SHOULDER || upper != FrequencyPeak::UPPER_SHOULDER) {
|
||||
if (lower != FrequencyPeak::NONE) {
|
||||
calculate_filtered_noise(FrequencyPeak::LOWER_SHOULDER, lower, freqs, config) && num_peaks++;
|
||||
if (lower != FrequencyPeak::NONE && calculate_filtered_noise(FrequencyPeak::LOWER_SHOULDER, lower, freqs, config)) {
|
||||
num_peaks++;
|
||||
} else {
|
||||
lower = FrequencyPeak::NONE;
|
||||
}
|
||||
if (upper != FrequencyPeak::NONE) {
|
||||
calculate_filtered_noise(FrequencyPeak::UPPER_SHOULDER, upper, freqs, config) && num_peaks++;
|
||||
if (upper != FrequencyPeak::NONE && calculate_filtered_noise(FrequencyPeak::UPPER_SHOULDER, upper, freqs, config)) {
|
||||
num_peaks++;
|
||||
} else {
|
||||
upper = FrequencyPeak::NONE;
|
||||
}
|
||||
if (center != FrequencyPeak::NONE) {
|
||||
calculate_filtered_noise(FrequencyPeak::CENTER, center, freqs, config) && num_peaks++;
|
||||
if (center != FrequencyPeak::NONE && calculate_filtered_noise(FrequencyPeak::CENTER, center, freqs, config)) {
|
||||
num_peaks++;
|
||||
} else {
|
||||
center = FrequencyPeak::NONE;
|
||||
}
|
||||
weighted_center_freq_hz = freqs.get_weighted_frequency(center);
|
||||
snr = freqs.get_signal_to_noise(center);
|
||||
_thread_state._tracked_peak[_update_axis] = center;
|
||||
_thread_state._center_peak[_update_axis] = center;
|
||||
update_snr_values(freqs);
|
||||
// if two adjacent peaks have simply swapped, we will allow this to continue indefinitely
|
||||
// as there is no loss of fidelity
|
||||
if (!((center == FrequencyPeak::LOWER_SHOULDER && lower == FrequencyPeak::CENTER)
|
||||
|| (center == FrequencyPeak::UPPER_SHOULDER && upper == FrequencyPeak::CENTER))) {
|
||||
_distorted_cycles[_update_axis]--;
|
||||
}
|
||||
return num_peaks;
|
||||
}
|
||||
|
||||
calculate_filtered_noise(FrequencyPeak::LOWER_SHOULDER, FrequencyPeak::LOWER_SHOULDER, freqs, config) && num_peaks++;
|
||||
calculate_filtered_noise(FrequencyPeak::UPPER_SHOULDER, FrequencyPeak::UPPER_SHOULDER, freqs, config) && num_peaks++;
|
||||
calculate_filtered_noise(FrequencyPeak::CENTER, FrequencyPeak::CENTER, freqs, config) && num_peaks++;
|
||||
num_peaks = calculate_tracking_peaks(weighted_center_freq_hz, freqs, config);
|
||||
|
||||
return num_peaks;
|
||||
}
|
||||
|
||||
// calculate the noise and whether valid for each peak
|
||||
uint8_t AP_GyroFFT::calculate_tracking_peaks(float& weighted_center_freq_hz, const FrequencyData& freqs, const EngineConfig& config)
|
||||
{
|
||||
uint8_t num_peaks = 0;
|
||||
if (calculate_filtered_noise(FrequencyPeak::LOWER_SHOULDER, FrequencyPeak::LOWER_SHOULDER, freqs, config)) {
|
||||
num_peaks++;
|
||||
}
|
||||
if (calculate_filtered_noise(FrequencyPeak::UPPER_SHOULDER, FrequencyPeak::UPPER_SHOULDER, freqs, config)) {
|
||||
num_peaks++;
|
||||
}
|
||||
if (calculate_filtered_noise(FrequencyPeak::CENTER, FrequencyPeak::CENTER, freqs, config)) {
|
||||
num_peaks++;
|
||||
}
|
||||
// record the number of cycles where something was tracked
|
||||
_distorted_cycles[_update_axis] = constrain_int16(_distorted_cycles[_update_axis] + 1, 0, FFT_MAX_MISSED_UPDATES);
|
||||
weighted_center_freq_hz = freqs.get_weighted_frequency(FrequencyPeak::CENTER);
|
||||
snr = freqs.get_signal_to_noise(FrequencyPeak::CENTER);
|
||||
_thread_state._tracked_peak[_update_axis] = FrequencyPeak::CENTER;
|
||||
_thread_state._center_peak[_update_axis] = FrequencyPeak::CENTER;
|
||||
|
||||
update_snr_values(freqs);
|
||||
|
||||
return num_peaks;
|
||||
}
|
||||
@ -1136,6 +1230,14 @@ bool AP_GyroFFT::calculate_filtered_noise(FrequencyPeak target_peak, FrequencyPe
|
||||
return false;
|
||||
}
|
||||
|
||||
void AP_GyroFFT::update_snr_values(const FrequencyData& freqs)
|
||||
{
|
||||
_thread_state._center_freq_snr[FrequencyPeak::CENTER][_update_axis] = freqs.get_signal_to_noise(FrequencyPeak::CENTER);
|
||||
_thread_state._center_freq_snr[FrequencyPeak::LOWER_SHOULDER][_update_axis] = freqs.get_signal_to_noise(FrequencyPeak::LOWER_SHOULDER);
|
||||
_thread_state._center_freq_snr[FrequencyPeak::UPPER_SHOULDER][_update_axis] = freqs.get_signal_to_noise(FrequencyPeak::UPPER_SHOULDER);
|
||||
}
|
||||
|
||||
|
||||
// filter values through a median sliding window followed by low pass filter
|
||||
// this eliminates temporary spikes in the detected frequency that are either pure noise
|
||||
// or a different peak that will erroneously bias the peak we are tracking
|
||||
@ -1166,7 +1268,7 @@ bool AP_GyroFFT::get_weighted_frequency(FrequencyPeak peak, float& weighted_peak
|
||||
|
||||
// calculate the SNR and center frequency energy
|
||||
const float max_energy = MAX(1.0f, _state->get_freq_bin(bin));
|
||||
const float ref_energy = MAX(1.0f, _ref_energy[_update_axis][bin]);
|
||||
const float ref_energy = MAX(1.0f, _ref_energy[bin][_update_axis]);
|
||||
snr = 10.f * (log10f(max_energy) - log10f(ref_energy));
|
||||
|
||||
// if the bin energy is above the noise threshold then we have a signal
|
||||
@ -1226,13 +1328,13 @@ void AP_GyroFFT::update_ref_energy(uint16_t max_bin)
|
||||
// determine a PS noise reference at each of the possible center frequencies
|
||||
if (_noise_cycles == 0 && _noise_calibration_cycles[_update_axis] > 0) {
|
||||
for (uint16_t i = 1; i < _state->_bin_count; i++) {
|
||||
_ref_energy[_update_axis][i] += _state->get_freq_bin(i);
|
||||
_ref_energy[i][_update_axis] += _state->get_freq_bin(i);
|
||||
}
|
||||
if (--_noise_calibration_cycles[_update_axis] == 0) {
|
||||
for (uint16_t i = 1; i < _state->_bin_count; i++) {
|
||||
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);
|
||||
_ref_energy[i][_update_axis] = (_ref_energy[i][_update_axis] / cycles) * sqrtf(cycles);
|
||||
}
|
||||
|
||||
WITH_SEMAPHORE(_sem);
|
||||
|
||||
@ -33,9 +33,16 @@
|
||||
// a library that leverages the HAL DSP support to perform FFT analysis on gyro samples
|
||||
class AP_GyroFFT
|
||||
{
|
||||
friend class ReplayGyroFFT;
|
||||
|
||||
public:
|
||||
typedef AP_HAL::DSP::FrequencyPeak FrequencyPeak;
|
||||
|
||||
enum class Options : uint32_t {
|
||||
FFTPostFilter = 1 << 0,
|
||||
ESCNoiseCheck = 1 << 1
|
||||
};
|
||||
|
||||
AP_GyroFFT();
|
||||
|
||||
// Do not allow copies
|
||||
@ -75,11 +82,11 @@ public:
|
||||
// detected peak frequency filtered at 1/3 the update rate
|
||||
const Vector3f& get_noise_center_freq_hz() const { return get_noise_center_freq_hz(FrequencyPeak::CENTER); }
|
||||
const Vector3f& get_noise_center_freq_hz(FrequencyPeak peak) const { return _global_state._center_freq_hz_filtered[peak]; }
|
||||
// slew frequency values
|
||||
float get_slewed_weighted_freq_hz(FrequencyPeak peak) const;
|
||||
float get_slewed_noise_center_freq_hz(FrequencyPeak peak, uint8_t axis) const;
|
||||
// frequency values
|
||||
float get_weighted_freq_hz(FrequencyPeak peak) const;
|
||||
// energy of the background noise at the detected center frequency
|
||||
const Vector3f& get_noise_signal_to_noise_db() const { return _global_state._center_snr; }
|
||||
const Vector3f& get_noise_signal_to_noise_db() const { return get_noise_signal_to_noise_db(FrequencyPeak::CENTER); }
|
||||
const Vector3f& get_noise_signal_to_noise_db(FrequencyPeak peak) const { return _global_state._center_freq_snr[peak];; }
|
||||
// detected peak frequency weighted by energy
|
||||
float get_weighted_noise_center_freq_hz() const;
|
||||
// all detected peak frequencies weighted by energy
|
||||
@ -100,6 +107,12 @@ public:
|
||||
float get_weighted_noise_center_bandwidth_hz() const;
|
||||
// log gyro fft messages
|
||||
void write_log_messages();
|
||||
// post filter mask of IMUs
|
||||
bool using_post_filter_samples() const { return (_options & uint32_t(Options::FFTPostFilter)) != 0; }
|
||||
// post filter mask of IMUs
|
||||
bool check_esc_noise() const { return (_options & uint32_t(Options::ESCNoiseCheck)) != 0; }
|
||||
// look for a frequency in the detected noise
|
||||
float has_noise_at_frequency_hz(float freq) const;
|
||||
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
static AP_GyroFFT *get_singleton() { return _singleton; }
|
||||
@ -164,7 +177,6 @@ private:
|
||||
float get_tl_noise_center_bandwidth_hz(FrequencyPeak peak, uint8_t axis) const { return _thread_state._center_bandwidth_hz_filtered[peak][axis]; };
|
||||
// thread-local mutators of filtered state
|
||||
float update_tl_noise_center_freq_hz(FrequencyPeak peak, uint8_t axis, float value) {
|
||||
_thread_state._prev_center_freq_hz_filtered[peak][axis] = _thread_state._center_freq_hz_filtered[peak][axis];
|
||||
return (_thread_state._center_freq_hz_filtered[peak][axis] = _center_freq_filter[peak].apply(axis, value));
|
||||
}
|
||||
float update_tl_center_freq_energy(FrequencyPeak peak, uint8_t axis, float value) {
|
||||
@ -178,9 +190,11 @@ private:
|
||||
// calculate the peak noise frequency
|
||||
void calculate_noise(bool calibrating, const EngineConfig& config);
|
||||
// calculate noise peaks based on energy and history
|
||||
uint8_t calculate_tracking_peaks(float& weighted_peak_freq_hz, float& snr, bool calibrating, const EngineConfig& config);
|
||||
uint8_t calculate_tracking_peaks(float& weighted_peak_freq_hz, bool calibrating, const EngineConfig& config);
|
||||
uint8_t calculate_tracking_peaks(float& weighted_center_freq_hz, const FrequencyData& freqs, const EngineConfig& config);
|
||||
// calculate noise peak frequency characteristics
|
||||
bool calculate_filtered_noise(FrequencyPeak target_peak, FrequencyPeak source_peak, const FrequencyData& freqs, const EngineConfig& config);
|
||||
void update_snr_values(const FrequencyData& freqs);
|
||||
// get the weighted frequency
|
||||
bool get_weighted_frequency(FrequencyPeak peak, float& weighted_peak_freq_hz, float& snr, const EngineConfig& config) const;
|
||||
// return the tracked noise peak
|
||||
@ -220,18 +234,18 @@ private:
|
||||
// bin of detected peak frequency
|
||||
Vector3ui _center_freq_bin;
|
||||
// fft engine health
|
||||
uint8_t _health;
|
||||
Vector3<uint8_t> _health;
|
||||
Vector3ul _health_ms;
|
||||
// fft engine output rate
|
||||
uint32_t _output_cycle_ms;
|
||||
// tracked frequency peak
|
||||
// tracked frequency peak for the purposes of notching
|
||||
Vector3<uint8_t> _tracked_peak;
|
||||
// signal to noise ratio of PSD at the detected centre frequency
|
||||
Vector3f _center_snr;
|
||||
// center frequency peak ignoring temporary energy changes / order switching
|
||||
Vector3<uint8_t> _center_peak;
|
||||
// signal to noise ratio of PSD at each of the detected centre frequencies
|
||||
Vector3f _center_freq_snr[FrequencyPeak::MAX_TRACKED_PEAKS];
|
||||
// filtered version of the peak frequency
|
||||
Vector3f _center_freq_hz_filtered[FrequencyPeak::MAX_TRACKED_PEAKS];
|
||||
// previous filtered version of the peak frequency
|
||||
Vector3f _prev_center_freq_hz_filtered[FrequencyPeak::MAX_TRACKED_PEAKS];
|
||||
// when we last calculated a value
|
||||
Vector3ul _last_output_us;
|
||||
// filtered energy of the detected peak frequency
|
||||
@ -278,8 +292,8 @@ private:
|
||||
float _harmonic_multiplier;
|
||||
// number of tracked peaks
|
||||
uint8_t _tracked_peaks;
|
||||
// engine health in tracked peaks
|
||||
uint8_t _health;
|
||||
// engine health in tracked peaks per axis
|
||||
Vector3<uint8_t> _health;
|
||||
// engine health on roll/pitch/yaw
|
||||
Vector3<uint8_t> _rpy_health;
|
||||
// averaged throttle output over averaging period
|
||||
@ -298,7 +312,7 @@ private:
|
||||
uint16_t _fft_sampling_rate_hz;
|
||||
// number of cycles without a detected signal
|
||||
uint8_t _missed_cycles[XYZ_AXIS_COUNT][FrequencyPeak::MAX_TRACKED_PEAKS];
|
||||
// number of cycles without a detected signal
|
||||
// number of cycles where peaks have swapped places
|
||||
uint8_t _distorted_cycles[XYZ_AXIS_COUNT];
|
||||
// whether the analyzer initialized correctly
|
||||
bool _initialized;
|
||||
@ -337,6 +351,8 @@ private:
|
||||
AP_Int8 _harmonic_peak;
|
||||
// number of output frames to retain for averaging
|
||||
AP_Int8 _num_frames;
|
||||
// mask of IMUs to record gyro frames after the filter bank
|
||||
AP_Int32 _options;
|
||||
AP_InertialSensor* _ins;
|
||||
#if DEBUG_FFT
|
||||
uint32_t _last_output_ms;
|
||||
|
||||
177
libraries/AP_GyroFFT/examples/ReplayGyroFFT/ReplayGyroFFT.cpp
Normal file
177
libraries/AP_GyroFFT/examples/ReplayGyroFFT/ReplayGyroFFT.cpp
Normal file
@ -0,0 +1,177 @@
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_HAL_Empty/AP_HAL_Empty.h>
|
||||
#include <AP_Arming/AP_Arming.h>
|
||||
#include <GCS_MAVLink/GCS_Dummy.h>
|
||||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
#include <AP_SerialManager/AP_SerialManager.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
#include <AP_GyroFFT/AP_GyroFFT.h>
|
||||
#include <AP_InertialSensor/AP_InertialSensor.h>
|
||||
#include <AP_Baro/AP_Baro.h>
|
||||
#include <AP_ExternalAHRS/AP_ExternalAHRS.h>
|
||||
#include <AP_Scheduler/AP_Scheduler.h>
|
||||
#include <AP_Arming/AP_Arming.h>
|
||||
#include <SITL/SITL.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
const AP_HAL::HAL &hal = AP_HAL::get_HAL();
|
||||
|
||||
static const uint32_t LOOP_RATE_HZ = 400;
|
||||
static const uint32_t LOOP_DELTA_US = 1000000 / LOOP_RATE_HZ;
|
||||
static const uint32_t RUN_TIME = 120; // 2 mins
|
||||
static const uint32_t LOOP_ITERATIONS = LOOP_RATE_HZ * RUN_TIME;
|
||||
|
||||
void setup();
|
||||
void loop();
|
||||
|
||||
static AP_SerialManager serial_manager;
|
||||
static AP_BoardConfig board_config;
|
||||
static AP_InertialSensor ins;
|
||||
static AP_Baro baro;
|
||||
AP_Int32 logger_bitmask;
|
||||
static AP_Logger logger{logger_bitmask};
|
||||
#if HAL_EXTERNAL_AHRS_ENABLED
|
||||
static AP_ExternalAHRS external_ahrs;
|
||||
#endif
|
||||
static SITL::SIM sitl;
|
||||
static AP_Scheduler scheduler;
|
||||
|
||||
// create fake gcs object
|
||||
GCS_Dummy _gcs;
|
||||
|
||||
const struct LogStructure log_structure[] = {
|
||||
LOG_COMMON_STRUCTURES
|
||||
};
|
||||
|
||||
const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
class Arming : public AP_Arming {
|
||||
public:
|
||||
Arming() : AP_Arming() {}
|
||||
bool arm(AP_Arming::Method method, bool do_arming_checks=true) override {
|
||||
armed = true;
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
static Arming arming;
|
||||
|
||||
class ReplayGyroFFT {
|
||||
public:
|
||||
void init() {
|
||||
fft._enable.set(1); // FFT_ENABLE
|
||||
fft._window_size.set(64); // FFT_WINDOW_SIZE
|
||||
fft._snr_threshold_db.set(10); // FFT_SNR_REF
|
||||
fft._fft_min_hz.set(50); // FFT_MINHZ
|
||||
fft._fft_max_hz.set(450); // FFT_MAXHZ
|
||||
|
||||
fft.init(LOOP_RATE_HZ);
|
||||
fft.update_parameters();
|
||||
}
|
||||
|
||||
void loop() {
|
||||
fft.sample_gyros();
|
||||
fft.update();
|
||||
// calibrate the FFT
|
||||
uint32_t now = AP_HAL::millis();
|
||||
if (!arming.is_armed()) {
|
||||
char buf[32];
|
||||
if (!fft.pre_arm_check(buf, 32)) {
|
||||
if (now - last_output_ms > 1000) {
|
||||
hal.console->printf("%s\n", buf);
|
||||
last_output_ms = now;
|
||||
}
|
||||
} else {
|
||||
logger.PrepForArming();
|
||||
arming.arm(AP_Arming::Method::RUDDER);
|
||||
logger.set_vehicle_armed(true);
|
||||
// apply throttle values to motors to make sure the fake IMU generates energetic enough data
|
||||
for (uint8_t i=0; i<4; i++) {
|
||||
hal.rcout->write(i, 1500);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
if (now - last_output_ms > 1000) {
|
||||
hal.console->printf(".");
|
||||
last_output_ms = now;
|
||||
}
|
||||
}
|
||||
fft.write_log_messages();
|
||||
}
|
||||
AP_GyroFFT fft;
|
||||
uint32_t last_output_ms;
|
||||
};
|
||||
|
||||
static ReplayGyroFFT replay;
|
||||
|
||||
void setup()
|
||||
{
|
||||
hal.console->printf("ReplayGyroFFT\n");
|
||||
board_config.init();
|
||||
serial_manager.init();
|
||||
|
||||
const bool generate = false;
|
||||
if (generate) {
|
||||
sitl.vibe_freq.set(Vector3f(250,250,250)); // SIM_VIB_FREQ
|
||||
sitl.drift_speed.set(0); // SIM_DRIFT_SPEED
|
||||
sitl.drift_time.set(0); // SIM_DRIFT_TIME
|
||||
sitl.gyro_noise[0].set(20); // SIM_GYR1_RND
|
||||
} else {
|
||||
sitl.speedup.set(100); // SIM_SPEEDUP
|
||||
sitl.gyro_file_rw.set(SITL::SIM::INSFileMode::INS_FILE_READ_STOP_ON_EOF); // SIM_GYR_FILE_RW
|
||||
}
|
||||
logger_bitmask.set(128); // IMU
|
||||
logger.Init(log_structure, ARRAY_SIZE(log_structure));
|
||||
ins.init(LOOP_RATE_HZ);
|
||||
baro.init();
|
||||
|
||||
replay.init();
|
||||
}
|
||||
|
||||
static uint32_t loop_iter = LOOP_ITERATIONS;
|
||||
|
||||
void loop()
|
||||
{
|
||||
if (!hal.console->is_initialized()) {
|
||||
return;
|
||||
}
|
||||
|
||||
ins.wait_for_sample();
|
||||
uint32_t sample_time_us = AP_HAL::micros();
|
||||
|
||||
ins.update();
|
||||
ins.periodic();
|
||||
logger.periodic_tasks();
|
||||
ins.Write_IMU();
|
||||
replay.loop();
|
||||
|
||||
uint32_t elapsed = AP_HAL::micros() - sample_time_us;
|
||||
if (elapsed < LOOP_DELTA_US) {
|
||||
hal.scheduler->delay_microseconds(LOOP_DELTA_US - elapsed);
|
||||
}
|
||||
|
||||
if (sitl.gyro_file_rw != SITL::SIM::INSFileMode::INS_FILE_READ_STOP_ON_EOF && loop_iter-- == 0) {
|
||||
hal.console->printf("\n");
|
||||
exit(0);
|
||||
}
|
||||
}
|
||||
|
||||
AP_HAL_MAIN();
|
||||
|
||||
#else
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
||||
|
||||
static void loop() { }
|
||||
static void setup()
|
||||
{
|
||||
printf("Board not currently supported\n");
|
||||
}
|
||||
|
||||
AP_HAL_MAIN();
|
||||
|
||||
#endif
|
||||
7
libraries/AP_GyroFFT/examples/ReplayGyroFFT/wscript
Normal file
7
libraries/AP_GyroFFT/examples/ReplayGyroFFT/wscript
Normal file
@ -0,0 +1,7 @@
|
||||
#!/usr/bin/env python
|
||||
# encoding: utf-8
|
||||
|
||||
def build(bld):
|
||||
bld.ap_example(
|
||||
use='ap',
|
||||
)
|
||||
Loading…
Reference in New Issue
Block a user