传感器数据融合¶
在多传感器系统中，部分特定传感器的算法需融合自身采集数据与运动传感器数据以生成有效结果。
1. 运动传感器数据存储¶
运动传感器数据存储在rt_list_t结构中，以便心率、地磁等其他传感器算法获取使用。
1.1 链表节点结构¶
typedef struct {
uint16_t num; // 数据数量
gsensors_data_t buf[GSENSOR_FIFO_SIZE]; // 传感器数据缓冲区
uint64_t time_ns; // 时间戳（纳秒）
rt_list_t list; // RT-Thread 链表节点
} gsensors_fifo_t;
static rt_list_t gsensor_data_store_list; // 全局数据链表头
1.2 链表初始化¶
当定义 USING_GSENSOR_DATA_STORE_LIST 宏时，gsensor_init() 将初始化专用数据链表：
#if defined(USING_GSENSOR_DATA_STORE_LIST)
rt_list_init(&gsensor_data_store_list);
#endif
1.3 数据存入¶
在周期性执行的 gsensor_algo_scheduler 中，完成传感器数据获取后，立即将数据存入链表：
#if defined(USING_GSENSOR_DATA_STORE_LIST)
gsensor_data_store(&gsensor_data);
#endif
1.4 存储与访问接口¶
数据存储函数：将传感器数据存入链表，若链表长度超过5则释放最早数据节点
static void gsensor_data_store(gsensors_fifo_t *fifo)
{
if (rt_list_len(&gsensor_data_store_list) >= 5) {
gsensor_data_free(false);
}
rt_mutex_take(&gsensor_mutex, RT_WAITING_FOREVER);
gsensors_fifo_t *save_data = rt_malloc(sizeof(gsensors_fifo_t));
if (!save_data) return;
memcpy(save_data, fifo, sizeof(gsensors_fifo_t));
rt_list_insert_before(&gsensor_data_store_list, &save_data->list);
rt_mutex_release(&gsensor_mutex);
}
数据获取接口：返回链表头指针供遍历使用
void *gsensor_get_stored_data(void)
{
if (SENSOR_ON != gsensor_is_opened())
return NULL;
return (void *)&gsensor_data_store_list;
}
2. 运动传感器数据访问¶
其他传感器算法可通过调用 gsensor_get_stored_data() 获取运动传感器数据链表指针，遍历链表以获取所需数据进行融合计算。
数据获取函数示例：通过链表头指针获取指定数据
index：buffer 索引
x, y, z：输出数据指针
g_in：链表头指针
static void get_gsensor_data(uint8_t index, float *x, float *y, float *z, void *g_in)
{
#if defined (USING_GSENSOR_DATA_STORE_LIST) && (defined (BF0_LCPU) || defined (SENSOR_IN_HCPU))
rt_list_t *pos, *next;
gsensors_fifo_t *node;
if (qmc6310_dev->gs_input)
{
rt_list_t *gsensor_data_header = (rt_list_t *)g_in;
if (!rt_list_len(gsensor_data_header))
return;
rt_list_for_each_safe(pos, next, gsensor_data_header)
{
node = rt_list_entry(pos, gsensors_fifo_t, list);
if (index > node ->num)
index = node ->num - 1;
*x = node -> buf[index].acce_data[0];
*y = node -> buf[index].acce_data[1];
*z = node -> buf[index].acce_data[2];
break;
}
}
#endif
}
3. 心率传感器数据融合¶
光学心率传感器（PPG）通过检测血液对光的吸收变化来测量心率，但是在运动状态下，PPG信号容易受到运动伪影的干扰，导致测量结果不准确。为了提高心率测量的准确性，通常会结合运动传感器的数据进行融合处理，从而减小运动伪影的影响。
3.1 获取数据¶
在hr_algo中，开启 USING_GSENSOR_DATA_STORE_LIST 与 SENSOR_USING_6D 宏后，在周期性执行的 hr_algo_scheduler 中，将原始数据与运动传感器数据链表一起传递给算法处理函数：
#if defined(USING_GSENSOR_DATA_STORE_LIST) && defined(SENSOR_USING_6D)
if (gsensor_is_opened())
gsensor_input = gsensor_get_stored_data(); // 获取加速度链表
#endif
// 调用算法处理函数，传入心率传感器原始数据与加速度数据链表
sensor_info.hr_algo_ops(hr_type, (void *)&hr_algo_raw_data, gsensor_input);
#if defined(USING_GSENSOR_DATA_STORE_LIST) && defined(SENSOR_USING_6D)
// 算法处理完成后释放已使用的数据节点
if (gsensor_is_opened())
gsensor_data_free(true);
#endif
3.2 算法融合¶
sensor_info.hr_algo_ops作为函数指针指向static void vc32s_algo_process(uint8_t hr_type, void *data, void *g_in)函数。
执行sensor_info.hr_algo_ops时，会将心率传感器原始数据与运动传感器数据链表传入 vc32s_algo_process 函数。
心率传感器算法内部通过遍历链表取出加速度数据：
展开代码
static void vc32s_get_gsensor_data(vc32s_get_gsensortypedef *pGsensorData, void *g_in)
{
uint16_t gs_data_len = 0;
uint16_t gs_node_num = 0;
uint8_t count_gs_data = 0, count_gs_data_left = 0;
uint16_t hr_gs_data_buf_len = VC32S_BUFF_SIZE;
uint8_t cash_num[20] = {0, 2, 4, 6, 8, 10, 12, 14, 16, 18, 20, 22, 24, 26, 28, 30, 32, 34, 36, 38};
rt_list_t *pos, *next;
gsensors_fifo_t *node;
if (g_in)
{
pGsensorData->sampling_rate = 25;
pGsensorData->sensing_rate = 4;
pGsensorData->efective_bits = 12;
gsensor_data_header = (rt_list_t *)g_in;
if (!rt_list_len(gsensor_data_header)) //if no gsensor data , return
return;
gs_data_len = 0;
rt_list_for_each_safe(pos, next, gsensor_data_header)
{
node = rt_list_entry(pos, gsensors_fifo_t, list);
if ((gs_data_len + node->num) >= hr_gs_data_buf_len)
gs_node_num = hr_gs_data_buf_len - gs_data_len;
else
gs_node_num = node->num;
//get gsdata
for (count_gs_data = 0; count_gs_data < gs_node_num; count_gs_data++)
{
pGsensorData->gsensor_x_axis[count_gs_data + gs_data_len] = node -> buf[count_gs_data].acce_data[0];
pGsensorData->gsensor_y_axis[count_gs_data + gs_data_len] = node -> buf[count_gs_data].acce_data[1];
pGsensorData->gsensor_z_axis[count_gs_data + gs_data_len] = node -> buf[count_gs_data].acce_data[2];
}
gs_data_len += gs_node_num;
if (gs_data_len == hr_gs_data_buf_len)
break;
}
}
if (gs_data_len < hr_gs_data_buf_len)
{
for (count_gs_data_left = gs_data_len; count_gs_data_left < hr_gs_data_buf_len; count_gs_data_left++)
{
pGsensorData->gsensor_x_axis[count_gs_data_left] = pGsensorData->gsensor_x_axis[gs_data_len - 1];
pGsensorData->gsensor_y_axis[count_gs_data_left] = pGsensorData->gsensor_y_axis[gs_data_len - 1];
pGsensorData->gsensor_z_axis[count_gs_data_left] = pGsensorData->gsensor_z_axis[gs_data_len - 1];
}
gs_data_len = hr_gs_data_buf_len;
}
//for(uint8_t i = 0; i < hr_gs_data_buf_len; i++)
//{
// VC32S_LOG_I("[hr_algo]>:gs_data_raw: x = %d, y = %d, z = %d;\n", pGsensorData->gsensor_x_axis[i], pGsensorData->gsensor_y_axis[i], pGsensorData->gsensor_z_axis[i]);
//}
VC32S_LOG_I("[hr_algo]>:gs_data_len = %d;\n");
for (count_gs_data = 0; count_gs_data < VC32S_BUFF_SIZE / 2; count_gs_data++)
{
//cash_num[40]
pGsensorData->gsensor_x_axis[count_gs_data] = pGsensorData->gsensor_x_axis[cash_num[count_gs_data]];
pGsensorData->gsensor_y_axis[count_gs_data] = pGsensorData->gsensor_y_axis[cash_num[count_gs_data]];
pGsensorData->gsensor_z_axis[count_gs_data] = pGsensorData->gsensor_z_axis[cash_num[count_gs_data]];
}
}
对加速度数据进行处理后，结合心率传感器数据进行融合计算：
展开代码
void vc32s_algo_process(uint8_t hr_type, void *data, void *g_in)
{
uint8_t algoCallNum = 0;
uint8_t vcSportFlag = 0;
vc32s_get_gsensortypedef gsensor_data = {0};
hrs_raw_data_t *p_data = (hrs_raw_data_t *)data;
vc32s_raw_data_t *hr_data = (vc32s_raw_data_t *) p_data->p_rawdata;
//clear health_value
rt_memset((void *)&health_value, 0, sizeof(vc32s_algo_result_t));
if (hr_data == RT_NULL)
{
VC32S_LOG_I("hr_algo: input data point is NULL!\n");
return;
}
if (hr_data->SampleRate == 0)
{
VC32S_LOG_I("hr_algo: input data is invalid! \n");
return;
}
//VC32S_LOG_I("vc32s_algo_process vcFifoReadFlag = %d, vcPsFlag = %d, wearstatus = %d, SampleRate = %d, ppgLen = %d;\n", \
//hr_data->vcFifoReadFlag, hr_data->vcPsFlag, hr_data->wearstatus, hr_data->SampleRate, hr_data->ppgLen);
//VC32S_LOG_I("wearstatus= %d\n", hr_data->wearstatus);
#if defined (USING_GSENSOR_DATA_STORE_LIST) && defined(SENSOR_USING_6D)
/* Note: when using this function, make sure that the HR cycle is a multiple of the gsensor cycle, otherwise there will be problems */
vc32s_get_gsensor_data(&gsensor_data, g_in);
#endif
if (hr_data->vcFifoReadFlag || hr_data->vcPsFlag)
{
if (SENSOR_HR == hr_type)
{
algoInputData.envSample = hr_data->envValue[0];
//VC32S_LOG_I("envSample = %d, \n", algoInputData.envSample);
for (algoCallNum = 0; algoCallNum < 20; algoCallNum++)
{
algoInputData.ppgSample = hr_data->ppgValue[algoCallNum];
algoInputData.axes.x = gsensor_data.gsensor_x_axis[algoCallNum]; //The direction vertical with ARM.
algoInputData.axes.y = gsensor_data.gsensor_y_axis[algoCallNum]; //The direction parallel with ARM.
algoInputData.axes.z = gsensor_data.gsensor_z_axis[algoCallNum]; //The direction upside.
//VC32S_LOG_I("ggp=%d, x=%d, y=%d, z=%d;\n", algoInputData.ppgSample, algoInputData.axes.x, algoInputData.axes.y, algoInputData.axes.z);
Algo_Input(&algoInputData, 1000 / hr_data->SampleRate, vcSportMode, 1, 0);
}
Algo_Output(&algoOutputData);
health_value.hr = algoOutputData.hrData;
HeartRateValue = algoOutputData.hrData;
#ifdef VC32S_DEBUG_LOG_EN
VC32S_LOG_I("vcHr02_process (hr) = %d\n", algoOutputData.hrData);
#endif
}
else if (SENSOR_SPO2 == hr_type)
{
for (algoCallNum = 0; algoCallNum < 20; algoCallNum++) //ppglength = 20
{
float vcIrPPG = (float)hr_data->ppgValue[algoCallNum * 2];
float vcRedPPG = (float)hr_data->ppgValue[algoCallNum * 2 + 1];
float vcSpo2Value = spo2Algo(vcRedPPG, vcIrPPG, 1);
vcSportFlag = vcSportMotionCalculate(gsensor_data.gsensor_x_axis[algoCallNum], gsensor_data.gsensor_y_axis[algoCallNum], gsensor_data.gsensor_z_axis[algoCallNum]);
if ((!vcSportFlag) && (vcSpo2Value > 0) && (vcSpo2Value <= 100))
{
health_value.spo2 = (uint8_t)vcSpo2Value;
#ifdef VC32S_DEBUG_LOG_EN
VC32S_LOG_I("health_value.spo2 = %d\n", health_value.spo2);
#endif
}
}
}
}
else
{
if (SENSOR_HR == hr_type)
{
health_value.hr = 0;
HeartRateValue = 0;
VC32S_LOG_I("no_process (hr) = %d;\n", HeartRateValue);
}
else if (SENSOR_SPO2 == hr_type)
{
health_value.spo2 = 0;
VC32S_LOG_I("no _process (SPO2) = %d;\n", health_value.spo2);
}
}
#if defined (USING_GSENSOR_DATA_STORE_LIST) && defined(SENSOR_USING_6D) && (SportMotionEn)
if (!hr_data->vcFifoReadFlag)
green_led_off_state_gsensor_abs_sum_diff_func(gsensor_data.gsensor_x_axis[0], gsensor_data.gsensor_y_axis[0], gsensor_data.gsensor_z_axis[0]);
#endif
health_value.status = hr_data->wearstatus;
return ;
}
4.地磁传感器数据融合¶
地磁传感器得到的数据是三轴磁场强度值。在倾斜状态下，单独使用地磁传感器数据计算出的方向角会产生误差。为了提高方向角的准确性，通常会结合加速度传感器的数据进行融合处理，从而补偿倾斜带来的影响。
4.1 获取数据¶
在mag_algo中，开启 USING_GSENSOR_DATA_STORE_LIST 与 SENSOR_USING_6D 宏后，在地磁传感器初始化函数mag_init中获取运动传感器数据链表并存储到sensor_info.gs_input中：
/*mag need gs data,so tell mag drv gs data list header*/
#if defined (SENSOR_USING_6D) && !defined (BSP_USING_PC_SIMULATOR) && defined (USING_GSENSOR_DATA_STORE_LIST)
if (gsensor_is_opened())
sensor_info.gs_input = gsensor_get_stored_data();
#endif
在周期性执行的 mag_algo_scheduler 中，将原始数据与加速度链表一起传递给算法处理函数：
/*LCPU need fetch data in mag_algo_scheduler*/
#ifndef SENSOR_IN_HCPU
ret = mag_data_fetch(0, &mag_data);
#endif
#ifdef MAG_ALGO_DEBUG
rt_kprintf("mag_data x= %f, num = %d;\n", mag_data.buf[0].mag_data[0], mag_data.num);
#endif
if (mag_data.num > 0 && sensor_info.mag_algo_ops)
{
sensor_info.mag_algo_ops(period, &mag_data, sensor_info.gs_input);
}
}
4.2 算法融合¶
sensor_info.mag_algo_ops作为函数指针指向static void qmc_algo_ops(uint32_t peroid, mag_fifo_t *data, void *g_in)函数。
执行sensor_info.mag_algo_ops时，会将地磁传感器原始数据与加速度链表传入 qmc_algo_ops 函数。
地磁传感器算法内部通过遍历链表取出加速度数据：
展开代码
static void qmc6310_get_gsensor_data(uint8_t index, float *x, float *y, float *z, void *g_in)
{
#if defined (USING_GSENSOR_DATA_STORE_LIST) && (defined (BF0_LCPU) || defined (SENSOR_IN_HCPU))
/*mag and gs all in hcpu or all in lcpu*/
rt_list_t *pos, *next;
gsensors_fifo_t *node;
if (qmc6310_dev->gs_input)
{
rt_list_t *gsensor_data_header = (rt_list_t *)g_in;
if (!rt_list_len(gsensor_data_header)) //if no gsensor data , return
return;
rt_list_for_each_safe(pos, next, gsensor_data_header)
{
node = rt_list_entry(pos, gsensors_fifo_t, list);
if (index > node ->num)
index = node ->num - 1;
*x = node -> buf[index].acce_data[0];
*y = node -> buf[index].acce_data[1];
*z = node -> buf[index].acce_data[2];
#ifdef QMC6310_DEBUG
rt_kprintf("qmc6310_get_gs_data: x=%f, y=%f, z=%f;\n", node -> buf[index].acce_data[0], node -> buf[index].acce_data[1], node -> buf[index].acce_data[2]);
#endif
break;
}
}
#else
/*mag inhcpu and gs in lcpu*/
//to be add: get gs data in ui;
#endif
}
对加速度数据进行处理后，结合地磁传感器数据进行融合计算：
展开代码
static void qmc_algo_ops(uint32_t peroid, mag_fifo_t *data, void *g_in)
{
float mag_data[3];
float result_data[3];
float offset_data[3];
float rr_data;
float ori_data[3];
int8_t accuracy;
uint8_t mag_coordinate;
float raw_mag[3];
if (g_in)
mag_get_gsensor_accxyz(g_in);
else
rt_kprintf("mag can't get acc data, try open macro USING_GSENSOR_DATA_STORE_LIST ,and open gs!\n");
mag_data[0] = data->buf[0].mag_data[0];
mag_data[1] = data->buf[0].mag_data[1];
mag_data[2] = data->buf[0].mag_data[2];
/* Convert the coordinate system */
mag_coordinate = 2; //default
#ifdef COORDINATE_GS_TO_MAG
mag_coordinate = 6;
#endif
mag_coordinate_raw_to_real(mag_coordinate, mag_data, mag_real_data);
raw_mag[0] = mag_real_data[0] * evers[0][0] + mag_real_data[1] * evers[0][1] + mag_real_data[2] * evers[0][2];
raw_mag[1] = mag_real_data[0] * evers[1][0] + mag_real_data[1] * evers[1][1] + mag_real_data[2] * evers[1][2];
raw_mag[2] = mag_real_data[0] * evers[2][0] + mag_real_data[1] * evers[2][1] + mag_real_data[2] * evers[2][2];
convert_magnetic(raw_mag, result_data, offset_data, &rr_data, &accuracy);
if ((!flag_calibrate) && accuracy == 3)
{
flag_calibrate = 1;
rt_kprintf("flag_calibrate set 1\n");
}
#ifdef QMC6310_DEBUG
rt_kprintf("x = %f, y = %f, z = %f, x_bias= %f, y_bias= %f, z_bias = %f, accuracy = %d;\n",
result_data[0], result_data[1], result_data[2], raw_mag[0] - result_data[0], raw_mag[1] - result_data[1],
raw_mag[2] - result_data[2], accuracy);
rt_kprintf("offset_data_x = %f, offset_data_y = %f, offset_data_z = %f, rr_data= %f;\n",
offset_data[0], offset_data[1], offset_data[2], rr_data);
#endif
QST_ORI_progress(acc, result_data, ori_data, &accuracy);
#ifdef QMC6310_DEBUG
rt_kprintf("qmc data: mag_real_X=%f, mag_real_Y=%f, mag_real_Z=%f, accX=%f, accY=%f, accZ=%f !\n", \
mag_real_data[0], mag_real_data[1], mag_real_data[2], acc[0], acc[1], acc[2]);
rt_kprintf("mag data:ori_data=%f,%f,%f,uracy=%d\n", ori_data[0], ori_data[1], ori_data[2], accuracy);
#endif
if ((!flag_calibrate))
qmc_msg.uracy = accuracy;
else
qmc_msg.uracy = 3;
qmc_msg.azimuth = ori_data[0];
qmc_msg.pitch = ori_data[1];
qmc_msg.roll = ori_data[2];
if (qmc_msg.uracy == 0)
{
rt_kprintf("mag data invalid, Please calibrate first.\n");
}
mag_meas_result_postprocess(peroid, &qmc_msg);
}