OmniX-Space/xiaozhi-esp32
1
0
Fork 0
Code Issues Pull Requests Actions Releases Activity

add some code

Browse Source
This commit is contained in:
苏雨洛
2025-09-05 13:25:11 +08:00
parent 9ff0a99e7a
commit 3cf1229a85
8911 changed files with 2535396 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

120
managed_components/espressif__esp-dsp/modules/support/cplx_gen/dsps_cplx_gen.S Normal file
View File

@@ -0,0 +1,120 @@
/*
* SPDX-FileCopyrightText: 2023 Espressif Systems (Shanghai) CO LTD
*
* SPDX-License-Identifier: Apache-2.0
*/
#include "dsps_cplx_gen_platform.h"
#if (dsps_cplx_gen_aes3_enbled || dsps_cplx_gen_ae32_enbled)
// This is a Complex signal generator for ESP32 processor.
.text
.align 4
.global dsps_cplx_gen_ae32
.type dsps_cplx_gen_ae32,@function
// The function implements the following C code:
// esp_err_t dsps_cplx_gen_ae32(cplx_sig_t *cplx_gen, void *output, int32_t len);
dsps_cplx_gen_ae32:
// Input params Variables float Variables fixed
//
// cplx_gen - a2 fr - f0 lut - a5
// output - a3 one_const - f1 lut_len - a6
// len - a4 lut_len_f - f2 sin_pos - a7
// ph_f - f3 cos_pos - a8
// sin_pos_f - f4 sin_to_cos - a9
// ph_floor - a10
// modulo - a11
entry a1, 32
l32i a5, a2, 0 // a5 - lut
l32i a6, a2, 4 // a6 - lut_len
lsi f0, a2, 8 // f0 - fr
lsi f3, a2, 12 // f3 - ph_f (phase increment)
const.s f1, 1 // f1 - constant 1
float.s f2, a6, 0 // f2 - lut_len_f
srli a9, a6, 2 // a9 - sin_to_cos = lut_len / 4
addi a11, a6, -1 // a11 - modulo = lut_len - 1
l32i a15, a2, 16 // a15 - d_type
beqz a15, _s16_fixed
// F32 floating point
loopnez a4, ._main_loop_float
floor.s a10, f3, 0 // turncate wiht rounding towards -infinity
// branch if ph_floor is greater than 0
bgez a10, _ph_check_low_float
add.s f3, f3, f1 // f3 = f3 - f1 (ph_f + 1)
floor.s a10, f3, 0 // turncate wiht rounding towards -infinity
_ph_check_low_float:
// branch if ph_ceil is lower than 2 (floored to 1)
blti a10, 1, _ph_check_great_float
sub.s f3, f3, f1 // f3 = f3 - f1 (ph_f - 1)
_ph_check_great_float:
mul.s f4, f3, f2 // sin_pos_f = ph_f * lut_len
trunc.s a7, f4, 0 // truncate sin_pos_f to sin_pos
add a8, a7, a9 // cos_pos (a8) = sin_pos(a7) + sin_to_cos(a9)
and a8, a8, a11 // cos_pos = cos_pos & modulo (lut_len - 1)
slli a8, a8, 2 // set index of the LUT (4 x cos_pos)
slli a7, a7, 2 // set index of the LUT (4 x sin_pos)
lsx f14, a5, a7 // load sin LUT value form *lut
lsx f15, a5, a8 // load cos LUT value form *lut
ssi f15, a3, 0 // save cos LUT value to the output, offset 0
ssi f14, a3, 4 // save sin LUT value to the output, offset 4
add.s f3, f3, f0 // ph_f += fr
addi.n a3, a3, 8 // increase the output pointer (2 x f32)
._main_loop_float:
movi.n a2, 0
retw.n
// Q15 fixed point
_s16_fixed:
loopnez a4, ._main_loop_fixed
floor.s a10, f3, 0 // turncate wiht rounding towards -infinity
// branch if ph_floor is greater than 0
bgez a10, _ph_check_low_fixed
add.s f3, f3, f1 // f3 = f3 - f1 (ph_f + 1)
floor.s a10, f3, 0 // turncate wiht rounding towards -infinity
_ph_check_low_fixed:
// branch if ph_ceil is lower than 2 (floored to 1)
blti a10, 1, _ph_check_great_fixed
sub.s f3, f3, f1 // f3 = f3 - f1 (ph_f - 1)
_ph_check_great_fixed:
mul.s f4, f3, f2 // sin_pos_f = ph_f * lut_len
trunc.s a7, f4, 0 // truncate sin_pos_f to sin_pos
add a8, a7, a9 // cos_pos (a8) = sin_pos(a7) + sin_to_cos(a9)
and a8, a8, a11 // cos_pos = cos_pos & modulo (lut_len - 1)
addx2 a15, a8, a5 // get cos index of the LUT (*lut + 2 x cos_pos)
addx2 a13, a7, a5 // get sin index of the LUT (*lut + 2 x sin_pos)
l16si a14, a15, 0 // load cos LUT value from *lut
l16si a12, a13, 0 // load sin LUT value from *lut
s16i a14, a3, 0 // save cos LUT value to the output (a3), offset 0
s16i a12, a3, 2 // save sin LUT value to the output (a3), offset 2
add.s f3, f3, f0 // ph_f += fr
addi.n a3, a3, 4 // increase the output pointer (2 x s16)
._main_loop_fixed:
movi.n a2, 0
retw.n
#endif // (dsps_cplx_gen_aes3_enbled || dsps_cplx_gen_ae32_enbled)

