|
|
楼主 |
发表于 2025-5-29 07:46:05
|
显示全部楼层
#include "stm32f4xx.h" // Device header
#include "IQ.h"
#include "fir.h"
#include "FM_FIR.h"
#include "usart.h"
//#define NCO_TABLE_SIZE 256
#define BUFFER_SIZE 500
#define PI 3.14159265358979323846f
#define NCO_TABLE_SIZE 256
#define PHASE_UNWRAP_THRESHOLD 3.1415926f // ??????(p)
#define SAMPLE_RATE 500000.0f // ???500kHz
#define MAX_DEVIATION_HZ 50000.0f // ????50kHz
#define DEEMPH_TIME_CONST 75e-6f // ???????75µs
#define OUTPUT_GAIN (SAMPLE_RATE / (2 * PI * MAX_DEVIATION_HZ))
//#define BUFFER_SIZE 256
typedef struct {
float32_t cos_table[NCO_TABLE_SIZE]; // ?????
float32_t sin_table[NCO_TABLE_SIZE]; // ?????
uint32_t phase_accum; // ?????(32?)
uint32_t phase_inc; // ????
} NCO_State;
static NCO_State nco;
void NCO_Init(uint32_t freq_hz, uint32_t fs_hz)
{
double phase_inc = (double)freq_hz * (1ULL << 32) / fs_hz;
nco.phase_inc = (uint32_t)(phase_inc + 0.5);
// ???sin/cos?(?????)
for (int i = 0; i < NCO_TABLE_SIZE; i++) {
float32_t theta = 2 * PI * i / NCO_TABLE_SIZE;
nco.cos_table[i] = arm_cos_f32(theta);
nco.sin_table[i] = arm_sin_f32(theta);
}
nco.phase_accum = 0;
}
void NCO_Generate(float32_t* I_out, float32_t* Q_out, uint32_t len)
{
for (uint32_t i = 0; i < len; i++)
{
uint8_t index = (nco.phase_accum >> 24) & 0xFF;
I_out[i] = nco.cos_table[index]; // I?(??)
Q_out[i] = nco.sin_table[index]; // Q?(??)
nco.phase_accum += nco.phase_inc;
}
}
void AM_Demod_Process(float32_t* input, float32_t* output, uint32_t len)
{
uint16_t i =0;
static float32_t I_buf[BUFFER_SIZE], Q_buf[BUFFER_SIZE];
float32_t mag_sq[BUFFER_SIZE];
static float32_t IMAX_buf[BUFFER_SIZE]={0}, QMAX_buf[BUFFER_SIZE]={0};
float32_t Ifir_buf[BUFFER_SIZE]={0}, Qfir_buf[BUFFER_SIZE]={0};
NCO_Generate(I_buf, Q_buf, len);
arm_mult_f32(input, Q_buf, QMAX_buf, len);
arm_mult_f32(input, I_buf, IMAX_buf, len);
// FIR??(??IIR??)
arm_fir_f32_lp_I(IMAX_buf, Ifir_buf);
arm_fir_f32_lp_Q(QMAX_buf, Qfir_buf);
float32_t sum_sq[BUFFER_SIZE];
arm_mult_f32(Ifir_buf, Ifir_buf, sum_sq, len);
arm_mult_f32(Qfir_buf, Qfir_buf, output, len);
arm_add_f32(sum_sq, output, sum_sq, len);
for (uint32_t i = 0; i < len; i++)
{
arm_sqrt_f32(sum_sq[i], &output[i]); // output = sqrt(I^2 + Q^2)
}
// for(i = 0;i<500;i++ )
// {
// printf("%f,%f,%f\n", sum_sq[i], output[i],input[i]);
// }
}
void FM_Demod_Process(float32_t* input, float32_t* output, uint32_t len)
{
uint16_t i = 0;
static float32_t FM_I_buf[BUFFER_SIZE], FM_Q_buf[BUFFER_SIZE];
static float32_t FM_IMAX_buf[BUFFER_SIZE] = {0}, FM_QMAX_buf[BUFFER_SIZE] = {0};
float32_t FM_Ifir_buf[BUFFER_SIZE] = {0}, FM_Qfir_buf[BUFFER_SIZE] = {0};
static float32_t prev_phase = 0.0f; // ??????
static float32_t prev_unwrapped = 0.0f; // ??????????
float32_t current_phase, phase_diff, unwrapped_phase;
static int32_t phase_jumps = 0; // ??????
static float32_t prev_deemph = 0.0f;
NCO_Generate(FM_I_buf, FM_Q_buf, len);
arm_mult_f32(input, FM_Q_buf, FM_QMAX_buf, len);
arm_mult_f32(input, FM_I_buf, FM_IMAX_buf, len);
// for(i = 0;i<500;i++ )
// {
// printf("%f,%f,%f\n",FM_I_buf[i],FM_Q_buf[i],FM_QMAX_buf[i]/800);
// }
// FIR??(??IIR??)
FM_arm_fir_f32_lp_I(FM_IMAX_buf, FM_Ifir_buf);
FM_arm_fir_f32_lp_Q(FM_QMAX_buf, FM_Qfir_buf);
const float32_t DEEMPH_ALPHA = 0.9746f;
for (i = 0; i < len; i++)
{
current_phase = cordic_atan2(FM_Qfir_buf[i], FM_Ifir_buf[i]); //·´ÕýÇÐ
phase_diff = current_phase - prev_phase;
if (phase_diff > PI) {
phase_jumps--;
phase_diff -= 2 * PI;
} else if (phase_diff < -PI) {
phase_jumps++;
phase_diff += 2 * PI;
}
unwrapped_phase = current_phase + 2*PI*phase_jumps;
output[i] = (unwrapped_phase - prev_unwrapped) * OUTPUT_GAIN;
output[i] = (1 - DEEMPH_ALPHA) * output[i] + DEEMPH_ALPHA * prev_deemph;
prev_deemph = output[i];
prev_phase = current_phase;
prev_unwrapped = unwrapped_phase;
}
for(i = 0;i<500;i++ )
{
printf("%f,%f,%f\n",output[i],input[i]/800,FM_QMAX_buf[i]/800);
}
}
float32_t cordic_atan2(float32_t y, float32_t x) {
float32_t angle = 0.0f;
int8_t sign = 1;
// ??x=0?????
if (x == 0.0f) return (y >= 0) ? PI/2 : -PI/2;
// ??????????
if (x < 0) { x = -x; y = -y; angle = PI; }
if (y < 0) { y = -y; sign = -1; }
// ???????(????9/10)
static const float32_t angles[20] = {
0.7853981634f, // arctan(1) (i=0)
0.4636476090f, // arctan(1/2^1) (i=1)
0.2449786631f, // arctan(1/2^2) (i=2)
0.1243549945f, // arctan(1/2^3) (i=3)
0.0624188097f, // arctan(1/2^4) (i=4)
0.0312398334f, // arctan(1/2^5) (i=5)
0.0156237286f, // arctan(1/2^6) (i=6)
0.0078123411f, // arctan(1/2^7) (i=7)
0.0039062301f, // arctan(1/2^8) (i=8)
0.0019531225f, // arctan(1/2^9) (i=9)
0.0009765622f, // arctan(1/2^10) (i=10)
0.0004882812f, // arctan(1/2^11) (i=11)
0.0002441406f, // arctan(1/2^12) (i=12)
0.0001220703f, // arctan(1/2^13) (i=13)
0.0000610352f, // arctan(1/2^14) (i=14)
0.0000305176f, // arctan(1/2^15) (i=15)
0.0000152588f, // arctan(1/2^16) (i=16)
0.0000076294f, // arctan(1/2^17) (i=17)
0.0000038147f, // arctan(1/2^18) (i=18)
0.0000019073f // arctan(1/2^19) (i=19)
};
for(int i=0; i<20; i++) {
float32_t factor = 1.0f/(1 << i);
if(y > 0) {
x += y * factor;
y -= x * factor;
angle += angles[i];
} else {
x -= y * factor;
y += x * factor;
angle -= angles[i];
}
}
return sign * angle * 0.607252935f;
}我现在解调FM，一样的参数解不出来了，我发现我这个dsp库里
没有反正切 |
|
发表于 2025-5-25 22:57:29
发表于 2025-5-26 08:37:21
