lmx/99_7018_lmx
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first

Browse Source
This commit is contained in:
lmx
2025-10-29 13:10:02 +08:00
commit 49a07fa419
2284 changed files with 642060 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

848
cpu/br28/spatial_effect/spatial_effect.c Normal file
View File

@ -0,0 +1,848 @@
/*****************************************************************
>file name : spatial_audio.c
>create time : Mon 03 Jan 2022 01:58:51 PM CST
*****************************************************************/
#include "typedef.h"
#include "spatial_effect_imu.h"
#include "system/includes.h"
#include "audio_base.h"
#include "app_config.h"
#include "audio_codec_clock.h"
#include "spatial_effect_tws.h"
#if TCFG_AUDIO_SPATIAL_EFFECT_ENABLE
#include "spatial_effect.h"
#if SPATIAL_AUDIO_EFFECT_ENABLE
#include "tech_lib/effect_surTheta_api.h"
#include "tech_lib/SpatialAudio_api.h"
#endif
const int voicechange_fft_PLATFORM = 2;
/*T0模式下有效
0：环境声更明显
1：最左最右的时候音量差异更明显*/
const int OLD_V_FLAG = 1;/*0 or 1*/
/*角度的映射音量密度
OLD_V_FLAG = 1时有效*/
const int P360_MRATE = 128;/*0 ~ 128*/
/*空间音频模式使能控制变量
* 0: 关闭音效，保留角度跟踪
* 1: 打开音效和角度跟踪
* 2: 轻量级音效和角度跟踪*/
const int P360_REVERB_NE = 1;
static struct spatial_audio_context *spatial_hdl = NULL;
struct sound360td_algo {
int angle;
const PointSound360TD_FUNC_API *ops;
unsigned int run_buf[0];
};
struct spatial_calculator {
void *privtate_data;
u8 work_buf[0];
};
int anglevolume = 150;
int angleresetflag = 0;
static RP_PARM_CONIFG parmK = {
.trackKIND = P360_T0,
.ReverbKIND = P360_R1,
.reverbance = 70,
.dampingval = 70,
};
static u8 param_update_flag = 0;
#if TCFG_SPATIAL_EFFECT_ONLINE_ENABLE
void spatial_effect_online_updata(RP_PARM_CONIFG *params)
{
memcpy(&parmK, params, sizeof(parmK));
param_update_flag = 1;
}
void get_spatial_effect_reverb_params(RP_PARM_CONIFG *params)
{
memcpy(params, &parmK, sizeof(parmK));
}
#endif /*TCFG_SPATIAL_EFFECT_ONLINE_ENABLE*/
static void *spatial_audio_effect_init(void)
{
struct sound360td_algo *algo = NULL;
PointSound360TD_FUNC_API *ops = get_PointSound360TD_func_api();
PointSound360TD_PARM_SET params;
RP_PARM_CONIFG rp_obj;
angleresetflag = 1;
/*音效参数初始化*/
memcpy(&rp_obj, &parmK, sizeof(parmK));
param_update_flag = 0;
params.theta = 0;
params.volume = anglevolume;
int buf_size = ops->need_buf(P360TD_REV_K1);
algo = (struct sound360td_algo *)malloc(sizeof(struct sound360td_algo) + buf_size);
if (!algo) {
return NULL;
}
algo->ops = ops;
algo->ops->open_config(algo->run_buf, &params, &rp_obj);
algo->angle = 0;
return algo;
}
static int spatial_audio_effect_handler(void *effect, void *data, int len)
{
struct sound360td_algo *algo = (struct sound360td_algo *)effect;
int frames = len >> 2;
#if TCFG_SPATIAL_EFFECT_ONLINE_ENABLE
if (param_update_flag) {
param_update_flag = 0;
PointSound360TD_PARM_SET params;
params.theta = 0;
params.volume = anglevolume;
algo->ops->open_config(algo->run_buf, &params, &parmK);
}
#endif /*TCFG_SPATIAL_EFFECT_ONLINE_ENABLE*/
algo->ops->run(algo->run_buf, data, data, frames);
return len;
}
static void spatial_audio_effect_close(void *effect)
{
struct sound360td_algo *algo = (struct sound360td_algo *)effect;
free(algo);
}
/*
* 空间效果参数配置接口
*/
static void spatial_audio_effect_params_setup(void *effect, int angle)
{
struct sound360td_algo *algo = (struct sound360td_algo *)effect;
if (angle != algo->angle) {
PointSound360TD_PARM_SET params;
//TODO
params.theta = angle;
params.volume = 100;
params.volume = anglevolume;
algo->ops->init(algo->run_buf, &params);
algo->angle = angle;
}
}
extern void put_float(double fv);
/*获取vm加速度计偏置*/
int read_vm_acc_cel_param(acc_cel_t *acc_cel)
{
int ret = 0;
ret = syscfg_read(CFG_IMU_ACC_OFFEST_ID, acc_cel, sizeof(acc_cel_t));
if (ret != sizeof(acc_cel_t)) {
printf("vm acc_cel read fail !!!");
acc_cel->acc_offx = -48.231f;
acc_cel->acc_offy = -57.035f;
acc_cel->acc_offz = -84.097f;
} else {
printf("vm acc_cel read succ !!!");
}
printf("acc_offx : ");
put_float(acc_cel->acc_offx);
printf("acc_offy : ");
put_float(acc_cel->acc_offy);
printf("acc_offz : ");
put_float(acc_cel->acc_offz);
return 0;
}
/*获取vm陀螺仪偏置*/
int read_vm_gyro_cel_param(gyro_cel_t *gyro_cel)
{
int ret = 0;
ret = syscfg_read(CFG_IMU_GYRO_OFFEST_ID, gyro_cel, sizeof(gyro_cel_t));
if (ret != sizeof(gyro_cel_t)) {
printf("vm gyro_cel read fail !!!");
gyro_cel->gyro_x = -1.694f;
gyro_cel->gyro_y = -0.521f;
gyro_cel->gyro_z = -0.078f;
} else {
printf("vm gyro_cel read succ !!!");
}
printf("gyro_x : ");
put_float(gyro_cel->gyro_x);
printf("gyro_y : ");
put_float(gyro_cel->gyro_y);
printf("gyro_z : ");
put_float(gyro_cel->gyro_z);
return 0;
}
/*配置传感器算法参数*/
int space_motion_param_init(info_spa_t *info_spa, spatial_config_t *conf, tranval_t *tranval)
{
#if TCFG_ICM42670P_ENABLE
if (info_spa) {
info_spa->fs = 99.f;
info_spa->len = 6;
info_spa->sensitivity = 16.4f;
info_spa->range = 16;
}
if (conf) {
conf->beta = 0.1f;
conf->cel_val = 0.14f;
conf->SerialTime = 0.2f;
conf->time = 1.0f;
conf->val = 0.02f;
conf->sensval = 0.02f;
}
if (tranval) {
tranval->trans_x[0] = 0;
tranval->trans_x[1] = 1;
tranval->trans_x[2] = 0;
tranval->trans_y[0] = 0;
tranval->trans_y[1] = 0;
tranval->trans_y[2] = -1;
tranval->trans_z[0] = -1;
tranval->trans_z[1] = 0;
tranval->trans_z[2] = 0;
}
#elif TCFG_LSM6DSL_ENABLE
if (info_spa) {
info_spa->fs = 104.f;
info_spa->len = 6;
info_spa->sensitivity = 14.28f;
info_spa->range = 16;
}
if (conf) {
conf->beta = 0.1f;
conf->cel_val = 0.12f;
conf->SerialTime = 0.24f;
conf->time = 1.f;
conf->val = 0.02f;
conf->sensval = 0.01f;
}
if (tranval) {
tranval->trans_x[0] = 0;
tranval->trans_x[1] = 1;
tranval->trans_x[2] = 0;
tranval->trans_y[0] = 0;
tranval->trans_y[1] = 0;
tranval->trans_y[2] = -1;
tranval->trans_z[0] = -1;
tranval->trans_z[1] = 0;
tranval->trans_z[2] = 0;
}
#elif TCFG_MPU6887P_ENABLE
if (info_spa) {
info_spa->fs = 100.f;
info_spa->len = 6;
info_spa->sensitivity = 16.4f;
info_spa->range = 16;
}
if (conf) {
conf->beta = 0.1f;
conf->cel_val = 0.12f;
conf->SerialTime = 0.2f;
conf->time = 1.0f;
conf->val = 0.07f;
conf->sensval = 0.01f;
}
if (tranval) {
tranval->trans_x[0] = -1;
tranval->trans_x[1] = 0;
tranval->trans_x[2] = 0;
tranval->trans_y[0] = 0;
tranval->trans_y[1] = 0;
tranval->trans_y[2] = -1;
tranval->trans_z[0] = 0;
tranval->trans_z[1] = -1;
tranval->trans_z[2] = 0;
}
#else /*TCFG_MPU6050_EN*/
if (info_spa) {
info_spa->fs = 100.f;
info_spa->len = 6;
info_spa->sensitivity = 16.4f;
info_spa->range = 16;
}
if (conf) {
conf->beta = 0.1f;
conf->cel_val = 0.08f;
conf->SerialTime = 0.2f;
conf->time = 1.0f;
conf->val = 0.02f;
conf->sensval = 0.01f;
}
if (tranval) {
tranval->trans_x[0] = -1;
tranval->trans_x[1] = 0;
tranval->trans_x[2] = 0;
tranval->trans_y[0] = 0;
tranval->trans_y[1] = 0;
tranval->trans_y[2] = -1;
tranval->trans_z[0] = 0;
tranval->trans_z[1] = -1;
tranval->trans_z[2] = 0;
}
#endif
return 0;
}
static void *space_motion_calculator_open(void)
{
struct spatial_calculator *c = NULL;
//传感器算法参数初始化
info_spa_t info_spa;
spatial_config_t conf;
tranval_t tranval;
space_motion_param_init(&info_spa, &conf, &tranval);
//陀螺仪偏置
gyro_cel_t gyro_cel;
read_vm_gyro_cel_param(&gyro_cel);
//加速度偏置
acc_cel_t acc_cel;
read_vm_acc_cel_param(&acc_cel);
int buf_size = get_Spatial_buf(info_spa.len);
c = (struct spatial_calculator *)zalloc(sizeof(struct spatial_calculator) + buf_size);
if (!c) {
return NULL;
}
init_Spatial(c->work_buf, &info_spa, &tranval, &conf, &gyro_cel, &acc_cel);
return c;
}
static void space_motion_calculator_close(void *calculator)
{
if (calculator) {
free(calculator);
}
}
/*
* 空间位置检测处理
*/
static int space_motion_detect(void *calculator, short *data, int len)
{
struct spatial_calculator *c = (struct spatial_calculator *)calculator;
int heading = 0;
if (len == 0) {
return 0;
}
/*
* TODO : 这里是空间运动检测处理
*/
if (angleresetflag) {
Spatial_reset(c->work_buf);
angleresetflag = 0;
}
int group_num = (len >> 1) / 6;
while (group_num--) {
Spatial_cacl(c->work_buf, data);
data += 6;
heading = get_Spa_angle(c->work_buf, TRACK_SENSITIVITY, ANGLE_RESET_SENSITIVITY);/*1 ~ 0.001，1：表示即时跟踪，数值越小跟踪越慢*/
int flag = Spatial_stra(c->work_buf, ANGLE_RESET_TIME, 0.5f);
if (flag == 1) {
Spatial_reset(c->work_buf);
flag = 0;
}
}
/* printf("%d\n", heading); */
return heading;
}
#if TCFG_USER_TWS_ENABLE
static void spatial_tws_data_handler(void *priv, void *data, int len)
{
struct spatial_audio_context *ctx = (struct spatial_audio_context *)priv;
memcpy(&ctx->tws_angle, data, sizeof(ctx->tws_angle));
}
#endif
extern u8 get_a2dp_spatial_audio_head_tracked(void);
void *spatial_audio_open(void)
{
struct spatial_audio_context *ctx;
#if SPATIAL_AUDIO_EFFECT_ENABLE
ctx = (struct spatial_audio_context *)zalloc(sizeof(struct spatial_audio_context) + 1024);
#else
ctx = (struct spatial_audio_context *)zalloc(sizeof(struct spatial_audio_context));
#endif
if (!ctx) {
return NULL;
}
spatial_hdl = ctx;
audio_codec_clock_set(SPATIAL_EFFECT_MODE, AUDIO_CODING_PCM, 0);
ctx->head_tracked = get_a2dp_spatial_audio_head_tracked();
if (ctx->head_tracked) {
ctx->sensor = space_motion_detect_open();
}
#if SPATIAL_AUDIO_EFFECT_ENABLE
ctx->effect = spatial_audio_effect_init();
ctx->calculator = space_motion_calculator_open();
#endif
#if TCFG_USER_TWS_ENABLE
ctx->tws_conn = spatial_tws_create_connection(100, (void *)ctx, spatial_tws_data_handler);
#endif
#if SPATIAL_AUDIO_EXPORT_DATA
#if SPATIAL_AUDIO_EXPORT_MODE == 0
extern void force_set_sd_online(char *sdx);
force_set_sd_online("sd0");
void *mnt = mount("sd0", "storage/sd0", "fat", 3, NULL);
if (!mnt) {
printf("sd0 mount fat failed.\n");
}
#endif
task_create(audio_export_task, (void *)ctx, "ftask");
os_taskq_post_msg("ftask", 1, 0);
#endif
/*空间音频正常的声道映射为左右声道*/
ctx->mapping_channel = AUDIO_CH_LR;
return ctx;
}
int spatial_audio_set_mapping_channel(void *sa, u8 channel)
{
struct spatial_audio_context *ctx = (struct spatial_audio_context *)sa;
ctx->mapping_channel = channel;
return 0;
}
void spatial_audio_close(void *sa)
{
struct spatial_audio_context *ctx = (struct spatial_audio_context *)sa;
if (ctx->sensor) {
space_motion_detect_close(ctx->sensor);
}
#if SPATIAL_AUDIO_EFFECT_ENABLE
if (ctx->effect) {
spatial_audio_effect_close(ctx->effect);
}
if (ctx->calculator) {
space_motion_calculator_close(ctx->calculator);
}
#endif
#if TCFG_USER_TWS_ENABLE
if (ctx->tws_conn) {
spatial_tws_delete_connection(ctx->tws_conn);
}
#endif
#if SPATIAL_AUDIO_EXPORT_DATA
OS_SEM *sem = (OS_SEM *)malloc(sizeof(OS_SEM));
os_sem_create(sem, 0);
os_taskq_post_msg("ftask", 2, 1, (int)sem);
os_sem_pend(sem, 0);
free(sem);
task_kill("ftask");
#endif
free(ctx);
spatial_hdl = NULL;
audio_codec_clock_del(SPATIAL_EFFECT_MODE);
}
static int spatial_audio_remapping_data_handler(struct spatial_audio_context *ctx, void *data, int len)
{
int pcm_frames = (len >> 2);
s16 *pcm_buf = (s16 *)data;
int i = 0;
int tmp = 0;
switch (ctx->mapping_channel) {
case AUDIO_CH_L:
for (i = 0; i < pcm_frames; i++) {
pcm_buf[i] = pcm_buf[i * 2];
}
len /= 2;
break;
case AUDIO_CH_R:
for (i = 0; i < pcm_frames; i++) {
pcm_buf[i] = pcm_buf[i * 2 + 1];
}
len /= 2;
break;
case AUDIO_CH_MIX_MONO:
for (i = 0; i < pcm_frames; i++) {
tmp = pcm_buf[i * 2] + pcm_buf[i * 2 + 1];
pcm_buf[i] = tmp / 2;
}
len /= 2;
break;
default:
break;
}
return len;
}
int spatial_audio_space_data_read(void *data)
{
int data_len = 0;
if (spatial_hdl) {
void *sensor = spatial_hdl->sensor;
data_len = space_motion_data_read(sensor, data, 1024);
}
return data_len;
}
extern int aud_spatial_sensor_get_data_len();
extern int aud_spatial_sensor_data_read(s16 *data, int len);
static int spatial_audio_data_handler(struct spatial_audio_context *ctx, void *data, int len)
{
int data_len = 0;
if (ctx->sensor) {
#if TCFG_SENSOR_DATA_READ_IN_DEC_TASK
data_len = aud_spatial_sensor_get_data_len();
if (data_len) {
data_len = aud_spatial_sensor_data_read(ctx->data, data_len);
}
#else
#if !SPATIAL_AUDIO_EXPORT_DATA
data_len = space_motion_data_read(ctx->sensor, ctx->data, 1024);
#endif /*SPATIAL_AUDIO_EXPORT_DATA*/
#endif /*TCFG_SENSOR_DATA_READ_IN_DEC_TASK*/
}
if (!ctx->calculator) {
return 0;
}
int angle = 0;
if (ctx->head_tracked == 0) {
angle = 0;
} else {
#if SPATIAL_AUDIO_EXPORT_DATA
angle = space_motion_detect(ctx->calculator, (s16 *)ctx->space_fifo_buf, ctx->space_data_single_len);
#else
angle = space_motion_detect(ctx->calculator, (s16 *)ctx->data, data_len);
#endif
}
#if TCFG_USER_TWS_ENABLE
if (ctx->tws_conn && data_len) {
spatial_tws_audio_data_sync(ctx->tws_conn, (void *)&angle, sizeof(angle));
}
#endif
if (ctx->effect) {
spatial_audio_effect_handler(ctx->effect, data, len);
#if (TCFG_SPATIAL_EFFECT_ONLINE_ENABLE)
extern int set_bt_media_imu_angle(int angle);
#if TCFG_USER_TWS_ENABLE
set_bt_media_imu_angle(ctx->tws_angle);
#else
if (data_len) {
set_bt_media_imu_angle(angle);
}
#endif /*TCFG_USER_TWS_ENABLE*/
#endif /*TCFG_SPATIAL_EFFECT_ONLINE_ENABLE*/
if (ctx->sensor) { /*处理一只耳机有传感器一只耳机没有传感器的情况*/
/*有传感器的时候需要判读有传感器数据才更新角度到算法*/
if (data_len) {
#if TCFG_USER_TWS_ENABLE
spatial_audio_effect_params_setup(ctx->effect, ctx->tws_angle);
#else
spatial_audio_effect_params_setup(ctx->effect, angle);
#endif
}
} else {
/*没有传感器的时候，直接使用对耳角度*/
#if TCFG_USER_TWS_ENABLE
spatial_audio_effect_params_setup(ctx->effect, ctx->tws_angle);
#else
spatial_audio_effect_params_setup(ctx->effect, angle);
#endif
}
}
return spatial_audio_remapping_data_handler(ctx, data, len);
}
void spatial_audio_head_tracked_en(struct spatial_audio_context *ctx, u8 en)
{
printf("head_tracked = %d\n", en);
ctx->head_tracked = en;
}
u8 get_spatial_audio_head_tracked(struct spatial_audio_context *ctx)
{
printf("head_tracked = %d\n", ctx->head_tracked);
return ctx->head_tracked;
}
int spatial_audio_filter(void *sa, void *data, int len)
{
struct spatial_audio_context *ctx = (struct spatial_audio_context *)sa;
if (!len) {
return 0;
}
#if SPATIAL_AUDIO_EXPORT_DATA
if (ctx->export) {
struct data_export_header header;
ctx->audio_data_len += len;
header.magic = 0x5A;
header.ch = 0;
header.seqn = ctx->audio_seqn++;
header.len = len;
header.timestamp = jiffies;
header.crc = CRC16(data, len);
header.total_len = ctx->audio_data_len;
if (cbuf_is_write_able(&ctx->audio_cbuf, sizeof(header) + len)) {
cbuf_write(&ctx->audio_cbuf, &header, sizeof(header));
cbuf_write(&ctx->audio_cbuf, data, len);
} else {
printf("--- audio data export error --\n");
}
if (ctx->sensor) {
int data_len = 1024;
/*putchar('1');*/
data_len = space_motion_data_read(ctx->sensor, ctx->space_fifo_buf, data_len);
/*putchar('2');*/
if (data_len) {
ASSERT(data_len % 12 == 0, " , data_len : %d\n", data_len);
ctx->space_data_len += data_len;
ctx->space_data_single_len = data_len;
header.magic = 0x5A;
header.ch = 1;
header.seqn = ctx->space_seqn++;
header.len = data_len;
header.timestamp = jiffies;
header.crc = CRC16(ctx->space_fifo_buf, data_len);
header.total_len = ctx->space_data_len;
#if 0
if (cbuf_is_write_able(&ctx->space_cbuf, sizeof(header) + data_len)) {
cbuf_write(&ctx->space_cbuf, &header, sizeof(header));
cbuf_write(&ctx->space_cbuf, ctx->space_fifo_buf, data_len);
} else {
printf("--- space data export error --\n");
}
#else
if (cbuf_is_write_able(&ctx->audio_cbuf, sizeof(header) + data_len)) {
cbuf_write(&ctx->audio_cbuf, &header, sizeof(header));
cbuf_write(&ctx->audio_cbuf, ctx->space_fifo_buf, data_len);
/*
s16 *print_data = ctx->space_fifo_buf;
data_len /= 2;
printf("[%d, %d, %d]\n", print_data[data_len - 3], print_data[data_len - 2], print_data[data_len - 1]);
*/
} else {
printf("--- space data export error --\n");
}
#endif
}
}
#if SPATIAL_AUDIO_EFFECT_ENABLE //增加处理后的数据
spatial_audio_data_handler(ctx, data, len);
header.magic = 0x5A;
header.ch = 2;
header.seqn = ctx->audio_out_seqn++;
header.len = len;
header.timestamp = jiffies;
header.crc = CRC16(data, len);
header.total_len = ctx->audio_data_len;
if (cbuf_is_write_able(&ctx->audio_cbuf, sizeof(header) + len)) {
cbuf_write(&ctx->audio_cbuf, &header, sizeof(header));
cbuf_write(&ctx->audio_cbuf, data, len);
} else {
printf("--- audio data export error --\n");
}
#endif/*SPATIAL_AUDIO_EFFECT_ENABLE*/
os_taskq_post_msg("ftask", 1, 2);
}
#elif SPATIAL_AUDIO_EFFECT_ENABLE
return spatial_audio_data_handler(ctx, data, len);
#endif
return len;
}
#if SPATIAL_AUDIO_EXPORT_DATA
static int spatial_audio_export_init(struct spatial_audio_context *ctx)
{
int audio_buf_size = 40 * 1024;
int space_buf_size = 8 * 1024;
ctx->space_fifo_buf = (u8 *)malloc(1024);
ctx->audio_buf = (u8 *)malloc(audio_buf_size);
/*ctx->space_buf = (u8 *)malloc(space_buf_size);*/
cbuf_init(&ctx->audio_cbuf, ctx->audio_buf, audio_buf_size);
/*cbuf_init(&ctx->space_cbuf, ctx->space_buf, space_buf_size);*/
#if SPATIAL_AUDIO_EXPORT_MODE == 0
ctx->audio_file = fopen("storage/sd0/C/spatial_audio/aud***.raw", "w+");
/*ctx->space_file = fopen("storage/sd0/C/spatial_audio/spa***.raw", "w+"); */
#elif SPATIAL_AUDIO_EXPORT_MODE == 1
aec_uart_init();
#endif
ctx->export = 1;
return 0;
}
static void spatial_audio_export_release(struct spatial_audio_context *ctx)
{
ctx->export = 0;
if (ctx->audio_buf) {
free(ctx->audio_buf);
ctx->audio_buf = NULL;
}
if (ctx->space_buf) {
free(ctx->space_buf);
ctx->space_buf = NULL;
}
if (ctx->space_fifo_buf) {
free(ctx->space_fifo_buf);
ctx->space_fifo_buf = NULL;
}
#if SPATIAL_AUDIO_EXPORT_MODE == 0
if (ctx->audio_file) {
fclose(ctx->audio_file);
}
if (ctx->space_file) {
fclose(ctx->space_file);
}
#elif SPATIAL_AUDIO_EXPORT_MODE == 1
aec_uart_close();
#endif
}
static int audio_data_export_handler(struct spatial_audio_context *ctx)
{
int err = -EINVAL;
#if SPATIAL_AUDIO_EXPORT_MODE == 0
if (ctx->audio_file) {
if (cbuf_get_data_len(&ctx->audio_cbuf) >= 512) {
fwrite(ctx->audio_file, cbuf_get_readptr(&ctx->audio_cbuf), 512);
cbuf_read_updata(&ctx->audio_cbuf, 512);
err = 0;
} else {
//TODO
}
}
#if 0
if (ctx->space_file) {
if (cbuf_get_data_len(&ctx->space_cbuf) >= 512) {
fwrite(ctx->space_file, cbuf_get_readptr(&ctx->space_cbuf), 512);
cbuf_read_updata(&ctx->space_cbuf, 512);
err = 0;
} else {
//TODO
}
}
#endif
#elif SPATIAL_AUDIO_EXPORT_MODE == 1
u8 send = 0;
if (cbuf_get_data_len(&ctx->audio_cbuf) >= 512) {
aec_uart_fill(0, cbuf_get_readptr(&ctx->audio_cbuf), 512);
cbuf_read_updata(&ctx->audio_cbuf, 512);
err = 0;
send = 1;
}
if (!send && cbuf_get_data_len(&ctx->space_cbuf) >= 512) {
aec_uart_fill(0, cbuf_get_readptr(&ctx->space_cbuf), 512);
cbuf_read_updata(&ctx->space_cbuf, 512);
err = 0;
send = 1;
}
if (send) {
putchar('t');
aec_uart_write();
}
#endif
return err;
}
void audio_export_task(void *arg)
{
int msg[16];
int res;
int pend_taskq = 1;
struct spatial_audio_context *ctx = (struct spatial_audio_context *)arg;
while (1) {
if (pend_taskq) {
res = os_taskq_pend("taskq", msg, ARRAY_SIZE(msg));
} else {
res = os_taskq_accept(ARRAY_SIZE(msg), msg);
}
if (res == OS_TASKQ) {
switch (msg[1]) {
case 0:
spatial_audio_export_init(ctx);
break;
case 1:
spatial_audio_export_release(ctx);
os_sem_post((OS_SEM *)msg[2]);
pend_taskq = 1;
break;
case 2: //音频数据
break;
case 3: //空间移动信息
break;
}
}
if (ctx->export) {
if (0 != audio_data_export_handler(ctx)) {
pend_taskq = 1;
} else {
pend_taskq = 0;
}
}
}
}
#endif
#endif/*TCFG_AUDIO_SPATIAL_EFFECT_ENABLE*/