40
managed_components/espressif__esp-dsp/modules/support/cplx_gen/dsps_cplx_gen.c Normal file
View File

@@ -0,0 +1,40 @@
/*
* SPDX-FileCopyrightText: 2023 Espressif Systems (Shanghai) CO LTD
*
* SPDX-License-Identifier: Apache-2.0
*/
#include "dsps_cplx_gen.h"
esp_err_t dsps_cplx_gen_ansi(cplx_sig_t *cplx_gen, void *output, int32_t len)
{
// angle frequency is already cplx_gen->freq
const int sin_to_cos = cplx_gen->lut_len / 4;
float ph = cplx_gen->phase;
const float fr = cplx_gen->freq;
int sin_pos, cos_pos;
for (int i = 0 ; i < len; i++) {
if (ph < 0) {
ph += 1.0;
}
if (ph >= 1.0) {
ph -= 1.0;
}
sin_pos = (int)(ph * (cplx_gen->lut_len));
cos_pos = (sin_pos + sin_to_cos) & (cplx_gen->lut_len - 1);
if (cplx_gen->d_type == S16_FIXED) {
((int16_t *)output)[i * 2 + 0] = ((int16_t *)cplx_gen->lut)[cos_pos];
((int16_t *)output)[i * 2 + 1] = ((int16_t *)cplx_gen->lut)[sin_pos];
} else {
((float *)output)[i * 2 + 0] = ((float *)cplx_gen->lut)[cos_pos];
((float *)output)[i * 2 + 1] = ((float *)cplx_gen->lut)[sin_pos];
}
ph += fr;
}
return ESP_OK;
}

148
managed_components/espressif__esp-dsp/modules/support/cplx_gen/dsps_cplx_gen_init.c Normal file
View File

