ming/Rader_IQ
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update project

Browse Source
This commit is contained in:
Admin
2026-03-02 11:38:36 +08:00
parent 439618888c
commit aa57e8192d
4 changed files with 133 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
src/main.cpp
View File

@@ -6,59 +6,42 @@
#include "radar_vitals.h"
#include "io_flash.h"
static float I_buf[BASE_DATA_LEN];
static float Q_buf[BASE_DATA_LEN];
// 全局变量声明
/**
* @brief 雷达数据采集任务
* 定期读取ADC数据并调用radar_vitals_process()处理数据
* @param parameter 任务参数（未使用）
*/
void radarDataTask(void *parameter) {
Serial.println("📡 雷达数据采集任务启动");
radar_sample_t sample;
radar_vitals_t vitals;
float I_buf[DATA_LEN];
float Q_buf[DATA_LEN];
while(1) {
// 采集DATA_LEN个样本
for(int i=0; i<DATA_LEN; i++) {
// 读取ADC值
sample.i_value = adc1_get_raw(ADC1_CHANNEL_0); // PIN_I (GPIO1) 对应 ADC1_CHANNEL_0
sample.q_value = adc1_get_raw(ADC1_CHANNEL_1); // PIN_Q (GPIO2) 对应 ADC1_CHANNEL_1
for(int i = 0; i < BASE_DATA_LEN; i++) {
sample.i_value = adc1_get_raw(ADC1_CHANNEL_0);
sample.q_value = adc1_get_raw(ADC1_CHANNEL_1);
sample.timestamp = millis();
// 转换为电压并存储
I_buf[i] = (sample.i_value / 4095.0f) * 3.3f;
Q_buf[i] = (sample.q_value / 4095.0f) * 3.3f;
// 延时以达到采样率
vTaskDelay(1000 / SAMPLE_RATE / portTICK_PERIOD_MS);
vTaskDelay(1000 / BASE_SAMPLE_RATE / portTICK_PERIOD_MS);
}
// 计算IQ电压平均值
float avgI = 0, avgQ = 0;
for(int i=0; i<DATA_LEN; i++) {
for(int i = 0; i < BASE_DATA_LEN; i++) {
avgI += I_buf[i];
avgQ += Q_buf[i];
}
avgI /= DATA_LEN;
avgQ /= DATA_LEN;
avgI /= BASE_DATA_LEN;
avgQ /= BASE_DATA_LEN;
// 使用新的处理方法
radar_process(I_buf, Q_buf, DATA_LEN);
radar_process(I_buf, Q_buf, BASE_DATA_LEN);
// 获取人体状态
human_state_t state = radar_get_state();
// 统一打印所有状态信息
if (state == NO_PERSON) {
Serial.printf("👤 No person - IQ电压: I=%.3fV, Q=%.3fV\n", avgI, avgQ);
} else {
// 打印静止状态、IQ电压和心率呼吸率
Serial.printf("👤 Human static - IQ电压: I=%.3fV, Q=%.3fV - 💓 心率: %.1f BPM, 呼吸: %.1f BPM\n", avgI, avgQ, radar_get_heart_bpm(), radar_get_resp_bpm());
}
@@ -86,7 +69,7 @@ void setup() {
xTaskCreatePinnedToCore(
radarDataTask,
"Radar Data Task",
8192,
32768,
NULL,
4,
NULL,

138
src/radar_vitals.cpp
View File

@@ -6,23 +6,43 @@
#define ADC_MAX 4095.0f
#define VREF 3.3f
#define TWO_PI 6.2831853f
#define RADAR_LAMBDA 0.0125f // 24GHz 波长（米）
#define RADAR_LAMBDA 0.0125f
/******** 内部状态 ********/
static float fs = 20.0f;
static float fs = BASE_SAMPLE_RATE;
// 状态变量
static float phase[DATA_LEN];
static float unwrap_phase[DATA_LEN];
static float phase[BASE_DATA_LEN];
static float unwrap_phase[BASE_DATA_LEN];
static human_state_t human_state = NO_PERSON;
static float resp_bpm = 0;
static float heart_bpm = 0;
// 平滑处理相关变量
static float resp_bpm_history[10] = {0};
static float heart_bpm_history[10] = {0};
static float resp_bpm_history[5] = {0};
static float heart_bpm_history[5] = {0};
static int history_index = 0;
// 降采样缓冲区
static float resp_phase[RESP_DATA_LEN];
static float hr_phase[HR_DATA_LEN];
// 简单低通滤波器
static float lowpass_filter(float input, float *state, float alpha) {
*state = alpha * input + (1.0f - alpha) * *state;
return *state;
}
// 降采样函数
void downsample(float *input, float *output, int input_len, int output_len) {
float ratio = (float)input_len / output_len;
for(int i = 0; i < output_len; i++) {
int idx = (int)(i * ratio);
if(idx >= input_len) idx = input_len - 1;
output[i] = input[idx];
}
}
@@ -32,19 +52,21 @@ static int history_index = 0;
/******** 新的雷达处理函数 ********/
void radar_process(float *I, float *Q, int len)
{
if(len > BASE_DATA_LEN) len = BASE_DATA_LEN;
/* 1. 去直流 (DC Removal)
* I'(n) = I(n) - mean(I)
* Q'(n) = Q(n) - mean(Q)
*/
float meanI = 0, meanQ = 0;
for(int i=0;i<len;i++){ meanI += I[i]; meanQ += Q[i]; }
for(int i = 0; i < len; i++) { meanI += I[i]; meanQ += Q[i]; }
meanI /= len; meanQ /= len;
for(int i=0;i<len;i++){ I[i] -= meanI; Q[i] -= meanQ; }
for(int i = 0; i < len; i++) { I[i] -= meanI; Q[i] -= meanQ; }
/* 2. IQ → 相位
* φ(n) = atan2(Q, I)
*/
for(int i=0;i<len;i++){
for(int i = 0; i < len; i++) {
phase[i] = atan2f(Q[i], I[i]);
}
@@ -52,7 +74,7 @@ void radar_process(float *I, float *Q, int len)
* Δφ > π → Δφ -= 2π
*/
unwrap_phase[0] = phase[0];
for(int i=1;i<len;i++){
for(int i = 1; i < len; i++) {
float d = phase[i] - phase[i-1];
if(d > M_PI) d -= 2*M_PI;
if(d < -M_PI) d += 2*M_PI;
@@ -63,15 +85,15 @@ void radar_process(float *I, float *Q, int len)
* Var(φ) = E[(φ-μ)^2]
*/
float mean = 0, var = 0;
for(int i=0;i<len;i++) mean += unwrap_phase[i];
for(int i = 0; i < len; i++) mean += unwrap_phase[i];
mean /= len;
for(int i=0;i<len;i++){
for(int i = 0; i < len; i++) {
float d = unwrap_phase[i] - mean;
var += d * d;
}
var /= len;
if(var < 1e-4){
if(var < 1e-4) {
human_state = NO_PERSON;
return;
}
@@ -80,73 +102,78 @@ void radar_process(float *I, float *Q, int len)
* E = mean[(φ(n)-φ(n-1))^2]
*/
float energy = 0;
for(int i=1;i<len;i++){
for(int i = 1; i < len; i++) {
float d = unwrap_phase[i] - unwrap_phase[i-1];
energy += d * d;
}
energy /= len;
if(energy > 0.05){
if(energy > 0.05) {
human_state = MOTION;
} else {
human_state = STATIC_HUMAN;
}
/* 6. 呼吸 / 心率频率估计
* 方法：频段能量最大值搜索（简化 DFT）
* Fs_eff = SAMPLE_RATE / decimation
*/
const float Fs_eff = 200.0f; // 4kHz / 20
/* 6. 降采样处理 */
downsample(unwrap_phase, resp_phase, len, RESP_DATA_LEN);
downsample(unwrap_phase, hr_phase, len, HR_DATA_LEN);
// ---- 呼吸：0.1 ~ 0.5 Hz ----
float max_resp_mag = 0; float resp_freq = 0;
for(float f=0.1f; f<=0.5f; f+=0.02f){
float re=0, im=0;
for(int n=0;n<len;n++){
float ang = 2*M_PI*f*n/Fs_eff;
re += unwrap_phase[n]*cosf(ang);
im -= unwrap_phase[n]*sinf(ang);
/* 7. 呼吸频率估计 (0.1 ~ 0.5 Hz) */
float max_resp_mag = 0;
float resp_freq = 0;
for(float f = 0.1f; f <= 0.6f; f += 0.002f) {
float re = 0, im = 0;
for(int n = 0; n < RESP_DATA_LEN; n++) {
float ang = 2 * M_PI * f * n / RESP_SAMPLE_RATE;
re += resp_phase[n] * cosf(ang);
im -= resp_phase[n] * sinf(ang);
}
float mag = re * re + im * im;
if(mag > max_resp_mag) {
max_resp_mag = mag;
resp_freq = f;
}
float mag = re*re + im*im;
if(mag > max_resp_mag){ max_resp_mag = mag; resp_freq = f; }
}
float new_resp_bpm = resp_freq * 60.0f;
// ---- 心率：0.8 ~ 2.5 Hz ----
float max_hr_mag = 0; float hr_freq = 0;
for(float f=0.8f; f<=2.5f; f+=0.05f){
float re=0, im=0;
for(int n=0;n<len;n++){
float ang = 2*M_PI*f*n/Fs_eff;
re += unwrap_phase[n]*cosf(ang);
im -= unwrap_phase[n]*sinf(ang);
/* 8. 心率频率估计 (0.8 ~ 2.5 Hz) */
float max_hr_mag = 0;
float hr_freq = 0;
for(float f = 0.6f; f <= 3.0f; f += 0.01f) {
float re = 0, im = 0;
for(int n = 0; n < HR_DATA_LEN; n++) {
float ang = 2 * M_PI * f * n / HR_SAMPLE_RATE;
re += hr_phase[n] * cosf(ang);
im -= hr_phase[n] * sinf(ang);
}
float mag = re * re + im * im;
if(mag > max_hr_mag) {
max_hr_mag = mag;
hr_freq = f;
}
float mag = re*re + im*im;
if(mag > max_hr_mag){ max_hr_mag = mag; hr_freq = f; }
}
float new_heart_bpm = hr_freq * 60.0f;
// 异常值检测和过滤
if(new_heart_bpm < 40 || new_heart_bpm > 180){
new_heart_bpm = heart_bpm; // 使用上次值
if(new_heart_bpm < 40 || new_heart_bpm > 180) {
new_heart_bpm = heart_bpm;
}
if(new_resp_bpm < 4 || new_resp_bpm > 40){
new_resp_bpm = resp_bpm; // 使用上次值
if(new_resp_bpm < 4 || new_resp_bpm > 40) {
new_resp_bpm = resp_bpm;
}
// 移动平均滤波
heart_bpm_history[history_index] = new_heart_bpm;
resp_bpm_history[history_index] = new_resp_bpm;
history_index = (history_index + 1) % 10;
history_index = (history_index + 1) % 5;
// 计算平均值
float sum_hr = 0, sum_resp = 0;
for(int i=0; i<10; i++){
for(int i = 0; i < 5; i++) {
sum_hr += heart_bpm_history[i];
sum_resp += resp_bpm_history[i];
}
heart_bpm = sum_hr / 10.0f;
resp_bpm = sum_resp / 10.0f;
heart_bpm = sum_hr / 5.0f;
resp_bpm = sum_resp / 5.0f;
// 四舍五入到最接近的整数
heart_bpm = roundf(heart_bpm);
@@ -166,13 +193,12 @@ float radar_get_heart_bpm(){ return heart_bpm; }
/******** 初始化 ********/
void radar_vitals_init(float fs_hz)
{
fs = fs_hz;
fs = BASE_SAMPLE_RATE;
human_state = NO_PERSON;
resp_bpm = 0;
heart_bpm = 0;
// 初始化历史数据数组
for(int i=0; i<10; i++){
for(int i = 0; i < 5; i++) {
resp_bpm_history[i] = 0;
heart_bpm_history[i] = 0;
}
@@ -184,17 +210,15 @@ void radar_vitals_init(float fs_hz)
* 初始化雷达相关的ADC和引脚
*/
void radar_init() {
// 配置雷达I/Q引脚为ADC输入
pinMode(PIN_I, INPUT);
pinMode(PIN_Q, INPUT);
// 初始化ADC
adc1_config_width(ADC_WIDTH_BIT_12);
adc1_config_channel_atten(ADC1_CHANNEL_0, ADC_ATTEN_DB_12); // PIN_I (GPIO1) 对应 ADC1_CHANNEL_0
adc1_config_channel_atten(ADC1_CHANNEL_1, ADC_ATTEN_DB_12); // PIN_Q (GPIO2) 对应 ADC1_CHANNEL_1
adc1_config_channel_atten(ADC1_CHANNEL_0, ADC_ATTEN_DB_12);
adc1_config_channel_atten(ADC1_CHANNEL_1, ADC_ATTEN_DB_12);
Serial.println("🏗️ 初始化雷达管理器...");
radar_vitals_init(SAMPLE_RATE);
radar_vitals_init(BASE_SAMPLE_RATE);
}

10
src/radar_vitals.h
View File

@@ -11,8 +11,14 @@
#define SAMPLE_RATE 4000 // 基础采样率(Hz)
#define DATA_LEN 256 // 数据长度
#define BASE_SAMPLE_RATE 200 // 基础采样率(Hz)
#define BASE_DATA_LEN 1024 // 基础数据长度 (5.12秒)
#define RESP_SAMPLE_RATE 10 // 呼吸处理采样率(Hz) - 2倍呼吸频率上限
#define HR_SAMPLE_RATE 50 // 心率处理采样率(Hz) - 2倍心率频率上限
#define RESP_DATA_LEN 51 // 呼吸处理数据长度 (5.1秒)
#define HR_DATA_LEN 256 // 心率处理数据长度 (5.12秒)
// 人体状态枚举
typedef enum {