aplm8000sdk/src/modules/imu/imu.c

// 周涛 2023-02-08
// 基于传感器参数计算位移

#include "../../includes/includes.h"

// 定义模块所需的全局变量
static IMU_INIT_PARAM_T gImu_stInitParam = {0}; // 模块初始化参数

uint8 *pgImu_byAccDataList = NULL;                       // 加速度数据队列(存储js模块注入的加速度)
static flt32 gImu_afAccDataList[IMU_ACC_LIST_LEN] = {0}; // 加速度点列表
static uint8 gImu_byGenerateIndex = 0;                   // 生产索引
static uint8 gImu_byConsumeIndex = 0;                    // 消费索引
static uint8 gImu_byRunFlag = 0;                         // 系统正常运行标识符，0：初始化，1：正常运行

// 模块初始化
void imu_init()
{
    pgImu_byAccDataList = (uint8 *)malloc(IMU_ACC_LIST_SIZE); // 申请一片内存，用于存储js注入的加速度数据队列

    gImu_byRunFlag = 0; // 初始化状态
    memset(&gImu_stInitParam, 0, sizeof(IMU_INIT_PARAM_T));
    memset(&gImu_afAccDataList, 0, sizeof(flt32) * IMU_ACC_LIST_LEN);

    // 加速度噪声
    gImu_stInitParam.fNoiseX = -0.0047304;
    gImu_stInitParam.fNoiseY = 0.047326;  // 0.057326
    gImu_stInitParam.fNoiseZ = -0.015313; //-0.984687

    // 选峰参数
    gImu_stInitParam.dwPeakPeakDistance = 35; // 35; // 峰间距
    gImu_stInitParam.dwPeakValueThres = 102;  // 峰值门限

    printf("imu init ok \n");

    return;
}

// 释放内存
void imu_free_memory()
{
    if (!pgImu_byAccDataList)
    {
        free(pgImu_byAccDataList);
        pgImu_byAccDataList = NULL;
    }
}

// 查询加速度存储地址(exchange模块使用)
int32 *imu_get_acc_address_exchange()
{
    return (int32 *)pgImu_byAccDataList;
}

// 保存加速度(exchange模块使用)
void imu_save_acc_list_exchange()
{
    uint32 dwAccListLen = 0;

    // 获得数队列长度
    memcpy(&dwAccListLen, (uint32 *)pgImu_byAccDataList, 4);
    imu_save_acc_xyz(dwAccListLen);
    memset(pgImu_byAccDataList, 0, IMU_ACC_LIST_SIZE); // 清空数据缓存
}

// 保存加速度
// fAccX:X轴加速度
// fAccY:Y轴加速度
// fAccZ:Z轴加速度
// dwTimestamp:时间戳
void imu_save_acc_xyz(uint32 dwAccListLen)
{
    IMU_ACC_DATA_T *pstAccData = NULL;

    flt32 fAccX = 0;
    flt32 fAccY = 0;
    flt32 fAccZ = 0;
    flt32 fAccTotal = 0; // 三轴加速度之和

    // 加速度队列
    pstAccData = (IMU_ACC_DATA_T *)(pgImu_byAccDataList + 4);
    for (uint32 i = 0; i < dwAccListLen; i++)
    {
        fAccX = pstAccData->fAccX - gImu_stInitParam.fNoiseX;
        fAccY = pstAccData->fAccY - gImu_stInitParam.fNoiseY;
        fAccZ = pstAccData->fAccZ - gImu_stInitParam.fNoiseZ;
        fAccTotal = fAccX * fAccX + fAccY * fAccY + fAccZ * fAccZ;
        fAccTotal = sqrt(fAccTotal); // 合加速度
        gImu_afAccDataList[gImu_byGenerateIndex] = fAccTotal;
        pstAccData++;

        // 生产索引加1
        gImu_byGenerateIndex++;
    }

    // 运行标识符置位
    if (!gImu_byRunFlag)
    {
        if (gImu_byGenerateIndex > 60)
        {
            gImu_byRunFlag = 1;
        }
    }

    return;
}