@@ -0,0 +1,148 @@
/*
* SPDX-FileCopyrightText: 2023 Espressif Systems (Shanghai) CO LTD
*
* SPDX-License-Identifier: Apache-2.0
*/
#include "dsps_cplx_gen.h"
#include "dsp_common.h"
#include "esp_log.h"
#include <math.h>
#include <malloc.h>
#define Q15_MAX INT16_MAX
static const char *TAG = "dsps_cplx_gen";
esp_err_t dsps_cplx_gen_init(cplx_sig_t *cplx_gen, out_d_type d_type, void *lut, int32_t lut_len, float freq, float initial_phase)
{
cplx_gen->lut_len = lut_len;
cplx_gen->freq = freq;
cplx_gen->lut = lut;
cplx_gen->free_status = 0;
cplx_gen->d_type = d_type;
cplx_gen->phase = initial_phase;
// length of the LUT must be power of 2
if (!dsp_is_power_of_two(lut_len)) {
ESP_LOGE(TAG, "The length of the LUT must be power of 2");
return ESP_ERR_DSP_INVALID_LENGTH;
}
// LUT length must be in a range from 256 to 8192
if ((lut == NULL) && ((cplx_gen->lut_len > 8192) || (cplx_gen->lut_len < 256))) {
ESP_LOGE(TAG, "The length of the LUT table out of range. Valid range is 256 to 8192");
return ESP_ERR_DSP_PARAM_OUTOFRANGE;
}
// frequency is a Nyquist frequency, must be in a range from (-1 to 1)
if ((cplx_gen->freq >= 1) || (cplx_gen->freq <= -1)) {
ESP_LOGE(TAG, "The frequency is out of range. Valid range is +/- 1. ");
return ESP_ERR_DSP_INVALID_PARAM;
}
// initial phase in a range from (-1 to 1)
if ((cplx_gen->phase >= 1) || (cplx_gen->phase <= -1)) {
ESP_LOGE(TAG, "The phase is out of range. Valid range is +/- 1. ");
return ESP_ERR_DSP_INVALID_PARAM;
}
// LUT table coefficients generation
if (lut == NULL) { // lut has not been provided by an user. Allocate and initialize it
cplx_gen->free_status |= 0x0001; // lut has been allocated, free_status indicates that the space must be freed afterwards
if (cplx_gen->d_type == S16_FIXED) { // Q15 fixed point
int16_t *local_lut = (int16_t *)malloc(cplx_gen->lut_len * sizeof(int16_t));
float term;
for (int i = 0 ; i < cplx_gen->lut_len; i++) {
term = (2.0 * M_PI) * ((float)(i) / (float)(cplx_gen->lut_len));
local_lut[i] = (int16_t)(sin(term) * Q15_MAX); // conversion to Q15 fixed point
}
cplx_gen->lut = (void *)local_lut;
} else if (cplx_gen->d_type == F32_FLOAT) { // Single precision floating point
float *local_lut = (float *)malloc(cplx_gen->lut_len * sizeof(float));
float term;
for (int i = 0 ; i < cplx_gen->lut_len; i++) {
term = (2.0 * M_PI) * ((float)(i) / (float)(cplx_gen->lut_len));
local_lut[i] = (float)sin(term);
}
cplx_gen->lut = (void *)local_lut;
} else {
cplx_gen->lut = NULL;
return ESP_ERR_DSP_INVALID_PARAM;
}
}
return ESP_OK;
}
esp_err_t dsps_cplx_gen_freq_set(cplx_sig_t *cplx_gen, float freq)
{
if ((freq >= 1) || (freq <= -1)) { // frequency is a Nyquist frequency, must be in a range from (-1 to 1)
ESP_LOGE(TAG, "The frequency is out of range. Valid range is +/- 1. ");
return ESP_ERR_DSP_INVALID_PARAM;
}
cplx_gen->freq = freq;
return ESP_OK;
}
float dsps_cplx_gen_freq_get(cplx_sig_t *cplx_gen)
{
// Check if the structure was initialized
if (!dsp_is_power_of_two(cplx_gen->lut_len)) {
ESP_LOGE(TAG, "cplx_gen strucure was not initialized");
return -2;
}
return (cplx_gen->freq);
}
esp_err_t dsps_cplx_gen_phase_set(cplx_sig_t *cplx_gen, float phase)
{
if ((phase >= 1) || (phase <= -1)) { // initial phase in a range from (-1 to 1)
ESP_LOGE(TAG, "The phase is out of range. Valid range is +/- 1. ");
return ESP_ERR_DSP_INVALID_PARAM;
}
cplx_gen->phase = phase;
return ESP_OK;
}
float dsps_cplx_gen_phase_get(cplx_sig_t *cplx_gen)
{
// Check if the structure was initialized
if (!dsp_is_power_of_two(cplx_gen->lut_len)) {
ESP_LOGE(TAG, "cplx_gen strucure was not initialized");
return -2;
}
return (cplx_gen->phase);
}
esp_err_t dsps_cplx_gen_set(cplx_sig_t *cplx_gen, float freq, float phase)
{
if ((freq >= 1) || (freq <= -1)) { // frequency is a Nyquist frequency, must be in a range from (-1 to 1)
ESP_LOGE(TAG, "The frequency is out of range. Valid range is +/- 1. ");
return ESP_ERR_DSP_INVALID_PARAM;
}
if ((phase >= 1) || (phase <= -1)) { // phase in a range from (-1 to 1)
ESP_LOGE(TAG, "The phase is out of range. Valid range is +/- 1. ");
return ESP_ERR_DSP_INVALID_PARAM;
}
cplx_gen->phase = phase;
cplx_gen->freq = freq;
return ESP_OK;
}
void cplx_gen_free(cplx_sig_t *cplx_gen)
{
if (cplx_gen->free_status & 0x0001) {
free(cplx_gen->lut);
cplx_gen->free_status = 0;
}
}

