/* * _Beep_dsp_handling.xc * * Created on: 8. jan. 2022 * Author: oyvindteig */ #define INCLUDES #ifdef INCLUDES #include #include // tile #include // delay_milliseconds(200), XS1_TIMER_HZ etc #include // uint8_t #include // printf #include // xScope logging #include // memcpy #include // REFERENCE_PARAM(my_app_ports_t, my_app_ports) -> my_app_ports_t &my_app_ports #include // not etc. #include #include #include #include #include #include #include // log10 etc #include #include "_version.h" // First this.. #include "_globals.h" // ..then this #include "maths.h" #include "mic_array.h" #include "maths_fix.h" // Needs stdint.h #include "mics_in_headset_out.h" #include "_Beep_dsp_params.h" #include "param.h" #include "i2c_client_task.h" #include "port_tile0_task.h" #include "logger.h" #include "_Beep_dsp_handling.h" #define DO_DEBUG_PRINT 0 // 0 has consequences if (USE_XSCOPE == 1) and amy timing at all needs to be met #define DO_DEBUG_PRINT_SPECIAL 1 #define DO_WARNINGS 1 #include "_print_macros_xc.h" #endif typedef struct { bool mul_N; // Handled first bool sqrt; // second bool dB; // third: irregardless of do_sqrt, this may run alone or after do_sqrt } do_calcs_t; bool my_dsp_magnitude ( // return ok: no overflow/underflow or Q24_NaN const int32_t mul_N, // signed because of usage const unsigned iof_spectrum, // not used yet const dsp_complex_t freq_spectrum_in [NUM_FREQ_POINTS_COMPLEX], // ..same size.. const do_calcs_t do_calcs, spectrum_val_t spectrum_val_out [NUM_FREQ_POINTS_REAL], // ..as this int_max_value_t &freq_max_value, error_info_t &error) { error_info_t error_prev; error_info_t error_round; for (unsigned ix=0; ix freq_max_value.val_at_ix) { freq_max_value.val_at_ix = spectrum_val_out[ix]; freq_max_value.ix = ix; freq_max_value.val_new_cnt++; } else { freq_max_value.val_stable_cnt++; } } else { freq_max_value.val_aux_cnt++; } } return not (error.err_cnt != error_prev.err_cnt); } void init_int_max_value (int_max_value_t &freq_max_value) { freq_max_value.val_at_ix = INT_MIN; freq_max_value.ix = 0; // in curve freq_max_value.val_new_cnt = 0; freq_max_value.val_stable_cnt = 0; freq_max_value.val_aux_cnt = 0; } #define NUM_BATCHES_TO_COUNT 50 #define MARGIN_MEAN_BATCH_CNT_MS 0 void init_error (error_info_t &error) { error.err_cnt = 0; error.debug_tag = 0; } typedef enum { state_get_new_data, state_process_spectrum // iof_spectrum 0 or 1 } state_e; // [[combinable]] // Not with ordered select void dsp_task_y ( chanend ch_out_in, // get_next_mic_samples_buffer_t then mic_samples_info_t and mic_samples_info_t chanend ch_logger_in_3out, streaming chanend sch_yb) // knock then data_ch_params_t and data_ch_results_t then { xassert ((sizeof (data_sch_knock_t)) <= MAX_SIZEOF_NONBLOCKING_STREAMING_CHAN); debug_print ("dsp_task_y started, sizeof mic_samples_buff_t: %u\n", (sizeof (mic_samples_buff_t))); #if ((NUM_COMPLEX_PARTS * NUM_FFT_POINTS_COMPLEX) != NUM_MIC_SAMPLES_REAL) #error ALL UNIONS MUST BE OF EQUAL SIZE #elif ((NUM_COMPLEX_PARTS * NUM_FREQ_SPECTRA * NUM_FREQ_POINTS_REAL) != NUM_MIC_SAMPLES_REAL) #error ALL UNIONS MUST BE OF EQUAL SIZE #else // This needs to be a thousand percent correct: // xassert ((sizeof (this struct) == NUM_MIC_SAMPLES_REAL * 4)); xassert (sizeof (mic_samples_buff_t) == (sizeof (int64_t) + (NUM_MIC_SAMPLES_REAL * (sizeof (int32_t))))); #endif #if (DEBUG_MIC_SAMPLES_INCREASING > 0) mic_sample_t debug_sliding_mic_sample = COUNTING_MIC_SAMPLE_INIT; #endif mic_samples_buff_t buffer; mic_samples_info_t mic_samples_info; get_next_mic_samples_buffer_t get_next_mic_samples_buffer = true; data_ch_params_t data_ch_params; bool display_data_sch_knock_ok = DO_DSP_RESULTS_TO_DISPLAY; // if false forever false BEEP-005 remove state_e state = state_get_new_data; bool allow_state_handling = false; // Wait for curve_finished iir_biquad_params_t filter_y_iir_biquad_params; dsp_design_biquad_lowpass ( FILTER_Y_FREQ_MINUS_3DB_NORMALIZED, FILTER_Y_Q_FACTOR, filter_y_iir_biquad_params.coeffs, FILTER_Q_FORMAT); // Initialize filter state data to zeros for (int32_t ix = 0; ix < DSP_NUM_STATES_PER_BIQUAD; ++ix) { filter_y_iir_biquad_params.states[ix] = 0; } data_ch_params.empty = true; unsigned batch_cnt = 0; unsigned was_late_cnt = 0; unsigned iof_spectrum; bool iof_spectrum_do_init = true; timer tmr; time32_t time_ticks; time32_t for_mean_time_ticks; time32_t time_start_dsp_ticks; time32_t time_end_dsp_ticks; unsigned dsp_time_sum_us = 0; char magnitude_str[Q_FORMAT_STR_LEN]; unsigned debug_all_zero_curves_cnt = 0; bool next_curve_ok; int_max_value_t freq_max_value; init_int_max_value (freq_max_value); error_info_t error; init_error (error); tmr :> for_mean_time_ticks; tmr :> time_ticks; while (1) { [[ordered]] select { case ch_logger_in_3out :> next_curve_ok : { // value not used any more if (iof_spectrum_do_init) { iof_spectrum_do_init = false; iof_spectrum = 0; } else if (iof_spectrum == (NUM_FREQ_SPECTRA-1)) { // ==1 state = state_get_new_data; // Last curve shown iof_spectrum = 0; } else { iof_spectrum++; // Essentially from 0 to 1 } allow_state_handling = true; tmr :> time_ticks; // Ready immediately } break; case (allow_state_handling) => tmr when timerafter (time_ticks) :> void : { tmr :> time_ticks; // Ready immediately if (state == state_get_new_data) { // One query, one answer, to avoid deadlock with a "bidirectional channel" // To be received on ch_in_out ch_out_in <: get_next_mic_samples_buffer; // Receive both (was sent on ch_in_out) ch_out_in :> mic_samples_info; // BEEP-005 hangs here ch_out_in :> buffer; xassert (mic_samples_info.num_samples == NUM_MIC_SAMPLES); // 1024 or 2048 signed // More debug prints here in v0618. For that version, tested and OK: #if (DEBUG_MIC_SAMPLES_INCREASING > 0) for (unsigned ix = 0; ix < mic_samples_info.num_samples; ix++) { VERIFY_MIC_SAMPLES_DELTA (30, 1, debug_sliding_mic_sample, buffer.u.mic_samples[ix], mic_samples_info.num_samples); } #endif tmr :> time_start_dsp_ticks; #if (DO_FILTER_Y == 1) for (unsigned ix=0; ix < mic_samples_info.num_samples; ix++) { // Anti-alias 16 kHz to 4 kHz buffer.u.mic_samples[ix] = dsp_filters_biquad ( buffer.u.mic_samples[ix], filter_y_iir_biquad_params.coeffs, filter_y_iir_biquad_params.states, FILTER_Q_FORMAT); } #endif // TODO hanning/hamming window here? // Windowing here, before bit reversing. See lib_mic_array user guide page 17. // buffer is a union: // mic_sample_t mic_samples [NUM_MIC_SAMPLES_REAL]; // 1 * 512, 1024 or 2048 // dsp_complex_t complex_array [NUM_FFT_POINTS_COMPLEX]; // 2 * 256, 512 or 1024 // dsp_complex_t complex_half_spectra [NUM_FREQ_SPECTRA][NUM_FREQ_POINTS_REAL]; // 2 * 2 * 128, 256 or 512 // ==== From dsp_lib ==== dsp_fft_bit_reverse ( buffer.u.complex_array, // 512, 1024 or 2048 signed seen as 512 or 1024 complex NUM_FFT_POINTS_COMPLEX); // 256, 512 or 1024 // ==== From dsp_lib ==== dsp_fft_forward ( buffer.u.complex_array, NUM_FFT_POINTS_COMPLEX, // 256, 512 or 1024 complex FFT_SINE(NUM_FFT_POINTS_COMPLEX)); // dsp_sine_256 is dim ( 256/4) + 1 = 65 // dsp_sine_512 is dim ( 512/4) + 1 = 129 // dsp_sine_1024 is dim (1024/4) + 1 = 257 // NUM_FFT_POINTS_COMPLEX fed into my_dsp_magnitude below // The magnitude of the FFT output is right shifted log2(N) times which corresponds to // division by N as shown in EQUATION 31-5 of http://www.dspguide.com/CH31.PDF. // ==== From dsp_lib ==== dsp_fft_split_spectrum ( buffer.u.complex_array, NUM_FFT_POINTS_COMPLEX); // NEXT STATE state = state_process_spectrum; } else if (state == state_process_spectrum) { allow_state_handling = false; bool ok; do_calcs_t do_calcs; data_ch_logger_t data_ch_logger; // Wrapping around spectrum since I cannot send an array over a chan do_calcs.mul_N = false; do_calcs.sqrt = true; // A ton of scaling problems if this is not set! do_calcs.dB = false; init_error (error); init_int_max_value (freq_max_value); ok = my_dsp_magnitude ( NUM_FFT_POINTS_COMPLEX, // if do_calcs.mul_N true iof_spectrum, buffer.u.complex_half_spectra[iof_spectrum], do_calcs, data_ch_logger.spectrum, freq_max_value, error); if (display_data_sch_knock_ok) { data_sch_knock_t data_sch_knock; data_sch_knock.empty = true; // not used sch_yb <: data_sch_knock; // Will never block, ie. no deadlock // Display data sent in due course is independent of iof_spectrum // BEEP-005 Moved code down here, the select allowed inputs to come in between causing deadlock sch_yb :> data_ch_params; // From menu etc. sch_yb <: freq_max_value; // My data from FFT etc. // display_data_sch_knock_ok = false; } else { // value dropped, should be ok. We'll soon have data } if (freq_max_value.val_at_ix == 0) { debug_all_zero_curves_cnt++; debug_print ("All zero, cnt %u\n", debug_all_zero_curves_cnt); } else { ch_logger_in_3out <: iof_spectrum; ch_logger_in_3out <: data_ch_logger; // My data from FFT etc. ch_logger_in_3out <: freq_max_value; // Max value } #if (DEBUG_DSP_ERROR_LOG==1) if (not ok) { debug_print ("tag1[%X], err:%u\n", error.debug_tag, error.err_cnt); } #endif if (iof_spectrum == (NUM_FREQ_SPECTRA - 1)) { // 1 tmr :> time_end_dsp_ticks; dsp_time_sum_us += ((time_end_dsp_ticks - time_start_dsp_ticks) / XS1_TIMER_MHZ); batch_cnt++; if (mic_samples_info.was_late) { was_late_cnt++; } else {} if ((batch_cnt % NUM_BATCHES_TO_COUNT) == 0) { time32_t for_mean_time_ticks_now; tmr :> for_mean_time_ticks_now; const unsigned mean_batch_time_ms = ((for_mean_time_ticks_now - for_mean_time_ticks)/XS1_TIMER_KHZ) / NUM_BATCHES_TO_COUNT; q24_signed_to_str (magnitude_str, freq_max_value.val_at_ix); #if (DEBUG_DSP_STATUS_LOG == 1) // Halts processor every 6.5 secs debug_print ("%d ms late:%u dsp:%u us max:%s %d(%#08x)\n", mean_batch_time_ms, was_late_cnt, dsp_time_sum_us/NUM_BATCHES_TO_COUNT, magnitude_str, freq_max_value.val_at_ix, freq_max_value.val_at_ix); #endif if ((batch_cnt % (NUM_BATCHES_TO_COUNT*2)) == 0) { // delay_milliseconds (mean_batch_time_ms - MARGIN_MEAN_BATCH_CNT_MS); } else {} was_late_cnt = 0; dsp_time_sum_us = 0; for_mean_time_ticks = for_mean_time_ticks_now; } } else {} } else { xassert (false); // wrong state } // if state } break; // select case } // select } } // dsp_task_x void mic_samples_info_init (mic_samples_info_t &mic_samples_info) { mic_samples_info.num_samples = 0; mic_samples_info.num_lost_overflowed = 0; mic_samples_info.is_full = false; mic_samples_info.was_late = false; } bool mic_samples_info_fill_buffer ( const data_ch_phase3_t phase3_ch_mic_samples, // in mic_samples_buff_t &buffer, mic_samples_info_t &mic_samples_info) // out NUM_MIC_SAMPLES_REAL { bool is_full_pre = mic_samples_info.is_full; const unsigned num_new_samples = phase3_ch_mic_samples.samples.rec_max_msg.nums.num_samples; unsigned num_new_samples_valid; // Add from incoming: mic_samples_info.num_lost_overflowed += phase3_ch_mic_samples.samples.rec_max_msg.nums.num_lost_overflowed; if (mic_samples_info.is_full) { mic_samples_info.num_lost_overflowed += num_new_samples; // Count and count.. num_new_samples_valid = 0; // Don't use any } else if ((mic_samples_info.num_samples + num_new_samples) >= NUM_MIC_SAMPLES_REAL) { // v0618 fix (was >) mic_samples_info.is_full = true; num_new_samples_valid = NUM_MIC_SAMPLES_REAL - mic_samples_info.num_samples; // Not to overflow array } else { num_new_samples_valid = num_new_samples; // Cannot overflow array } #define DO_MEMFILL_TYPE MEMFILL_TYPE_FOR_LOOP #if (DO_MEMFILL_TYPE == MEMFILL_TYPE_FOR_LOOP) for (unsigned ix=0; ix < num_new_samples_valid; ix++) { buffer.u.mic_samples[mic_samples_info.num_samples+ix] = phase3_ch_mic_samples.samples.rec_max_msg.mic_samples.samples[ix]; } #else #error #endif mic_samples_info.num_samples += num_new_samples_valid; // Max is then NUM_MIC_SAMPLES_REAL xassert (mic_samples_info.num_samples <= NUM_MIC_SAMPLES_REAL); return (mic_samples_info.is_full != is_full_pre); } void send_pair ( chanend ch_out, mic_samples_info_t mic_samples_info, mic_samples_buff_t buffer) { // Send: both (to be received on ch_out_in) ch_out <: mic_samples_info; ch_out <: buffer; } // send_pair // Alternative: movable pointer, see 14 Double buffering example of the // XMOS Programming Guide. 2015/9/18 XMOS (XM004440A). void buffer_task_x ( chanend ch_in, // data_ch_phase0_t then data_ch_phase3_t chanend ch_in_out) // mic_samples_info_t then get_next_mic_samples_buffer_t { #define NUM_BUFFERS 2 // Double buffering debug_print ("%s\n", "buffer_task_x started"); data_sch_phase1_t phase1_sch_mic_sample; // data_ch_phase2_t phase2_ch_ack_etc; // No PHASE2 data_ch_phase3_t phase3_ch_mic_samples; mic_samples_buff_t buffer[NUM_BUFFERS]; mic_samples_info_t mic_samples_info[NUM_BUFFERS]; unsigned iof_buffer_to_fill = 0; unsigned iof_buffer_to_send_late = 0; get_next_mic_samples_buffer_t get_next_mic_samples_buffer = false; bool ready_mic_samples_info = false; unsigned debug_one_sample_cnt = 0; unsigned debug_all_samples_cnt = 0; #if (DEBUG_MIC_SAMPLES_INCREASING > 0) mic_sample_t debug_sliding_mic_sample = COUNTING_MIC_SAMPLE_INIT; #endif for (unsigned ix=0; ix get_next_mic_samples_buffer : { // was sent on ch_out_in // One query, one answer (later), to avoid deadlock with a "bidirectional channel" if (ready_mic_samples_info) { // Send now if ready ready_mic_samples_info = false; // Send: both (to be received on ch_out_in) // BEEP-005 gets in here. I had send_pair commented out, why? send_pair (ch_in_out, mic_samples_info[iof_buffer_to_send_late], buffer [iof_buffer_to_send_late]); get_next_mic_samples_buffer = false; mic_samples_info_init (mic_samples_info[iof_buffer_to_send_late]); } else { // No code, send when it's ready } } break; case ch_in :> phase1_sch_mic_sample : { // Match Handle_Beep_BRRRR_task_b bool became_full_now; // =================================================================================== // Observe that there is no PHASE1 -> PHASE2 -> PHASE3 here! // Either there are // NUM_000: N * phase1_sch_mic_sample or // NUM_001 or larger: 1 * phase1_sch_mic_sample then 1 * phase3_ch_mic_samples // =================================================================================== if (phase1_sch_mic_sample.chan_mutual_packet_dim == NUM_000) { // Fill data into first position for phase3_ch_mic_samples buffer // ####### union, so any would go: phase3_ch_mic_samples.samples.rec_001.mic_samples.samples[0] = phase1_sch_mic_sample.iff.valid.mic_sample; phase3_ch_mic_samples.samples.rec_001.nums.num_samples = 1; debug_one_sample_cnt++; VERIFY_MIC_SAMPLES_DELTA (10, 1, debug_sliding_mic_sample, phase1_sch_mic_sample.iff.valid.mic_sample, NUM_000); became_full_now = mic_samples_info_fill_buffer ( phase3_ch_mic_samples, buffer[iof_buffer_to_fill], mic_samples_info[iof_buffer_to_fill]); } else { // >= NUM_001 meaning phase1_sch_mic_sample.iff.valid.is_void = MIC_SAMPLE_VOID; receive_variant_protocol ( ch_in, phase1_sch_mic_sample.chan_mutual_packet_dim, phase3_ch_mic_samples); // For mic_samples_info_fill_buffer const unsigned num_samples = phase3_ch_mic_samples.samples.rec_max_msg.nums.num_samples; const mic_sample_t next_sample = phase3_ch_mic_samples.samples.rec_max_msg.mic_samples.samples[num_samples-1]; VERIFY_MIC_SAMPLES_DELTA (12, num_samples, debug_sliding_mic_sample, next_sample, -1); debug_all_samples_cnt++; became_full_now = mic_samples_info_fill_buffer ( phase3_ch_mic_samples, buffer[iof_buffer_to_fill], mic_samples_info[iof_buffer_to_fill]); } if (became_full_now) { if (get_next_mic_samples_buffer) { mic_samples_info[iof_buffer_to_fill].was_late = false; // Send [iof_buffer_to_fill] since this was the last filled: // both (to be received on ch_out_in) send_pair (ch_in_out, mic_samples_info[iof_buffer_to_fill], buffer [iof_buffer_to_fill]); get_next_mic_samples_buffer = false; // Buffer gone, keep filing into iof_buffer_to_fill } else { // Not asked for yet, set buffer aside ready_mic_samples_info = true; iof_buffer_to_send_late = iof_buffer_to_fill; mic_samples_info[iof_buffer_to_send_late].was_late = true; // For the the next sample we need to change buffer and fill into the other: iof_buffer_to_fill = (iof_buffer_to_fill + 1) % NUM_BUFFERS; } mic_samples_info_init (mic_samples_info[iof_buffer_to_fill]); debug_one_sample_cnt = 0; debug_all_samples_cnt = 0; } else {} } break; } } } // buffer_task_x