96
cpu/br28/spatial_effect/spatial_effect.h Normal file
View File

@ -0,0 +1,96 @@
#ifndef _SPATIAL_EFFECT_H_
#define _SPATIAL_EFFECT_H_
#include "system/includes.h"
#include "generic/typedef.h"
#include "app_config.h"
#include "tech_lib/effect_surTheta_api.h"
#define SPATIAL_AUDIO_EXPORT_DATA 0
#define SPATIAL_AUDIO_EXPORT_MODE 0
/*音效使能*/
#define SPATIAL_AUDIO_EFFECT_ENABLE 1
/*头部跟踪灵敏度：范围 0.001 ~ 1，1：表示即时跟踪，数值越小跟踪越慢*/
#define TRACK_SENSITIVITY (1.0f)
/*静止角度复位灵敏度：范围 0.001 ~ 1，1：表示立刻复位回正，数值越小回正越慢*/
#define ANGLE_RESET_SENSITIVITY (0.01f)
/*头部跟踪角度复位时间,单位：秒*/
#define ANGLE_RESET_TIME (8)
#if SPATIAL_AUDIO_EXPORT_DATA
struct data_export_header {
u8 magic;
u8 ch;
u16 seqn;
u16 crc;
u16 len;
u32 timestamp;
u32 total_len;
u8 data[0];
};
void audio_export_task(void *arg);
#endif
struct spatial_audio_context {
void *sensor;
void *effect;
void *calculator;
#if TCFG_USER_TWS_ENABLE
void *tws_conn;
int tws_angle;
#endif
u8 mapping_channel;
u32 head_tracked;
#if SPATIAL_AUDIO_EXPORT_DATA
cbuffer_t audio_cbuf;
cbuffer_t space_cbuf;
u8 *audio_buf;
u8 *space_buf;
void *space_fifo_buf;
u16 audio_seqn;
u16 audio_out_seqn;
u16 space_seqn;
u32 audio_data_len;
u32 space_data_len;
u32 timestamp;
int space_data_single_len;
u8 export;
#if SPATIAL_AUDIO_EXPORT_MODE == 0
void *audio_file;
void *space_file;
#endif
#endif
u8 data[0];
};
/*设置并更新混响参数*/
void spatial_effect_online_updata(RP_PARM_CONIFG *params);
/*获取当前混响的参数*/
void get_spatial_effect_reverb_params(RP_PARM_CONIFG *params);
/*打开空间音效*/
void *spatial_audio_open(void);
/*关闭空间音效*/
void spatial_audio_close(void *sa);
/*音效处理*/
int spatial_audio_filter(void *sa, void *data, int len);
/*设置空间音频的输出声道映射*/
int spatial_audio_set_mapping_channel(void *sa, u8 channel);
/*读取传感器的数据*/
int spatial_audio_space_data_read(void *data);
/*头部跟踪使能*/
void spatial_audio_head_tracked_en(struct spatial_audio_context *ctx, u8 en);
/*获取头部跟踪使能状态*/
u8 get_spatial_audio_head_tracked(struct spatial_audio_context *ctx);
#endif

