diff --git a/libraries/AP_GyroFFT/AP_GyroFFT.cpp b/libraries/AP_GyroFFT/AP_GyroFFT.cpp index 5a1c0e79666842..cff834a4b4b559 100644 --- a/libraries/AP_GyroFFT/AP_GyroFFT.cpp +++ b/libraries/AP_GyroFFT/AP_GyroFFT.cpp @@ -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 @@ -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; } } @@ -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; } @@ -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; } @@ -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 @@ -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); } @@ -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; @@ -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; } } @@ -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; } @@ -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; } @@ -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); @@ -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