Skip to content

Commit

Permalink
AP_GyroFFT: correct compilation when HAL_GCS_ENABLED is false
Browse files Browse the repository at this point in the history
  • Loading branch information
@peterbarker
peterbarker committed Mar 8, 2024
1 parent 4b97dba commit aa79967
Showing 1 changed file with 19 additions and 19 deletions.
38 changes: 19 additions & 19 deletions libraries/AP_GyroFFT/AP_GyroFFT.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -236,10 +236,10 @@ void AP_GyroFFT::init(uint16_t loop_rate_hz)
// INS: XYZ_AXIS_COUNT * INS_MAX_INSTANCES * _window_size, DSP: 3 * _window_size, FFT: XYZ_AXIS_COUNT + 3 * _window_size
const uint32_t allocation_count = (XYZ_AXIS_COUNT * INS_MAX_INSTANCES + 3 + XYZ_AXIS_COUNT + 3 + _num_frames) * sizeof(float);
if (allocation_count * FFT_DEFAULT_WINDOW_SIZE > hal.util->available_memory() / 2) {
gcs().send_text(MAV_SEVERITY_WARNING, "AP_GyroFFT: disabled, required %u bytes", (unsigned int)allocation_count * FFT_DEFAULT_WINDOW_SIZE);
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AP_GyroFFT: disabled, required %u bytes", (unsigned int)allocation_count * FFT_DEFAULT_WINDOW_SIZE);
return;
} else if (allocation_count * _window_size > hal.util->available_memory() / 2) {
gcs().send_text(MAV_SEVERITY_WARNING, "AP_GyroFFT: req alloc %u bytes (free=%u)", (unsigned int)allocation_count * _window_size, (unsigned int)hal.util->available_memory());
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AP_GyroFFT: req alloc %u bytes (free=%u)", (unsigned int)allocation_count * _window_size, (unsigned int)hal.util->available_memory());
_window_size.set(FFT_DEFAULT_WINDOW_SIZE);
}
// save any changes that were made
Expand All @@ -252,7 +252,7 @@ void AP_GyroFFT::init(uint16_t loop_rate_hz)
_fft_sampling_rate_hz = loop_rate_hz / _sample_mode;
for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
if (!_downsampled_gyro_data[axis].set_size(_window_size + _samples_per_frame)) {
gcs().send_text(MAV_SEVERITY_WARNING, "Failed to allocate window for AP_GyroFFT");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Failed to allocate window for AP_GyroFFT");
return;
}
}
Expand All @@ -261,7 +261,7 @@ void AP_GyroFFT::init(uint16_t loop_rate_hz)

_ref_energy = new Vector3f[_window_size];
if (_ref_energy == nullptr) {
gcs().send_text(MAV_SEVERITY_WARNING, "Failed to allocate window for AP_GyroFFT");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Failed to allocate window for AP_GyroFFT");
return;
}

Expand Down Expand Up @@ -312,7 +312,7 @@ void AP_GyroFFT::init(uint16_t loop_rate_hz)
// initialise the HAL DSP subsystem
_state = hal.dsp->fft_init(_window_size, _fft_sampling_rate_hz, _num_frames);
if (_state == nullptr) {
gcs().send_text(MAV_SEVERITY_WARNING, "Failed to initialize DSP engine");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Failed to initialize DSP engine");
return;
}

Expand Down Expand Up @@ -619,7 +619,7 @@ bool AP_GyroFFT::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len)

// check for sane frequency resolution - for 1k backends with length 32 this will be 32Hz
if (_state->_bin_resolution > 50.0f) {
gcs().send_text(MAV_SEVERITY_WARNING, "FFT: resolution is %.1fHz, increase length", _state->_bin_resolution);
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "FFT: resolution is %.1fHz, increase length", _state->_bin_resolution);
return true; // a low resolution is not fatal
}
#if 0 // these calculations do not result in a long enough expected delay
Expand All @@ -643,7 +643,7 @@ bool AP_GyroFFT::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len)

if (_calibrated) {
// provide the user with some useful information about what they have configured
gcs().send_text(MAV_SEVERITY_INFO, "FFT: calibrated %.1fKHz/%.1fHz/%.1fHz", _fft_sampling_rate_hz * 0.001f,
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "FFT: calibrated %.1fKHz/%.1fHz/%.1fHz", _fft_sampling_rate_hz * 0.001f,
_state->_bin_resolution * 0.5, 1000.0f * XYZ_AXIS_COUNT / _frame_time_ms);
}

Expand Down Expand Up @@ -699,7 +699,7 @@ void AP_GyroFFT::start_notch_tune()
}

if (!hal.dsp->fft_start_average(_state)) {
gcs().send_text(MAV_SEVERITY_WARNING, "FFT: Unable to start FFT averaging");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "FFT: Unable to start FFT averaging");
}
// throttle averaging for average fft calculation
_avg_throttle_out = 0.0f;
Expand Down Expand Up @@ -754,25 +754,25 @@ void AP_GyroFFT::stop_notch_tune()
uint8_t harmonics;
float harmonic = calculate_notch_frequency(freqs, numpeaks, _harmonic_fit, harmonics);