260
managed_components/espressif__esp-dsp/modules/support/cplx_gen/test/test_cplx_gen.c Normal file
View File

@@ -0,0 +1,260 @@
/*
* SPDX-FileCopyrightText: 2023 Espressif Systems (Shanghai) CO LTD
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <malloc.h>
#include <stdint.h>
#include "unity.h"
#include "dsp_platform.h"
#include "esp_log.h"
#include "esp_dsp.h"
#include <math.h>
#include "dsp_tests.h"
#include "dsps_cplx_gen.h"
#include "dsps_wind.h"
#include "dsps_view.h"
#include "dsps_fft2r.h"
#define LEAKAGE_BINS 10 // fft leakage bins
static const char *TAG = "dsps_cplx_gen";
// Error message handler function, which detects errors returned by dsps_cplx_gen_init() function
void error_msg_handler(cplx_sig_t *cplx_signal, esp_err_t status)
{
if (status != ESP_OK) {
cplx_gen_free(cplx_signal);
switch (status) {
case ESP_ERR_DSP_INVALID_LENGTH:
TEST_ASSERT_MESSAGE(false, "LUT table has invalid length, must be power of 2");
break;
case ESP_ERR_DSP_PARAM_OUTOFRANGE:
TEST_ASSERT_MESSAGE(false, "LUT table length must be in a range from 256 to 8192");
break;
case ESP_ERR_DSP_INVALID_PARAM:
TEST_ASSERT_MESSAGE(false, "Frequency and initial phase must be in a range from -1 to 1");
break;
default:
TEST_ASSERT_MESSAGE(false, "Unspecified error");
break;
}
}
}
TEST_CASE("cplx_gen_functionality_test", "[dsps]")
{
const int32_t out_len = 4096;
const int32_t lut_len = 1024;
const float frequency = 0.001;
const float init_phase = 0.1;
cplx_sig_t cplx_signal, cplx_signal_compare;
// F32 float
esp_err_t status1 = dsps_cplx_gen_init(&cplx_signal, F32_FLOAT, NULL, lut_len, frequency, init_phase);
error_msg_handler(&cplx_signal, status1);
esp_err_t status2 = dsps_cplx_gen_init(&cplx_signal_compare, F32_FLOAT, cplx_signal.lut, lut_len, frequency, init_phase);
error_msg_handler(&cplx_signal_compare, status2);
float *out_array_float = (float *)malloc(out_len * 2 * sizeof(float)); // times 2 for real and complex part
float *out_array_compare_float = (float *)malloc(out_len * 2 * sizeof(float));
dsps_cplx_gen_ansi(&cplx_signal_compare, (void *)out_array_compare_float, out_len);
dsps_cplx_gen(&cplx_signal, (void *)out_array_float, out_len);
for (int i = 0; i < out_len * 2; i++) {
TEST_ASSERT_EQUAL(out_array_compare_float[i], out_array_float[i]);
}
free(out_array_float);
free(out_array_compare_float);
cplx_gen_free(&cplx_signal);
cplx_gen_free(&cplx_signal_compare);
// S16 fixed
status1 = dsps_cplx_gen_init(&cplx_signal, S16_FIXED, NULL, lut_len, frequency, init_phase);
error_msg_handler(&cplx_signal, status1);
status2 = dsps_cplx_gen_init(&cplx_signal_compare, S16_FIXED, cplx_signal.lut, lut_len, frequency, init_phase);
error_msg_handler(&cplx_signal_compare, status2);
int16_t *out_array_fixed = (int16_t *)malloc(out_len * 2 * sizeof(int16_t)); // times 2 for real and complex part
int16_t *out_array_compare_fixed = (int16_t *)malloc(out_len * 2 * sizeof(int16_t));
dsps_cplx_gen_ansi(&cplx_signal_compare, (void *)out_array_compare_fixed, out_len);
dsps_cplx_gen(&cplx_signal, (void *)out_array_fixed, out_len);
for (int i = 0; i < out_len * 2; i++) {
TEST_ASSERT_EQUAL(out_array_compare_fixed[i], out_array_fixed[i]);
}
free(out_array_fixed);
free(out_array_compare_fixed);
cplx_gen_free(&cplx_signal);
cplx_gen_free(&cplx_signal_compare);
}
TEST_CASE("cplx_gen_benchmark_test", "[dsps]")
{
int32_t out_len = 32;
const int32_t lut_len = 256;
const float frequency = 0.02;
const float init_phase = 0.9;
const int repeat_count = 4;
cplx_sig_t cplx_signal_float, cplx_signal_fixed;
esp_err_t status1 = dsps_cplx_gen_init(&cplx_signal_float, F32_FLOAT, NULL, lut_len, frequency, init_phase);
error_msg_handler(&cplx_signal_float, status1);
esp_err_t status2 = dsps_cplx_gen_init(&cplx_signal_fixed, S16_FIXED, NULL, lut_len, frequency, init_phase);
error_msg_handler(&cplx_signal_fixed, status2);
float *out_array_float = (float *)malloc(out_len * 2 * 32 * sizeof(float)); // 8192 (max_out len) * 2 (real and imaginary)
int16_t *out_array_fixed = (int16_t *)malloc(out_len * 2 * 32 * sizeof(int16_t));
for (int i = 0; i < 6; i++) {
const unsigned int start_float = dsp_get_cpu_cycle_count();
for (int j = 0 ; j < repeat_count ; j++) {
dsps_cplx_gen(&cplx_signal_float, (void *)out_array_float, out_len);
}
const unsigned int end_float = dsp_get_cpu_cycle_count();
const unsigned int start_fixed = dsp_get_cpu_cycle_count();
for (int j = 0 ; j < repeat_count ; j++) {
dsps_cplx_gen(&cplx_signal_fixed, (void *)out_array_fixed, out_len);
}
const unsigned int end_fixed = dsp_get_cpu_cycle_count();
const float total_float = end_float - start_float;
const float total_fixed = end_fixed - start_fixed;
const float cycles_float = total_float / (float)(repeat_count);
const float cycles_fixed = total_fixed / (float)(repeat_count);
const float cycles_per_lut_sample_float = total_float / (float)(out_len * repeat_count);
const float cycles_per_lut_sample_fixed = total_fixed / (float)(out_len * repeat_count);
ESP_LOGI(TAG, "Float : %.2f total cycles, %.2f cycles per sample, for %"PRId32" LUT samples, %"PRId32" output array length",
cycles_float, cycles_per_lut_sample_float, lut_len, out_len);
ESP_LOGI(TAG, "Fixed : %.2f total cycles, %.2f cycles per sample, for %"PRId32" LUT samples, %"PRId32" output array length \n",
cycles_fixed, cycles_per_lut_sample_fixed, lut_len, out_len);
out_len *= 2;
}
free(out_array_fixed);
free(out_array_float);
cplx_gen_free(&cplx_signal_float);
cplx_gen_free(&cplx_signal_fixed);
}
TEST_CASE("cplx_gen_noise_SNR_test", "[dsps]")
{
const int32_t out_len = 2048;
const int32_t lut_len = 8192;
const int32_t n_fft = out_len * 2; // * 2 (real and imaginary)
const float frequency = 0.01;
const float init_phase = 0.0;
const float real_ampl = 0.5;
const float imag_ampl = 0.2;
cplx_sig_t cplx_signal_float;
esp_err_t status = dsps_cplx_gen_init(&cplx_signal_float, F32_FLOAT, NULL, lut_len, frequency, init_phase);
error_msg_handler(&cplx_signal_float, status);
float *out_array_float = (float *)memalign(16, n_fft * sizeof(float));
dsps_cplx_gen(&cplx_signal_float, (void *)out_array_float, out_len);
// Signal windowing
float *window = (float *)memalign(16, out_len * sizeof(float));
dsps_wind_blackman_harris_f32(window, out_len);
for (int i = 0 ; i < out_len ; i++) {
out_array_float[i * 2 + 0] *= (window[i] * real_ampl);
out_array_float[i * 2 + 1] *= (window[i] * imag_ampl);
}
free(window);
// Initialize FFT
esp_err_t ret = dsps_fft2r_init_fc32(NULL, n_fft);
if (ret != ESP_OK) {
ESP_LOGE(TAG, "Not possible to initialize FFT. Error = %i", ret);
return;
}
// Do the FFT
dsps_fft2r_fc32(out_array_float, out_len);
dsps_bit_rev_fc32(out_array_float, out_len);
dsps_cplx2reC_fc32(out_array_float, out_len);
// Convert the FFT spectrum from amplitude to watts, find the max value and its position
float max_val_1 = -1000000, max_val_2 = -1000000;
int max_pos_1 = 0, max_pos_2 = 0, spur_pos_1 = 0, spur_pos_2 = 0;
for (int i = 0 ; i < n_fft / 2 ; i++) {
out_array_float[i] = (out_array_float[i * 2 + 0] * out_array_float[i * 2 + 0] + out_array_float[i * 2 + 1] * out_array_float[i * 2 + 1]) / (n_fft * 3);
if (i < n_fft / 4) {
if (out_array_float[i] > max_val_1) {
max_val_1 = out_array_float[i];
max_pos_1 = i;
}
} else {
if (out_array_float[i] > max_val_2) {
max_val_2 = out_array_float[i];
max_pos_2 = i;
}
}
}
// Calculate the power of the signal and noise of the spectrum and convert the spectrum to dB
float signal_pow_1 = 0, signal_pow_2 = 0, noise_pow_1 = 0, noise_pow_2 = 0;
float spur_1 = -1000000, spur_2 = -1000000;
for (int i = 0 ; i < n_fft / 2 ; i++) {
if (i < n_fft / 4) {
if ((i >= max_pos_1 - LEAKAGE_BINS) && (i <= max_pos_1 + LEAKAGE_BINS)) {
signal_pow_1 += out_array_float[i];
} else {
noise_pow_1 += out_array_float[i];
if (out_array_float[i] > spur_1) {
spur_1 = out_array_float[i];
spur_pos_1 = i;
}
}
} else {
if ((i >= max_pos_2 - LEAKAGE_BINS) && (i <= max_pos_2 + LEAKAGE_BINS)) {
signal_pow_2 += out_array_float[i];
} else {
noise_pow_2 += out_array_float[i];
if (out_array_float[i] > spur_2) {
spur_2 = out_array_float[i];
spur_pos_2 = i;
}
}
}
out_array_float[i] = 10 * log10f(0.0000000000001 + out_array_float[i]);
}
// Convert the signal power and noise power from watts to dB and calculate SNR and SFDR
const float snr_1 = 10 * log10f(signal_pow_1 / noise_pow_1);
const float snr_2 = 10 * log10f(signal_pow_2 / noise_pow_2);
noise_pow_1 = 10 * log10f(noise_pow_1);
noise_pow_2 = 10 * log10f(noise_pow_2);
signal_pow_1 = 10 * log10f(signal_pow_1);
signal_pow_2 = 10 * log10f(signal_pow_2);
const float sfdr_1 = out_array_float[max_pos_1] - out_array_float[spur_pos_1];
const float sfdr_2 = out_array_float[max_pos_2] - out_array_float[spur_pos_2];
ESP_LOGI(TAG, "\nSignal Power: \t%f\nNoise Power: \t%f\nSNR: \t\t%f \nSFDR: \t\t%f", signal_pow_1, noise_pow_1, snr_1, sfdr_1);
dsps_view(out_array_float, n_fft / 4, 64, 16, -140, 40, '|');
putchar('\n');
ESP_LOGI(TAG, "\nSignal Power: \t%f\nNoise Power: \t%f\nSNR: \t\t%f \nSFDR: \t\t%f", signal_pow_2, noise_pow_2, snr_2, sfdr_2);
dsps_view(out_array_float + (n_fft / 4), n_fft / 4, 64, 16, -140, 40, '|');
free(out_array_float);
cplx_gen_free(&cplx_signal_float);
}