821
cpu/br28/spatial_effect/spatial_effect_imu.c Normal file
View File

@ -0,0 +1,821 @@
/*****************************************************************
>
>file name : spatial_imu_data.c
>create time : Mon 03 Jan 2022 12:21:19 PM CST
*****************************************************************/
#include "typedef.h"
#include "app_config.h"
#include "gSensor/mpu6050.h"
#include "gSensor/gSensor_manage.h"
#include "spatial_effect_imu.h"
#include "imu_sensor/imuSensor_manage.h"
#include "imu_sensor/mpu6887/mpu6887p.h"
#include "imu_sensor/qmi8658/qmi8658c.h"
#include "imu_sensor/icm_42670p/icm_42670p.h"
#define MOTION_SENSOR_FIFO_ENABLE 1
struct space_data_context {
u8 first_data;
u8 init_state;
u16 timeout;
u16 timer;
#if (TCFG_SENSOR_DATA_EXPORT_ENABLE == SENSOR_DATA_EXPORT_USE_UART)
u8 cbuf[1024];
cbuffer_t data_cbuf;
#endif /*TCFG_SENSOR_DATA_EXPORT_ENABLE*/
};
extern int aec_uart_open(u8 nch, u16 single_size);
extern int aec_uart_fill(u8 ch, void *buf, u16 size);
extern void aec_uart_write(void);
extern int aec_uart_close(void);
void imu_sensor_power_ctl(u32 gpio, u8 value);
static s16 mpu6050_read_data(addr)
{
u8 high;
u8 low;
_gravity_sensor_get_ndata(I2C_ADDR_MPU6050_R, addr, &high, 1);
_gravity_sensor_get_ndata(I2C_ADDR_MPU6050_R, addr + 1, &low, 1);
return (high << 8) | low;
}
static int mpu6050_fifo_read_data(void *data, int len)
{
int remain_len = len;
int rlen = 0;
int total_rlen = 0;
do {
rlen = _gravity_sensor_get_ndata(I2C_ADDR_MPU6050_R, MPU6050_RA_FIFO_R_W, data, remain_len > 254 ? 254 : remain_len);
remain_len -= rlen;
total_rlen += rlen;
} while (remain_len > 0);
return total_rlen;
}
static int swap_data_endian(s16 *data, int len)
{
#define SWAP_16(x) ((((x)&0xFF00) >> 8) | (((x)&0x00FF) << 8))
len >>= 1;
for (int i = 0; i < len; i++) {
data[i] = SWAP_16(data[i]);
}
return len << 1;
}
/*
+------------------------------------------------+
| 陀螺仪满量程与LSB灵敏度对应表 |
+--------+--------------------+------------------+
| FS_SEL | Full Scale Range | LSB Sensitivity |
+--------+--------------------+------------------+
| 0 | ±250°/s | 131 LSB/°/s |
+--------+--------------------+------------------+
| 1 | ±500°/s | 65.5 LSB/°/s |
+--------+--------------------+------------------+
| 2 | ±1000°/s | 32.8 LSB/°/s |
+--------+--------------------+------------------+
| 3 | ±2000°/s | 16.4 LSB/°/s |
+--------+--------------------+------------------+
+------------------------------------------------+
| 加速计满量程与LSB灵敏度对应表 |
+--------+--------------------+------------------+
| FS_SEL | Full Scale Range | LSB Sensitivity |
+--------+--------------------+------------------+
| 0 | ±2g | 16384 LSB/g |
+--------+--------------------+------------------+
| 1 | ±4g | 8192 LSB/g |
+--------+--------------------+------------------+
| 2 | ±8g | 4096 LSB/g |
+--------+--------------------+------------------+
| 3 | ±16g | 2048 LSB/g |
+--------+--------------------+------------------+
*/
static void space_motion_detect_timer(void *arg)
{
#if MOTION_SENSOR_FIFO_ENABLE
s16 data_len = mpu6050_read_data(MPU6050_RA_FIFO_COUNTH);
s16 *data = NULL;
int i = 0;
if (data_len) {
data = (s16 *)malloc(data_len);
mpu6050_fifo_read_data(data, data_len);
swap_data_endian(data, data_len);
printf("-- data_len : %d --\n", data_len);
data_len /= sizeof(*data);
s16 *print_data = data;
for (i = 0; i < data_len; i += 3) {
printf("[%d, %d, %d]\n", data[i], data[i + 1], data[i + 2]);
}
free(data);
}
#else
s16 gyro_xout, gyro_yout, gyro_zout;
s16 accel_xout, accel_yout, accel_zout;
gyro_xout = mpu6050_read_data(MPU6050_RA_GYRO_XOUT_H);
gyro_yout = mpu6050_read_data(MPU6050_RA_GYRO_YOUT_H);
gyro_zout = mpu6050_read_data(MPU6050_RA_GYRO_ZOUT_H);
accel_xout = mpu6050_read_data(MPU6050_RA_ACCEL_XOUT_H);
accel_yout = mpu6050_read_data(MPU6050_RA_ACCEL_YOUT_H);
accel_zout = mpu6050_read_data(MPU6050_RA_ACCEL_ZOUT_H);
printf("x : %d, y : %d, z : %d\n accel, x : %d, y : %d, z : %d\n", gyro_xout, gyro_yout, gyro_zout, accel_xout, accel_yout, accel_zout);
#endif
}
int __mpu6050_space_motion_data_read(void *sensor, void *data, int len)
{
struct space_data_context *ctx = (struct space_data_context *)sensor;
#if 1
if (!ctx->init_state) {
return 0;
}
s16 data_len = mpu6050_read_data(MPU6050_RA_FIFO_COUNTH);
if (data_len < 12) {
return 0;
}
if (data_len > 1000) {
printf("gsensor fifo full : %d\n", data_len);
gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_USER_CTRL, 0x44);
}
data_len = (data_len / 12) * 12;
data_len = mpu6050_fifo_read_data(data, data_len);
if (ctx->first_data) {
ctx->first_data = 0;
return 0;
}
swap_data_endian((s16 *)data, data_len);
return data_len;
#else
int *rand_data = (int *)data;
rand_data[0] = 0x12345678;
rand_data[1] = 0x23456789;
rand_data[2] = 0x3456789A;
rand_data[3] = 0x456789AB;
rand_data[4] = 0x56789ABC;
rand_data[5] = 0x6789ABCD;
rand_data[6] = 0x789ABCDE;
rand_data[7] = 0x89ABCDEF;
return 32;
#endif
}
static void space_motion_sensor_init(void *arg)
{
struct space_data_context *ctx = (struct space_data_context *)arg;
sys_timeout_del(ctx->timeout);
ctx->timeout = 0;
gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_PWR_MGMT_1, 0x4); //关闭温度传感器
gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_INT_ENABLE, 0x0); //关闭中断
#if 1
gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_SMPLRT_DIV, 7); //设置采样率, 采样率=陀螺仪输出速率/(1+SMPLRT_DIV)
#if MOTION_SENSOR_FIFO_ENABLE
gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_USER_CTRL, 0x44);
gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_FIFO_EN, 0x78); // 陀螺仪xyz fifo使能，加速计fifo使能
#endif
gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_CONFIG, 0x6); // 1kHz
gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_GYRO_CONFIG, 0x18); // ±2000°/s
gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_ACCEL_CONFIG, 0x19); // ±2g
#endif
ctx->init_state = 1;
}
static void space_motion_sensor_reset(void)
{
gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_PWR_MGMT_1, 0x80); // 复位
}
void *__mpu6050_space_motion_detect_open(void)
{
u8 id = 0;
_gravity_sensor_get_ndata(I2C_ADDR_MPU6050_R, MPU6050_RA_WHO_AM_I, &id, 1);
if (id != MPU_ADDR) {
return NULL;
}
struct space_data_context *ctx = (struct space_data_context *)zalloc(sizeof(struct space_data_context));
if (!ctx) {
return NULL;
}
space_motion_sensor_reset();
ctx->timeout = sys_timeout_add((void *)ctx, space_motion_sensor_init, 100);
ctx->first_data = 1;
/*ctx->timer = sys_timer_add(NULL, space_motion_detect_timer, 100); */
return ctx;
}
void __mpu6050_space_motion_detect_close(void *sensor)
{
struct space_data_context *ctx = (struct space_data_context *)sensor;
#if 1
if (ctx->timeout) {
sys_timeout_del(ctx->timeout);
}
gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_USER_CTRL, 0);
gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_FIFO_EN, 0); // 陀螺仪xyz fifo使能，加速计fifo使能
/*gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_PWR_MGMT_1, BIT(6));*/
#endif
if (ctx) {
if (ctx->timer) {
sys_timer_del(ctx->timer);
ctx->timer = 0;
}
free(ctx);
}
}
#if TCFG_ICM42670P_ENABLE
int __icm42670p_space_motion_data_read(void *sensor, void *data, int len)
{
struct space_data_context *ctx = (struct space_data_context *)sensor;
#if 1
if (!ctx->init_state) {
return 0;
}
u8 sensor_name[20] = "icm42670p";
int data_len = imu_sensor_io_ctl(sensor_name, IMU_GET_SENSOR_READ_FIFO, (u8 *)data);
if (data_len < 12) {
return 0;
}
//丢掉第一次读到的数据
if (ctx->first_data) {
ctx->first_data = 0;
return 0;
}
return data_len;
#else
int *rand_data = (int *)data;
rand_data[0] = 0x12345678;
rand_data[1] = 0x23456789;
rand_data[2] = 0x3456789A;
rand_data[3] = 0x456789AB;
rand_data[4] = 0x56789ABC;
rand_data[5] = 0x6789ABCD;
rand_data[6] = 0x789ABCDE;
rand_data[7] = 0x89ABCDEF;
return 32;
#endif
}
void *__icm42670p_space_motion_detect_open(void)
{
u8 sensor_name[20] = "icm42670p";
u8 arg_data;
//唤醒MPU
imu_sensor_io_ctl(sensor_name, IMU_SENSOR_WAKEUP, &arg_data);
//检查传感器id
int ret = imu_sensor_io_ctl(sensor_name, IMU_SENSOR_SEARCH, &arg_data);
printf("ret : %d\n", ret);
if (ret != 0) {
return NULL;
}
struct space_data_context *ctx = (struct space_data_context *)zalloc(sizeof(struct space_data_context));
if (!ctx) {
return NULL;
}
ctx->first_data = 1;
ctx->init_state = 1;
printf(" __space_motion_detect_open success");
return ctx;
}
void __icm42670p_space_motion_detect_close(void *sensor)
{
struct space_data_context *ctx = (struct space_data_context *)sensor;
u8 sensor_name[20] = "icm42670p";
u8 arg_data;
#if 1
if (ctx->timeout) {
sys_timeout_del(ctx->timeout);
}
#endif
if (ctx) {
//MPU进入睡眠
imu_sensor_io_ctl(sensor_name, IMU_SENSOR_SLEEP, NULL);
if (ctx->timer) {
sys_timer_del(ctx->timer);
ctx->timer = 0;
}
free(ctx);
}
}
#endif /*TCFG_ICM42670P_ENABLE*/
#if TCFG_MPU6887P_ENABLE
/*剔除mpu6887p fifo数据中的温度数据*/
/*数据格式:acc + temp + gyro*/
static int fifo_data_delete_temp(u8 *buf, int len)
{
int data_len = len;
len = len / 14; //数据帧数
buf = buf + 6;//温度数据起始位置
int tmp = 2;
//把每帧温度数据后面的12字节的加速度和陀螺仪依次往前移，覆盖温度数据
for (int i = 0; i < len; i++) {
memcpy(buf, buf + tmp, 12);
tmp = tmp + 2;
buf = buf + 12;
}
return (data_len - (len << 1));
}
int __mpu6887p_space_motion_data_read(void *sensor, void *data, int len)
{
struct space_data_context *ctx = (struct space_data_context *)sensor;
#if 1
if (!ctx->init_state) {
return 0;
}
int data_len = mpu6887p_read_fifo_data((u8 *)data);
if (data_len < 14) {
return 0;
}
/* if (data_len > 1000) { */
/* printf("gsensor fifo full : %d\n", data_len); */
/* gravity_sensor_command(I2C_ADDR_MPU6050_W, MPU6050_RA_USER_CTRL, 0x44); */
/* } */
//丢掉第一次读到的数据
if (ctx->first_data) {
ctx->first_data = 0;
return 0;
}
//交换s16数据的高低位
swap_data_endian((s16 *)data, data_len);
//剔除温度数据
data_len = fifo_data_delete_temp((u8 *)data, data_len);
return data_len;
#else
int *rand_data = (int *)data;
rand_data[0] = 0x12345678;
rand_data[1] = 0x23456789;
rand_data[2] = 0x3456789A;
rand_data[3] = 0x456789AB;
rand_data[4] = 0x56789ABC;
rand_data[5] = 0x6789ABCD;
rand_data[6] = 0x789ABCDE;
rand_data[7] = 0x89ABCDEF;
return 32;
#endif
}
void *__mpu6887p_space_motion_detect_open(void)
{
u8 name_test[20] = "mpu6887p";
u8 arg_data;
//唤醒MPU
mpu6887p_set_sleep_enabled(0);// 0:唤醒MPU, 1:disable
//判断mpu6887p是否已经初始化了
u8 status_temp = mpu6887p_read_status();
printf("status:0x%x", status_temp);
if ((status_temp & 0x01) == 0) {
//如果还没有初始化，重新初始化
imu_sensor_io_ctl(name_test, IMU_SENSOR_ENABLE, &arg_data);
}
//检查传感器id
int ret = imu_sensor_io_ctl(name_test, IMU_SENSOR_SEARCH, &arg_data);
printf("ret : %d\n", ret);
if (ret != 0) {
return NULL;
}
struct space_data_context *ctx = (struct space_data_context *)zalloc(sizeof(struct space_data_context));
if (!ctx) {
return NULL;
}
//关闭温度传感器
mpu6887p_disable_temp_Sensor(1);
//设置fifo使能
mpu6887p_config_fifo(300, 1, 1, 1);
ctx->first_data = 1;
ctx->init_state = 1;
printf(" __space_motion_detect_open success");
return ctx;
}
void __mpu6887p_space_motion_detect_close(void *sensor)
{
struct space_data_context *ctx = (struct space_data_context *)sensor;
#if 1
if (ctx->timeout) {
sys_timeout_del(ctx->timeout);
}
#endif
if (ctx) {
//MPU进入睡眠
mpu6887p_set_sleep_enabled(1);// 0:唤醒MPU, 1:disable
if (ctx->timer) {
sys_timer_del(ctx->timer);
ctx->timer = 0;
}
free(ctx);
}
}
#endif /*TCFG_MPU6887P_ENABLE*/
#if TCFG_QMI8658_ENABLE
int __qmi8658_space_motion_data_read(void *sensor, void *data, int len)
{
struct space_data_context *ctx = (struct space_data_context *)sensor;
#if 1
if (!ctx->init_state) {
return 0;
}
u8 name_test[20] = "qmi8658";
int data_len = imu_sensor_io_ctl(name_test, IMU_GET_SENSOR_READ_FIFO, (u8 *)data);
if (data_len < 12) {
return 0;
}
//丢掉第一次读到的数据
if (ctx->first_data) {
ctx->first_data = 0;
return 0;
}
return data_len;
#else
int *rand_data = (int *)data;
rand_data[0] = 0x12345678;
rand_data[1] = 0x23456789;
rand_data[2] = 0x3456789A;
rand_data[3] = 0x456789AB;
rand_data[4] = 0x56789ABC;
rand_data[5] = 0x6789ABCD;
rand_data[6] = 0x789ABCDE;
rand_data[7] = 0x89ABCDEF;
return 32;
#endif
}
void *__qmi8658_space_motion_detect_open(void)
{
u8 name_test[20] = "qmi8658";
u8 arg_data;
//唤醒MPU
imu_sensor_io_ctl(name_test, IMU_SENSOR_WAKEUP, &arg_data);
//检查传感器id
int ret = imu_sensor_io_ctl(name_test, IMU_SENSOR_SEARCH, &arg_data);
printf("ret : %d\n", ret);
if (ret != 0) {
return NULL;
}
struct space_data_context *ctx = (struct space_data_context *)zalloc(sizeof(struct space_data_context));
if (!ctx) {
return NULL;
}
//设置fifo使能
/* u8 mytmp[4]; */
/* mytmp[0] = 128; */
/* mytmp[1] = Qmi8658_Fifo_128; */
/* mytmp[2] = Qmi8658_Fifo_Stream; */
/* mytmp[3] = QMI8658_FORMAT_12_BYTES; */
/* imu_sensor_io_ctl(name_test, IMU_SET_SENSOR_FIFO_CONFIG, &mytmp); */
ctx->first_data = 1;
ctx->init_state = 1;
printf(" __space_motion_detect_open success");
return ctx;
}
void __qmi8658_space_motion_detect_close(void *sensor)
{
struct space_data_context *ctx = (struct space_data_context *)sensor;
u8 name_test[20] = "qmi8658";
u8 arg_data;
#if 1
if (ctx->timeout) {
sys_timeout_del(ctx->timeout);
}
#endif
if (ctx) {
//MPU进入睡眠
imu_sensor_io_ctl("qmi8658", IMU_SENSOR_SLEEP, NULL);
if (ctx->timer) {
sys_timer_del(ctx->timer);
ctx->timer = 0;
}
free(ctx);
}
}
#endif /*TCFG_QMI8658_ENABLE*/
#if TCFG_LSM6DSL_ENABLE
/*交换lsm6dsl陀螺仪和加速度数据的位置*/
/*交换前数据格式:gyro + acc*/
static int swap_gyro_acc_data(s16 *buf, int len)
{
int frames = len / 12; //数据帧数
s16 tmp[3];
for (int i = 0; i < frames; i++) {
memcpy(tmp, buf, 6);
memcpy(buf, buf + 3, 6);
memcpy(buf + 3, tmp, 6);
buf = buf + 6;
}
return len;
}
int __lsm6dsl_space_motion_data_read(void *sensor, void *data, int len)
{
struct space_data_context *ctx = (struct space_data_context *)sensor;
#if 1
if (!ctx->init_state) {
return 0;
}
u8 sensor_name[20] = "lsm6dsl";
int data_len = imu_sensor_io_ctl(sensor_name, IMU_GET_SENSOR_READ_FIFO, (u8 *)data);
if (data_len < 12) {
return 0;
}
//丢掉第一次读到的数据
if (ctx->first_data) {
ctx->first_data = 0;
return 0;
}
//交换陀螺仪和加速度的数据位置
swap_gyro_acc_data(data, len);
return data_len;
#else
int *rand_data = (int *)data;
rand_data[0] = 0x12345678;
rand_data[1] = 0x23456789;
rand_data[2] = 0x3456789A;
rand_data[3] = 0x456789AB;
rand_data[4] = 0x56789ABC;
rand_data[5] = 0x6789ABCD;
rand_data[6] = 0x789ABCDE;
rand_data[7] = 0x89ABCDEF;
return 32;
#endif
}
void *__lsm6dsl_space_motion_detect_open(void)
{
u8 sensor_name[20] = "lsm6dsl";
u8 arg_data;
//唤醒MPU
imu_sensor_io_ctl(sensor_name, IMU_SENSOR_WAKEUP, &arg_data);
//检查传感器id
int ret = imu_sensor_io_ctl(sensor_name, IMU_SENSOR_SEARCH, &arg_data);
printf("ret : %d\n", ret);
if (ret != 0) {
return NULL;
}
struct space_data_context *ctx = (struct space_data_context *)zalloc(sizeof(struct space_data_context));
if (!ctx) {
return NULL;
}
//设置fifo使能
/* u8 mytmp[4]; */
/* mytmp[0] = 128; */
/* mytmp[1] = Qmi8658_Fifo_128; */
/* mytmp[2] = Qmi8658_Fifo_Stream; */
/* mytmp[3] = QMI8658_FORMAT_12_BYTES; */
/* imu_sensor_io_ctl(sensor_name, IMU_SET_SENSOR_FIFO_CONFIG, &mytmp); */
ctx->first_data = 1;
ctx->init_state = 1;
printf(" __space_motion_detect_open success");
return ctx;
}
void __lsm6dsl_space_motion_detect_close(void *sensor)
{
struct space_data_context *ctx = (struct space_data_context *)sensor;
u8 sensor_name[20] = "lsm6dsl";
u8 arg_data;
#if 1
if (ctx->timeout) {
sys_timeout_del(ctx->timeout);
}
#endif
if (ctx) {
//MPU进入睡眠
imu_sensor_io_ctl(sensor_name, IMU_SENSOR_SLEEP, NULL);
if (ctx->timer) {
sys_timer_del(ctx->timer);
ctx->timer = 0;
}
free(ctx);
}
}
#endif /*TCFG_LSM6DSL_ENABLE*/
int space_motion_data_read(void *sensor, void *data, int len)
{
struct space_data_context *ctx = (struct space_data_context *)sensor;
if (!ctx) {
return 0;
}
int data_len = 0;
#if TCFG_ICM42670P_ENABLE
data_len = __icm42670p_space_motion_data_read(sensor, data, len);
#elif TCFG_LSM6DSL_ENABLE
data_len = __lsm6dsl_space_motion_data_read(sensor, data, len);
#elif TCFG_MPU6887P_ENABLE
data_len = __mpu6887p_space_motion_data_read(sensor, data, len);
#elif TCFG_QMI8658_ENABLE
data_len = __qmi8658_space_motion_data_read(sensor, data, len);
#elif TCFG_MPU6050_EN
data_len = __mpu6050_space_motion_data_read(sensor, data, len);
#endif
#if (TCFG_SENSOR_DATA_EXPORT_ENABLE == SENSOR_DATA_EXPORT_USE_UART)
int wlen = cbuf_write(&ctx->data_cbuf, data, data_len);
if (cbuf_get_data_size(&ctx->data_cbuf) >= 360) {
u8 tmp_buf[512];
cbuf_read(&ctx->data_cbuf, tmp_buf, 360);
aec_uart_fill(0, tmp_buf, 360);
aec_uart_write();
putchar('|');
}
#elif (TCFG_SENSOR_DATA_EXPORT_ENABLE == SENSOR_DATA_EXPORT_USE_SPP)
extern int audio_data_export_run(u8 ch, u8 * data, int len);
audio_data_export_run(0, data, data_len);
#endif /*TCFG_SENSOR_DATA_EXPORT_ENABLE*/
return data_len;
}
void *space_motion_detect_open(void)
{
struct space_data_context *ctx = NULL;
/*打开传感器电源*/
imu_sensor_power_ctl(TCFG_IMU_SENSOR_PWR_PORT, 1);
os_time_dly(1);
#if TCFG_ICM42670P_ENABLE
ctx = __icm42670p_space_motion_detect_open();
#elif TCFG_LSM6DSL_ENABLE
ctx = __lsm6dsl_space_motion_detect_open();
#elif TCFG_MPU6887P_ENABLE
ctx = __mpu6887p_space_motion_detect_open();
#elif TCFG_QMI8658_ENABLE
ctx = __qmi8658_space_motion_detect_open();
#elif TCFG_MPU6050_EN
ctx = __mpu6050_space_motion_detect_open();
#endif
if (ctx) {
#if (TCFG_SENSOR_DATA_EXPORT_ENABLE == SENSOR_DATA_EXPORT_USE_UART)
cbuf_init(&ctx->data_cbuf, ctx->cbuf, sizeof(ctx->cbuf));
aec_uart_open(1, 360);
#endif /*TCFG_SENSOR_DATA_EXPORT_ENABLE*/
}
return ctx;
}
void space_motion_detect_close(void *sensor)
{
if (!sensor) {
return;
}
#if TCFG_ICM42670P_ENABLE
__icm42670p_space_motion_detect_close(sensor);
#elif TCFG_LSM6DSL_ENABLE
__lsm6dsl_space_motion_detect_close(sensor);
#elif TCFG_MPU6887P_ENABLE
__mpu6887p_space_motion_detect_close(sensor);
#elif TCFG_QMI8658_ENABLE
__qmi8658_space_motion_detect_close(sensor);
#elif TCFG_MPU6050_EN
__mpu6050_space_motion_detect_close(sensor);
#endif
/*关闭传感器电源*/
/* imu_sensor_power_ctl(TCFG_IMU_SENSOR_PWR_PORT, 0); */
#if (TCFG_SENSOR_DATA_EXPORT_ENABLE == SENSOR_DATA_EXPORT_USE_UART)
aec_uart_close();
#endif /*TCFG_SENSOR_DATA_EXPORT_ENABLE*/
}
void imu_sensor_ad0_selete(u32 gpio, u8 value)
{
#if (TCFG_MPU6887P_AD0_SELETE_IO != NO_CONFIG_PORT) || \
(TCFG_QMI8658_AD0_SELETE_IO != NO_CONFIG_PORT)
gpio_set_die(gpio, 1);
gpio_set_pull_up(gpio, 0);
gpio_set_pull_down(gpio, 0);
gpio_direction_output(gpio, value);
#endif
}
void imu_sensor_power_ctl(u32 gpio, u8 value)
{
#if (TCFG_IMU_SENSOR_PWR_PORT != NO_CONFIG_PORT)
gpio_set_die(gpio, 1);
gpio_set_pull_up(gpio, 0);
gpio_set_pull_down(gpio, 0);
gpio_direction_output(gpio, value);
#endif
}
void imu_sensor_test(void)
{
#if 0
#include "imu_sensor/imuSensor_manage.h"
#include "imu_sensor/mpu6887/mpu6887p.h"
#include "imu_sensor/lsm6dsl/lsm6dsl.h"
/* u8 name_test[20] = "mpu6887p"; */
/* u8 name_test[20] = "lsm6dsl"; */
u8 name_test[20] = "icm42670p";
int arg_data = 0;
int ret = 0;
//获取传感器状态
ret = imu_sensor_io_ctl(name_test, IMU_GET_SENSOR_STATUS, &arg_data);
printf("sta : %d\n", arg_data);
if ((arg_data & 0x01) == 0) {
//如果还没有初始化，重新初始化
imu_sensor_io_ctl(name_test, IMU_SENSOR_ENABLE, &arg_data);
}
//检查传感器id
ret = imu_sensor_io_ctl(name_test, IMU_SENSOR_SEARCH, &arg_data);
extern int swap_data_endian(s16 * data, int len);
extern int fifo_data_delete_temp(u8 * buf, int len);
//关闭温度传感器
arg_data = 1;
/* ret = imu_sensor_io_ctl(name_test, IMU_SET_SENSOR_TEMP_DISABLE, &arg_data); */
//设置fifo使能
u8 mytmp[5];
mytmp[0] = (1000 & 0xff);
mytmp[1] = (1000 >> 8) & 0xff;
mytmp[2] = 1;
mytmp[3] = 1;
mytmp[4] = 1;
/* ret = imu_sensor_io_ctl(name_test, IMU_SET_SENSOR_FIFO_CONFIG, &mytmp); */
s16 s_data[1024];
int data_len = 0;
//读取六轴数据
printf("lsm6dsl_read_raw_acc_gyro_xyz");
/* data_len = imu_sensor_io_ctl(name_test, IMU_SENSOR_READ_DATA, s_data); */
//d读取fifo数据
u8 cnt = 10;
while (cnt--) {
printf("lsm6dsl_read_fifo");
data_len = imu_sensor_io_ctl(name_test, IMU_GET_SENSOR_READ_FIFO, s_data);
printf("data_len: %d", data_len);
if (data_len >= 12) {
/* swap_data_endian((s16 *)s_data, data_len); */
/* data_len = fifo_data_delete_temp((u8 *)s_data, data_len); */
for (int i = 0; i < data_len / 6; i = i + 6) {
printf("%d, %d, %d, %d, %d, %d\n", s_data[i], s_data[i + 1], s_data[i + 2], s_data[i + 3], s_data[i + 4], s_data[i + 5]);
}
}
os_time_dly(50);//500ms
}
#endif
}