gcs().send_text(MAV_SEVERITY_INFO, "FFT: Found peaks at %.1f/%.1f/%.1fHz", freqs[0], freqs[1], freqs[2]);
gcs().send_text(MAV_SEVERITY_NOTICE, "FFT: Selected %.1fHz\n", harmonic);
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "FFT: Found peaks at %.1f/%.1f/%.1fHz", freqs[0], freqs[1], freqs[2]);
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "FFT: Selected %.1fHz\n", harmonic);

// if we don't have a throttle value then all bets are off
if (is_zero(_avg_throttle_out) || is_zero(harmonic)) {
gcs().send_text(MAV_SEVERITY_WARNING, "FFT: Unable to calculate notch: need stable hover");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "FFT: Unable to calculate notch: need stable hover");
AP_Notify::events.autotune_failed = true;
return;
}

if (!_ins->setup_throttle_gyro_harmonic_notch(harmonic, (float)_fft_min_hz.get(), _avg_throttle_out, harmonics)) {
gcs().send_text(MAV_SEVERITY_WARNING, "FFT: Unable to set throttle notch with %.1fHz/%.2f",
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "FFT: Unable to set throttle notch with %.1fHz/%.2f",
harmonic, _avg_throttle_out);
AP_Notify::events.autotune_failed = true;
// save results to FFT slots
_throttle_ref.set(_avg_throttle_out);
_freq_hover_hz.set(harmonic);
} else {
gcs().send_text(MAV_SEVERITY_NOTICE, "FFT: Notch frequency %.1fHz and ref %.2f selected", harmonic, _avg_throttle_out);
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "FFT: Notch frequency %.1fHz and ref %.2f selected", harmonic, _avg_throttle_out);
AP_Notify::events.autotune_complete = true;
}
}
Expand Down Expand Up @@ -1004,9 +1004,9 @@ void AP_GyroFFT::write_log_messages()
if ((now - _last_output_ms) > 1000) {
// doing this from the update thread overflows the stack
WITH_SEMAPHORE(_sem);
gcs().send_text(MAV_SEVERITY_WARNING, "FFT: f:%.1f, fr:%.1f, b:%u, fd:%.1f",
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",
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[_debug_max_bin][_update_axis], _debug_snr);
_last_output_ms = now;
}
Expand Down Expand Up @@ -1378,7 +1378,7 @@ void AP_GyroFFT::update_ref_energy(uint16_t max_bin)
float AP_GyroFFT::self_test_bin_frequencies()
{
if (_state->_window_size * sizeof(float) > hal.util->available_memory() / 2) {
gcs().send_text(MAV_SEVERITY_WARNING, "FFT: unable to run self-test, required %u bytes", (unsigned int)(_state->_window_size * sizeof(float)));
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "FFT: unable to run self-test, required %u bytes", (unsigned int)(_state->_window_size * sizeof(float)));
return 0.0f;
}

Expand Down Expand Up @@ -1424,7 +1424,7 @@ float AP_GyroFFT::self_test(float frequency, FloatBuffer& test_window)
uint16_t max_bin = hal.dsp->fft_analyse(_state, _config._fft_start_bin, _config._fft_end_bin, _config._attenuation_cutoff);

if (max_bin == 0) {
gcs().send_text(MAV_SEVERITY_WARNING, "FFT: self-test failed, failed to find frequency %.1f", frequency);
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "FFT: self-test failed, failed to find frequency %.1f", frequency);
}

calculate_noise(true, _config);
Expand All @@ -1433,11 +1433,11 @@ float AP_GyroFFT::self_test(float frequency, FloatBuffer& test_window)
// make sure the selected frequencies are in the right bin
max_divergence = MAX(max_divergence, fabsf(frequency - _thread_state._center_freq_hz[0]));
if (_thread_state._center_freq_hz[0] < (frequency - MAX(_state->_bin_resolution * 0.5f, 1)) || _thread_state._center_freq_hz[0] > (frequency + MAX(_state->_bin_resolution * 0.5f, 1))) {
gcs().send_text(MAV_SEVERITY_WARNING, "FFT: self-test failed: wanted %.1f, had %.1f", frequency, _thread_state._center_freq_hz[0]);
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "FFT: self-test failed: wanted %.1f, had %.1f", frequency, _thread_state._center_freq_hz[0]);
}
#if DEBUG_FFT
else {
gcs().send_text(MAV_SEVERITY_INFO, "FFT: self-test succeeded: wanted %.1f, had %.1f", frequency, _thread_state._center_freq_hz[0]);
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "FFT: self-test succeeded: wanted %.1f, had %.1f", frequency, _thread_state._center_freq_hz[0]);
}
#endif

Expand Down

0 comments on commit aa79967

Please sign in to comment.