// 计算位移(定位模块使用)
// 返回值，0：失败，1：成功，静止，50：成功，移动
uint32 imu_cal_distance_lct()
{
    uint8 byMoveFlag = 0;

    if (!gImu_byRunFlag)
    {
        return 0;
    }

    // 计算位移
    byMoveFlag = imu_cal_distance();
    return byMoveFlag;
}

// 计算位移
// 返回值，0：失败，1：成功，静止，50：成功，移动
uint32 imu_cal_distance()
{
    uint8 byResult = 0;
    flt32 afAccDataList[IMU_ACC_LIST_ONE_SECOND_LEN] = {0}; // 有效加速度点队列（1秒）
    uint8 byAccDataListLen = 0;                             // 有效加速度点队列长度

    uint8 byMoveFlag = 0;

    // 系统处于初始化阶段
    if (!gImu_byRunFlag)
    {
        return 0;
    }

    memset(afAccDataList, 0, sizeof(flt32) * IMU_ACC_LIST_ONE_SECOND_LEN);

    // 选择加速度
    byResult = imu_select_acc_xyz(afAccDataList, &byAccDataListLen);
    if (!byResult || byAccDataListLen <= 0)
    {
        return 0;
    }

    // 判断加速度是否超过移动门限值
    byResult = imu_check_max_acc(afAccDataList, byAccDataListLen, &byMoveFlag);
    if (!byResult)
    {
        return 0;
    }

    // 检查移动标识
    if (byMoveFlag)
    {
        return 50; // 成功，但移动
    }

    return 1; // 成功，但静止
}

// 选择加速度点集
// pfValidAccDataList:有效加速度队列，返回参数
// pfValidAccDataListLen:有效加速度队列长度，返回参数
uint8 imu_select_acc_xyz(flt32 *pfValidAccDataList, uint8 *pfValidAccDataListLen)
{
    flt32 *pfData = gImu_afAccDataList; // 加速度数据队列
    uint8 byCounter = 0;                // 有效加速度计数器

    // 容错处理
    if (pfValidAccDataList == NULL || pfValidAccDataListLen == NULL)
    {
        return 0;
    }

    // 遍历队列，选择当前窗口内的加速度(全部)
    while (gImu_byConsumeIndex != gImu_byGenerateIndex)
    {
        pfValidAccDataList[byCounter] = pfData[gImu_byConsumeIndex];
        byCounter++;

        if (byCounter >= IMU_ACC_LIST_ONE_SECOND_LEN)
        {
            gImu_byConsumeIndex++;
            break;
        }
        gImu_byConsumeIndex++;
    }

    *pfValidAccDataListLen = byCounter;

    return 1;
}

// 检查加最大速度是否超过门限值
// pfAccDataList:加速度队列
// byListLen:加速度队列长度
// pbyMoveFlag:移动标识符，1:移动，0:不移动
uint8 imu_check_max_acc(flt32 *pfAccDataList, uint8 byListLen, uint8 *pbyMoveFlag)
{
    flt32 fMaxValue = FLT_MIN;
    flt32 fThres = (flt32)gImu_stInitParam.dwPeakValueThres / 100; // 峰值门限

    if (pfAccDataList == NULL || byListLen <= 0 || pbyMoveFlag == NULL)
    {
        return 0;
    }

    // 查找加速度最大值
    for (uint8 i = 0; i < byListLen; i++)
    {
        if (pfAccDataList[i] > fMaxValue)
        {
            fMaxValue = pfAccDataList[i];
        }
    }

    // 判断加速度是否超过门限
    if (fMaxValue > fThres)
    {
        *pbyMoveFlag = 1; // 有移动
    }
    else
    {
        *pbyMoveFlag = 0; // 静止
    }

    return 1;
}