15
cpu/br28/spatial_effect/spatial_effect_imu.h Normal file
View File

@ -0,0 +1,15 @@
/*****************************************************************
>file name : spatial_effect_imu.h
>create time : Mon 03 Jan 2022 02:44:09 PM CST
*****************************************************************/
#ifndef _SPATIAL_EFFECT_IMU_H_
#define _SPATIAL_EFFECT_IMU_H_
void *space_motion_detect_open(void);
void space_motion_detect_close(void *sensor);
int space_motion_data_read(void *sensor, void *data, int len);
#endif/*_SPATIAL_EFFECT_IMU_H_*/

70
cpu/br28/spatial_effect/spatial_effect_test.c Normal file
View File

@ -0,0 +1,70 @@
/*****************************************************************
>file name : spatial_audio_test.c
>create time : Thu 19 May 2022 03:47:18 PM CST
*****************************************************************/
#include "app_config.h"
#include "tone_player.h"
#if TCFG_AUDIO_SPATIAL_EFFECT_ENABLE
extern int a2dp_spatial_audio_open(void);
extern void a2dp_spatial_audio_close(void);
extern void a2dp_spatial_audio_head_tracked_en(u8 en);
extern void a2dp_spatial_audio_setup(u8 en, u8 head_track);
extern void spatial_audio_set_effect_mode(int mode);
static int test_mode = 0;
static u8 first_in = 1;
int a2dp_spatial_audio_test(void)
{
if (++test_mode > 4) {
test_mode = 0;
}
if (first_in) {
first_in = 0;
test_mode = 0;
}
spatial_audio_set_effect_mode(test_mode);
if (test_mode == 0) {
printf("SpatialAudio:open_fixed");
a2dp_spatial_audio_setup(1, 0);
//tone_play_index(IDEX_TONE_SA_FIXED,1);
tone_play_index(IDEX_TONE_NORMAL, 1);
} else if (test_mode == 1) {
printf("SpatialAudio:open_tracked");
a2dp_spatial_audio_setup(1, 1);
tone_play_index(IDEX_TONE_NORMAL, 1);
}
else if (test_mode == 2) {
printf("SpatialAudio:open_tracked");
a2dp_spatial_audio_setup(1, 0);
tone_play_index(IDEX_TONE_NORMAL, 1);
} else if (test_mode == 3) {
printf("SpatialAudio:open_tracked");
a2dp_spatial_audio_setup(1, 1);
//tone_play_index(IDEX_TONE_SA_TRACKED,1);
tone_play_index(IDEX_TONE_NORMAL, 1);
}
/*
else if (test_mode == 4){
printf("SpatialAudio:open_tracked");
a2dp_spatial_audio_setup(1,0);
tone_play_index(IDEX_TONE_NUM_4,1);
} else if (test_mode == 5){
printf("SpatialAudio:open_tracked");
a2dp_spatial_audio_setup(1,1);
tone_play_index(IDEX_TONE_NUM_5,1);
}
*/
else {
printf("SpatialAudio:close");
a2dp_spatial_audio_setup(0, 0);
tone_play_index(IDEX_TONE_NORMAL, 1);
}
return 0;
}
#endif/*TCFG_AUDIO_SPATIAL_EFFECT_ENABLE*/

104
cpu/br28/spatial_effect/spatial_effect_tws.c Normal file
View File

@ -0,0 +1,104 @@
/*****************************************************************
>file name : spatial_tws.c
>create time : Fri 08 Apr 2022 11:48:16 AM CST
*****************************************************************/
#include "spatial_effect_tws.h"
#include "app_config.h"
extern int CONFIG_BTCTLER_TWS_ENABLE;
struct spatial_tws_connection {
u16 period;
u32 next_period;
void *priv;
void (*data_handler)(void *priv, void *data, int len);
};
static struct spatial_tws_connection *local_conn = NULL;
#define SPATIAL_TWS_AUDIO \
((int)(('S' + 'P' + 'A' + 'T' + 'I' + 'A' + 'L') << (2 * 8)) | \
(int)(('T' + 'W' + 'S') << (1 * 8)) | \
(int)(('A' + 'U' + 'D' + 'I' + 'O') << (0 * 8)))
static void spatial_tws_audio_handler(void *buf, u16 len, bool rx)
{
if (!CONFIG_BTCTLER_TWS_ENABLE) {
return;
}
local_irq_disable();
if (local_conn) {
local_conn->data_handler(local_conn->priv, buf, len);
}
local_irq_enable();
}
REGISTER_TWS_FUNC_STUB(spatial_tws_audio) = {
.func_id = SPATIAL_TWS_AUDIO,
.func = spatial_tws_audio_handler,
};
void *spatial_tws_create_connection(int period, void *priv, void (*handler)(void *, void *data, int len))
{
if (!CONFIG_BTCTLER_TWS_ENABLE) {
return NULL;
}
struct spatial_tws_connection *conn = (struct spatial_tws_connection *)zalloc(sizeof(struct spatial_tws_connection));
if (!conn) {
return NULL;
}
conn->period = period;
conn->priv = priv;
conn->data_handler = handler;
conn->next_period = jiffies;
local_conn = conn;
return conn;
}
void spatial_tws_delete_connection(void *conn)
{
if (!CONFIG_BTCTLER_TWS_ENABLE) {
return;
}
local_irq_disable();
local_conn = NULL;
local_irq_enable();
if (conn) {
free(conn);
}
}
int spatial_tws_audio_data_sync(void *_conn, void *buf, int len)
{
if (!CONFIG_BTCTLER_TWS_ENABLE) {
return 0;
}
struct spatial_tws_connection *conn = (struct spatial_tws_connection *)_conn;
if (!conn) {
return 0;
}
if (!(tws_api_get_tws_state() & TWS_STA_SIBLING_CONNECTED)) {
conn->data_handler(conn->priv, buf, len);
return len;
}
if (time_after(jiffies, conn->next_period)) {
conn->next_period = jiffies + msecs_to_jiffies(conn->period);
#if ((defined TCFG_TWS_SPATIAL_AUDIO_AS_CHANNEL) && (TCFG_TWS_SPATIAL_AUDIO_AS_CHANNEL == 'L' || TCFG_TWS_SPATIAL_AUDIO_AS_CHANNEL == 'R'))
if (tws_api_get_local_channel() == TCFG_TWS_SPATIAL_AUDIO_AS_CHANNEL) {
#else
if (tws_api_get_role() == TWS_ROLE_MASTER) {
#endif
int err = tws_api_send_data_to_sibling(buf, len, SPATIAL_TWS_AUDIO);
if (err) {
return 0;
}
}
}
return len;
}

17
cpu/br28/spatial_effect/spatial_effect_tws.h Normal file
View File

@ -0,0 +1,17 @@
/*****************************************************************
>file name : spatial_effect_tws.h
>create time : Fri 08 Apr 2022 07:55:01 PM CST
*****************************************************************/
#ifndef _SPATIAL_EFFECT_TWS_H_
#define _SPATIAL_EFFECT_TWS_H_
#include "system/includes.h"
#include "classic/tws_api.h"
void *spatial_tws_create_connection(int period, void *priv, void (*handler)(void *, void *data, int len));
void spatial_tws_delete_connection(void *conn);
int spatial_tws_audio_data_sync(void *conn, void *buf, int len);
#endif/*_SPATIAL_EFFECT_TWS_H_*/

418
cpu/br28/spatial_effect/spatial_imu_trim.c Normal file
View File

@ -0,0 +1,418 @@
#include "spatial_imu_trim.h"
#include "spatial_effect_imu.h"
/* #include "imuSensor_manage.h" */
#include "tech_lib/SpatialAudio_api.h"
#include "tech_lib/SensorCalib_api.h"
#include "system/includes.h"
#include "asm/timer.h"
#include "app_config.h"
#include "task.h"
#include "event.h"
#if TCFG_AUDIO_SPATIAL_EFFECT_ENABLE
/*校准时间配置*/
#define IMU_TRIM_TIME 10
#if TCFG_MPU6887P_ENABLE
/*icm42607p*/
static info_spa_t info_spa = {
.fs = 99.f,
.len = 6,
.sensitivity = 16.4f,
.range = 16,//+-16g量程
};
static spatial_config_t conf = {
.beta = 0.1f,
.cel_val = 0.14f,
.SerialTime = 0.2f,
.time = 1.0f,
.val = 0.02f,
.sensval = 0.01f,
};
#elif TCFG_MPU6887P_ENABLE
/*mpu6887*/
static info_spa_t info_spa = {
.fs = 100.f,
.len = 6,
.sensitivity = 16.4f,
.range = 16,//+-16g量程
};
static spatial_config_t conf = {
.beta = 0.1f,
.cel_val = 0.08f,
.SerialTime = 0.2f,
.time = 1.0f,
.val = 0.02f,
.sensval = 0.01f,
};
#else
/*lsm6dsl*/
static info_spa_t info_spa = {
.fs = 104.f,
.len = 6,
.sensitivity = 14.28f,
.range = 16,
};
static spatial_config_t conf = {
.beta = 0.1f,
.cel_val = 0.12f,
.SerialTime = 0.24f,
.time = 1.f,
.val = 0.02f,
.sensval = 0.01f
};
#endif
static Thval_t Thval = {
.thv1 = 0.85f,
.thv2 = 1.2f,
};
//以上参数取值以实际为例
typedef struct {
acc_cel_t acc_cel;
char *Accbuf;
gyro_cel_t gyro_cel;
char *Gyrobuf;
tranval_t tranval;
s16 in_buf[512];
void *sensor;
} imu_trim_hdl_t;
static imu_trim_hdl_t *imu_trim_hdl = NULL;
/*获取校准传感器的状态
* 0：未校准
* 1：校准成功
* -1：校准中
* -2：校准失败
*/
static int global_imu_trim_state = 0;
int get_imu_trim_state()
{
return global_imu_trim_state;
}
typedef struct {
u8 cmd; //命令
u8 magic_code[4]; //"csmr"
u8 data_len; //LTV结构总长度
u8 len0; //L : 数据长度
u8 type0; //T : 数据类型
u8 diaplay[20]; //V : 数据内容
u8 len1; //L : 数据长度
u8 type1; //T : 数据类型
u8 notice; //V : 数据内容
} testbox_data_hdl_t;
static testbox_data_hdl_t testbox_data = {
.cmd = 0x01,
/* .magic_code[] = "csmr", */
.data_len = 25,
.len0 = 21,
.type0 = 0x01, //显示内容
/* .diaplay[20] = " ", */
.len1 = 2,
.type1 = 0x02, //提示音
.notice = 0,
};
static u8 notice_flag = 1;
int testbox_imu_trim_run(u8 *send_buf)
{
int state = get_imu_trim_state();
char csmr[] = "csmr";
char imu_trim_start[] = " trim start !";
char imu_trim_fail[] = " trim fail !";
char imu_trim_succ[] = " trim succ !";
char imu_trim_run[] = " trim run...";
/* printf("state %d", state); */
memcpy(testbox_data.magic_code, csmr, 4);
if (state == 0) {
/*还没有校准时，校准一遍*/
spatial_imu_trim_start();
memcpy(testbox_data.diaplay, imu_trim_start, sizeof(imu_trim_start));
testbox_data.notice = 1; //开始校准 ‘di’一声
} else if (state == 1) {
/*校准成功了，不再做校准了*/
memcpy(testbox_data.diaplay, imu_trim_succ, sizeof(imu_trim_succ));
if (notice_flag) {
notice_flag = 0;
testbox_data.notice = 2; //校准成功 ‘didi’两声
} else {
testbox_data.notice = 0;
}
} else if (state == -1) {
/*正在校准中*/
putchar('.');
memcpy(testbox_data.diaplay, imu_trim_run, sizeof(imu_trim_run));
testbox_data.notice = 0;
} else if (state == -2) {
/*校准失败*/
memcpy(testbox_data.diaplay, imu_trim_fail, sizeof(imu_trim_fail));
if (notice_flag) {
notice_flag = 0;
testbox_data.notice = 3; //校准失败 ‘dididi’三声
} else {
testbox_data.notice = 0;
}
}
memcpy(&send_buf[2], &testbox_data, sizeof(testbox_data_hdl_t));
return sizeof(testbox_data_hdl_t);
}
void testbox_trim_flag_reset()
{
global_imu_trim_state = 0;
notice_flag = 1;
}
/*校准资源初始化*/
static int spatial_imu_trim_init()
{
printf("%sn", __func__);
if (imu_trim_hdl) {
printf("imu trim is alreadly init !!!");
return -1;
}
imu_trim_hdl = zalloc(sizeof(imu_trim_hdl_t));
if (imu_trim_hdl == NULL) {
printf("imu_trim_hdl zalloc fail !!!");
return -1;
}
int Accsize = AccCelBuff();//加速度计校准buf大小
printf("AccCelBuff size %d", Accsize);
imu_trim_hdl->Accbuf = (char *)zalloc(Accsize);
AccCelInit(imu_trim_hdl->Accbuf, &info_spa, IMU_TRIM_TIME);//加速度计校准参数初始化
int Gyrosize = GroCelBuff();//陀螺仪计校准buf大小
printf("GroCelBuff size %d", Accsize);
imu_trim_hdl->Gyrobuf = (char *)zalloc(Gyrosize);
GroCelInit(imu_trim_hdl->Gyrobuf, &info_spa, IMU_TRIM_TIME);//校准参数初始化
/*imu初始化*/
if (imu_trim_hdl->sensor == NULL) {
imu_trim_hdl->sensor = space_motion_detect_open();
}
if (imu_trim_hdl->sensor == NULL) {
printf("sensor open fail !!!");
return -1;
}
global_imu_trim_state = -1;//校准中
return 0;
}
/*重启传感器，等待数据稳定 mpu6887p*/
static int spatial_imu_restart()
{
if (imu_trim_hdl) {
if (imu_trim_hdl->sensor) {
os_time_dly(10);
space_motion_detect_close(imu_trim_hdl->sensor);
imu_trim_hdl->sensor = NULL;
}
/*imu初始化*/
if (imu_trim_hdl->sensor == NULL) {
os_time_dly(10);
imu_trim_hdl->sensor = space_motion_detect_open();
}
}
return 0;
}
/*释放校准资源*/
static int spatial_imu_trim_exit()
{
printf("%s\n", __func__);
if (imu_trim_hdl) {
if (imu_trim_hdl->Accbuf) {
free(imu_trim_hdl->Accbuf);
imu_trim_hdl->Accbuf = NULL;
}
if (imu_trim_hdl->Gyrobuf) {
free(imu_trim_hdl->Gyrobuf);
imu_trim_hdl->Gyrobuf = NULL;
}
if (imu_trim_hdl->sensor) {
space_motion_detect_close(imu_trim_hdl->sensor);
imu_trim_hdl->sensor = NULL;
}
free(imu_trim_hdl);
imu_trim_hdl = NULL;
}
return 0;
}
extern void put_float(double fv);
/*校准传感器*/
static int spatial_imu_trim_acc_gyro()
{
printf("%s\n", __func__);
if (imu_trim_hdl == NULL) {
return 0;
}
int data_len = 0;
int i = 0;
int flag_A = 0;
int flag_G = 0;
int mode = 0;
global_imu_trim_state = -1;//校准中
do {
i = 0;
os_time_dly(10);
data_len = space_motion_data_read(imu_trim_hdl->sensor, imu_trim_hdl->in_buf, 1024);
/* printf("data_len %d", data_len); */
putchar('.');
while (data_len) {
data_len -= 12;
flag_A = Acc_Calibration(imu_trim_hdl->Accbuf,
&imu_trim_hdl->in_buf[i],
&info_spa,
&imu_trim_hdl->acc_cel,
&Thval);
if (flag_A == 1) {
printf("imu_trim_acc succ!!!");
printf("acc_cel:x:");
put_float(imu_trim_hdl->acc_cel.acc_offx);
printf("acc_cel:y:");
put_float(imu_trim_hdl->acc_cel.acc_offy);
printf("acc_cel:z:");
put_float(imu_trim_hdl->acc_cel.acc_offz);
int ret = syscfg_write(CFG_IMU_ACC_OFFEST_ID, &imu_trim_hdl->acc_cel, sizeof(acc_cel_t));
if (ret != sizeof(acc_cel_t)) {
printf("acc_cel write vm fail %d !!!", ret);
}
/* return 0; */
} else if (flag_A == -2) {
printf("imu_trim_acc fail!!!");
global_imu_trim_state = -2;//校准失败
return -2;
}
flag_G = Gro_Calibration(imu_trim_hdl->Gyrobuf,
&imu_trim_hdl->in_buf[i],
&info_spa,
&imu_trim_hdl->tranval,
mode,
&imu_trim_hdl->gyro_cel);//计算mode=0；完成陀螺仪校准，完成返回0，否则返回-1
if (flag_G == mode) {
printf("imu_trim_gyro succ!!!");
printf("gyro_cel:x:");
put_float(imu_trim_hdl->gyro_cel.gyro_x);
printf("gyro_cel:y:");
put_float(imu_trim_hdl->gyro_cel.gyro_y);
printf("gyro_cel:z:");
put_float(imu_trim_hdl->gyro_cel.gyro_z);
int ret = syscfg_write(CFG_IMU_GYRO_OFFEST_ID, &imu_trim_hdl->gyro_cel, sizeof(gyro_cel_t));
if (ret != sizeof(gyro_cel_t)) {
printf("gyro_cel write vm fail %d !!!", ret);
}
global_imu_trim_state = 1;//校准成功
return 0;
} else if (flag_G == -2) {
printf("imu_trim_gyro fail!!!");
global_imu_trim_state = -2;//校准失败
return -2;
}
global_imu_trim_state = -1;//校准中
i += 6;
}
} while (1);
return 0;
}
/*开始校准*/
int spatial_imu_trim_start()
{
struct sys_event e;
e.type = SYS_AUD_EVENT;
e.arg = (void *)AUDIO_EVENT_TRIM_IMU_START;
sys_event_notify(&e);
}
/*关闭校准*/
int spatial_imu_trim_stop()
{
struct sys_event e;
e.type = SYS_AUD_EVENT;
e.arg = (void *)AUDIO_EVENT_TRIM_IMU_STOP;
sys_event_notify(&e);
}
/*校准任务*/
static void spatial_imu_trim_task(void *p)
{
int ret = 0;
/*初始化传感器*/
ret = spatial_imu_trim_init();
if (ret != 0) {
global_imu_trim_state = -2;//校准失败
goto err0;
}
/*等待传感器数据稳定*/
spatial_imu_restart();
/*校准传感器*/
ret = spatial_imu_trim_acc_gyro();
/*判断是否校准成功，0：校准成功，-2：校准失败*/
if (ret == -2) {
global_imu_trim_state = -2;//校准失败
}
err0 :
/*关闭传感器*/
spatial_imu_trim_exit();
/*关闭校准任务*/
spatial_imu_trim_stop();
}
/*创建校准任务*/
int spatial_imu_trim_task_start()
{
int err = 0;
err = task_create(spatial_imu_trim_task, imu_trim_hdl, "imu_trim");
if (err != OS_NO_ERR) {
printf("imu_trim task create fail : %d !!!", err);
}
return err;
}
/*删除校准任务和校准资源*/
int spatial_imu_trim_task_stop()
{
int err = 0;
err = task_kill("imu_trim");
if (err != OS_NO_ERR) {
printf("imu_trim task kill fail : %d !!!", err);
}
/*释放校准资源*/
spatial_imu_trim_exit();
return err;
}
static u8 spatial_imu_trim_idle_query(void)
{
return ((imu_trim_hdl == NULL) ? 1 : 0);
}
REGISTER_LP_TARGET(spatial_imu_trim_lp_target) = {
.name = "spatial_imu_trim",
.is_idle = spatial_imu_trim_idle_query,
};
#endif /*TCFG_AUDIO_SPATIAL_EFFECT_ENABLE*/

18
cpu/br28/spatial_effect/spatial_imu_trim.h Normal file
View File

@ -0,0 +1,18 @@
#ifndef _SPATIAL_IMU_TRIM_
#define _SPATIAL_IMU_TRIM_
#include "generic/typedef.h"
/*开始校准*/
int spatial_imu_trim_start();
/*关闭校准*/
int spatial_imu_trim_stop();
/*创建校准任务*/
int spatial_imu_trim_task_start();
/*删除校准任务和校准资源*/
int spatial_imu_trim_task_stop();
#endif