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

修改了六轴配置

Browse Source
This commit is contained in:
lmx
2025-11-11 19:31:34 +08:00
parent 23a71377a2
commit 58ad14691e
37 changed files with 172314 additions and 171919 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
apps/earphone/board/br28/board_jl701n_demo.c
View File

@ -520,7 +520,8 @@ const struct hw_iic_config hw_iic_cfg[] = {
{IO_PORTC_02, IO_PORTC_03}, //group c
{IO_PORTA_05, IO_PORTA_06}, //group d
*/
.port = TCFG_HW_I2C0_PORTS,
// .port = TCFG_HW_I2C0_PORTS,
.port = {IO_PORTC_04,IO_PORTC_05}, // portB: scl、sda
.baudrate = TCFG_HW_I2C0_CLK, //IIC通讯波特率
.hdrive = 0, //是否打开IO口强驱
.io_filter = 1, //是否打开滤波器（去纹波）

6
apps/earphone/board/br28/board_jl701n_demo_cfg.h
View File

@ -34,14 +34,14 @@
#define TCFG_SW_I2C0_DAT_PORT IO_PORTA_06 //软件IIC DAT脚选择
#define TCFG_SW_I2C0_DELAY_CNT 10 //IIC延时参数，影响通讯时钟频率
/*硬件IIC端口选择
/*硬件IIC端口选择 -- 具体看手册，这里写的不准 -- lmx
SCL SDA
'A': IO_PORT_DP IO_PORT_DM
'B': IO_PORTA_09 IO_PORTA_10
'C': IO_PORTA_07 IO_PORTA_08
'D': IO_PORTA_05 IO_PORTA_06
*/
#define TCFG_HW_I2C0_PORTS 'D'
#define TCFG_HW_I2C0_PORTS 'B'
#define TCFG_HW_I2C0_CLK 100000 //硬件IIC波特率
//*********************************************************************************//
@ -915,7 +915,7 @@ DAC硬件上的连接方式,可选的配置：
#define TCFG_STK8321_EN 0
#define TCFG_IRSENSOR_ENABLE 0
#define TCFG_JSA1221_ENABLE 0
#define TCFG_GSENOR_USER_IIC_TYPE 0 //0:软件IIC 1:硬件IIC
#define TCFG_GSENOR_USER_IIC_TYPE 1 //0:软件IIC 1:硬件IIC
//*********************************************************************************//
// imu-sensor配置 //

2
apps/earphone/xtell_Sensor/README.md
View File

@ -1,4 +1,4 @@
# 时间间隔
# 时间间隔 -- 软件模拟iic的情况下
目前测试代码如下：

123
apps/earphone/xtell_Sensor/buffer/circle_buffer.c
View File

@ -1,117 +1,60 @@
#include "circle_buffer.h"
#include <string.h>
//////////////////////////////////////////////////////////////////////////////////////////////////
//START -- 宏定义
#define ENABLE_XLOG 1
#ifdef xlog
#undef xlog
#endif
#if ENABLE_XLOG
#define xlog(format, ...) printf("[%s] " format, __func__, ##__VA_ARGS__)
#else
#define xlog(format, ...) ((void)0)
#endif
//END -- 宏定义
//////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//START -- 变量定义
//END -- 变量定义
//////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//START -- 函数定义
//END -- 函数定义
//////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////
//实现
// 初始化环形缓冲区
void circle_buffer_init(circle_buffer_t *cb, u8 *buffer, u16 capacity) {
cb->buffer = buffer;
void circle_buffer_init(circle_buffer_t *cb, void *buffer, u16 capacity, u16 element_size) {
cb->buffer = (u8 *)buffer;
cb->capacity = capacity;
cb->element_size = element_size;
cb->head = 0;
cb->tail = 0;
cb->size = 0;
}
// 向环形缓冲区写入数据
u16 circle_buffer_write(circle_buffer_t *cb, const u8 *data, u16 length) {
if (length > circle_buffer_get_free_space(cb)) {
// 如果剩余空间不足，则只写入能放下的部分
length = circle_buffer_get_free_space(cb);
// 向环形缓冲区写入一个元素
bool circle_buffer_write(circle_buffer_t *cb, const void *element) {
if (circle_buffer_is_full(cb)) {
return false; // 缓冲区已满
}
if (length == 0) {
return 0;
}
u8 *dest = cb->buffer + (cb->head * cb->element_size);
memcpy(dest, element, cb->element_size);
// 检查是否需要回环
if (cb->head + length > cb->capacity) {
u16 part1_len = cb->capacity - cb->head;
u16 part2_len = length - part1_len;
memcpy(cb->buffer + cb->head, data, part1_len);
memcpy(cb->buffer, data + part1_len, part2_len);
cb->head = part2_len;
} else {
memcpy(cb->buffer + cb->head, data, length);
cb->head += length;
if (cb->head == cb->capacity) {
cb->head = 0;
}
}
cb->size += length;
return length;
cb->head = (cb->head + 1) % cb->capacity;
cb->size++;
return true;
}
// 从环形缓冲区读取数据
u16 circle_buffer_read(circle_buffer_t *cb, u8 *data, u16 length) {
if (length > cb->size) {
// 如果要读取的长度超过了已有的数据，则只读取已有的部分
length = cb->size;
// 从环形缓冲区读取一个元素
bool circle_buffer_read(circle_buffer_t *cb, void *element) {
if (circle_buffer_is_empty(cb)) {
return false; // 缓冲区为空
}
if (length == 0) {
return 0;
}
u8 *src = cb->buffer + (cb->tail * cb->element_size);
memcpy(element, src, cb->element_size);
// 检查是否需要回环
if (cb->tail + length > cb->capacity) {
u16 part1_len = cb->capacity - cb->tail;
u16 part2_len = length - part1_len;
memcpy(data, cb->buffer + cb->tail, part1_len);
memcpy(data + part1_len, cb->buffer, part2_len);
cb->tail = part2_len;
} else {
memcpy(data, cb->buffer + cb->tail, length);
cb->tail += length;
if (cb->tail == cb->capacity) {
cb->tail = 0;
}
}
cb->size -= length;
return length;
cb->tail = (cb->tail + 1) % cb->capacity;
cb->size--;
return true;
}
// 获取已用空间的大小
// 获取已用空间的大小（以元素为单位）
u16 circle_buffer_get_size(circle_buffer_t *cb) {
return cb->size;
}
// 获取剩余空间的大小
// 获取剩余空间的大小（以元素为单位）
u16 circle_buffer_get_free_space(circle_buffer_t *cb) {
return cb->capacity - cb->size;
}
// 检查缓冲区是否已满
bool circle_buffer_is_full(circle_buffer_t *cb) {
return cb->size == cb->capacity;
}
// 检查缓冲区是否为空
bool circle_buffer_is_empty(circle_buffer_t *cb) {
return cb->size == 0;
}

57
apps/earphone/xtell_Sensor/buffer/circle_buffer.h
View File

@ -5,51 +5,66 @@
// 定义环形缓冲区的结构体
typedef struct {
u8 *buffer; // 缓冲区指针
u16 capacity; // 缓冲区总容量
u16 head; // 头部指针（写入位置）
u16 tail; // 尾部指针（读取位置）
u16 size; // 当前已用大小
u8 *buffer; // 缓冲区指针
u16 capacity; // 缓冲区总容量（以元素为单位）
u16 element_size; // 每个元素的大小（以字节为单位）
u16 head; // 头部指针（写入位置，以元素为单位）
u16 tail; // 尾部指针（读取位置，以元素为单位）
u16 size; // 当前已用大小（以元素为单位）
} circle_buffer_t;
/**
* @brief 初始化环形缓冲区
* @param cb 指向环形缓冲区结构体的指针
* @param buffer 外部提供的缓冲区内存
* @param capacity 缓冲区的总容量
* @param capacity 缓冲区的总容量（以元素数量为单位）
* @param element_size 每个元素的大小（字节）
*/
void circle_buffer_init(circle_buffer_t *cb, u8 *buffer, u16 capacity);
void circle_buffer_init(circle_buffer_t *cb, void *buffer, u16 capacity, u16 element_size);
/**
* @brief 向环形缓冲区写入数据
* @brief 向环形缓冲区写入一个元素
* @param cb 指向环形缓冲区结构体的指针
* @param data 要写入的数据的指针
* @param length 要写入的数据的长度
* @return 实际写入的字节数
* @param element 要写入的元素的指针
* @return 成功返回true，失败返回false
*/
u16 circle_buffer_write(circle_buffer_t *cb, const u8 *data, u16 length);
bool circle_buffer_write(circle_buffer_t *cb, const void *element);
/**
* @brief 从环形缓冲区读取数据
* @brief 从环形缓冲区读取一个元素
* @param cb 指向环形缓冲区结构体的指针
* @param data 用于存放读取数据的缓冲区的指针
* @param length 想要读取的数据的长度
* @return 实际读取的字节数
* @param element 用于存放读取元素的缓冲区的指针
* @return 成功返回true，失败返回false
*/
u16 circle_buffer_read(circle_buffer_t *cb, u8 *data, u16 length);
bool circle_buffer_read(circle_buffer_t *cb, void *element);
/**
* @brief 获取环形缓冲区中已用空间的大小
* @brief 获取环形缓冲区中已用空间的大小（以元素为单位）
* @param cb 指向环形缓冲区结构体的指针
* @return 已用空间的大小
* @return 已用空间的大小（元素数量）
*/
u16 circle_buffer_get_size(circle_buffer_t *cb);
/**
* @brief 获取环形缓冲区中剩余空间的大小
* @brief 获取环形缓冲区中剩余空间的大小（以元素为单位）
* @param cb 指向环形缓冲区结构体的指针
* @return 剩余空间的大小
* @return 剩余空间的大小（元素数量）
*/
u16 circle_buffer_get_free_space(circle_buffer_t *cb);
/**
* @brief 检查缓冲区是否已满
* @param cb 指向环形缓冲区结构体的指针
* @return 如果已满返回true，否则返回false
*/
bool circle_buffer_is_full(circle_buffer_t *cb);
/**
* @brief 检查缓冲区是否为空
* @param cb 指向环形缓冲区结构体的指针
* @return 如果为空返回true，否则返回false
*/
bool circle_buffer_is_empty(circle_buffer_t *cb);
#endif // CIRCLE_BUFFER_H

0
apps/earphone/xtell_Sensor/calculate/read_send_data.c
View File

92
apps/earphone/xtell_Sensor/calculate/skiing_tracker.c
View File

@ -22,13 +22,13 @@
// 加速度模长与重力的差值大于此值，认为开始运动；降低阈值，让“油门”更灵敏，以便能捕捉到真实的慢速启动
#define START_SKIING_ACC_THRESHOLD 0.5f
// 陀螺仪方差阈值，以允许启动瞬间的正常抖动，但仍能过滤掉混乱的、非滑雪的晃动。
#define SKIING_GYR_VARIANCE_THRESHOLD 15.0f
#define SKIING_GYR_VARIANCE_THRESHOLD 20.0f
// --- 滑雪过程 ---
//加速度 模长，低于此值视为 在做匀速运动
#define SKIING_ACC_MAG_THRESHOLD 0.5f
//陀螺仪 模长，高于此值视为 摔倒了
#define FALLEN_GRY_MAG_THRESHOLD 1000.0f //未确定
#define FALLEN_GRY_MAG_THRESHOLD 2000.0f //未确定
// --- 原地旋转抖动 ---
// 用于原地旋转判断的加速度方差阈值。此值比ZUPT阈值更宽松，
@ -41,7 +41,7 @@
// 加速度方差阈值，大于此值，滑雪过程可能发生了急转弯
#define WHEEL_ACC_VARIANCE_THRESHOLD 7.0f
// 旋转/摆动检测阈值：角速度总模长大于此值(度/秒)，认为滑雪过程中进行急转弯
#define WHEEL_GYR_MAG_THRESHOLD 220.0f // 150.0f 到 250.0f之间进行调整
#define WHEEL_GYR_MAG_THRESHOLD 500.0f //
// --- 用于消除积分漂移的滤波器和阈值 ---
// 高通滤波器系数 (alpha)。alpha 越接近1，滤除低频(直流偏移)的效果越强，但可能滤掉真实的慢速运动。
@ -54,9 +54,10 @@
// --- 模拟摩擦力，进行速度衰减 ---
#define SPEED_ATTENUATION 1.0f //暂不模拟
BLE_KS_send_data_t KS_data;
#ifdef XTELL_TEST
BLE_KS_send_data_t KS_data;
debug_t debug1;
debug_t debug2;
#endif
@ -377,12 +378,13 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
}else if(tracker->state == CONSTANT_SPEED){ //匀速
#ifdef XTELL_TEST
debug2.acc_magnitude = acc_horizontal_mag;
#endif
//保持上次的速度不变。只更新距离
tracker->distance += tracker->speed * dt;
}else if(tracker->state == WHEEL){ //匀速
//暂时：保持上次的速度不变。只更新距离
tracker->distance += tracker->speed * dt;
}else{
// 在静止或旋转状态下，速度已经在状态机内部被清零
// 额外增加速度衰减，模拟摩擦力，进一步抑制漂移
@ -395,10 +397,10 @@ void skiing_tracker_update(skiing_tracker_t *tracker, float *acc_g, float *gyr_d
}
#if 0
/**
* @brief 传感器数据采集与处理任务，外部每10ms调用一次，如果需要更新时间间隔，也需要同步更新宏“ DELTA_TIME ”
*
* 使用到了卡尔曼
* @param acc_data_buf 三轴加速度原始数据
* @param gyr_data_buf 三轴陀螺仪原始数据
* @return BLE_send_data_t
@ -427,14 +429,14 @@ BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short
unsigned char status;
if (!calibration_done) { //第1次启动，开启零漂检测
status = SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
status = Original_SL_SC7U22_Angle_Output(1, combined_raw_data, final_angle_data, 0);
if (status == 1) {
calibration_done = 1;
printf("Sensor calibration successful! Skiing mode is active.\n");
}
} else {
// printf("Calculate the time interval =============== start\n");
status = SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
status = Original_SL_SC7U22_Angle_Output(0, combined_raw_data, final_angle_data, 0);
}
if (status == 1) {
@ -469,10 +471,11 @@ BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short
BLE_send_data.sensor_state = status;
BLE_send_data.skiing_state = my_skiing_tracker.state;
for (int i = 0; i < 3; i++) {
#ifndef XTELL_TEST
// #ifndef XTELL_TEST
BLE_send_data.acc_original[i] = (int)acc_data_buf[i];
BLE_send_data.gyr_original[i] = (int)gyr_data_buf[i];
#endif
BLE_send_data.Angle[i] = final_angle_data[i];
// #endif
#if KS_BLE
KS_data.acc_KS[i] = (int)(calibrated_acc_g[i] * G_ACCELERATION * 100); //cm/s^s
KS_data.gyr_KS_dps[i] = (int)calibrated_gyr_dps[i];
@ -498,4 +501,65 @@ BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short
// printf("Angle calculation error or calibration not finished.\n");
}
return BLE_send_data;
}
}
#else
/**
* @brief 滑雪数据计算
*
* @param acc_data_buf 传入的三轴加速度数据
* @param gyr_data_buf 传入的三轴陀螺仪数据
* @param angle_data 传入的欧若拉角数据
* @return BLE_send_data_t 要发送给蓝牙的数据
*/
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data) {
static int initialized = 0;
static float acc_data_g[3];
static float gyr_data_dps[3];
// const float delta_time = DELTA_TIME+0.01f;
// const float delta_time = DELTA_TIME + 0.005f;
const float delta_time = DELTA_TIME+0.01f;
BLE_send_data_t BLE_send_data;
if (!initialized) {
skiing_tracker_init(&my_skiing_tracker);
initialized = 1;
printf("Skiing Tracker Initialized. Waiting for sensor calibration...\n");
}
// 加速度 LSB to g
acc_data_g[0] = (float)acc_data_buf[0] / 8192.0f;
acc_data_g[1] = (float)acc_data_buf[1] / 8192.0f;
acc_data_g[2] = (float)acc_data_buf[2] / 8192.0f;
// 陀螺仪 LSB to dps (度/秒)
// ±2000dps量程下，转换系数约为 0.061
gyr_data_dps[0] = (float)gyr_data_buf[0] * 0.061f;
gyr_data_dps[1] = (float)gyr_data_buf[1] * 0.061f;
gyr_data_dps[2] = (float)gyr_data_buf[2] * 0.061f;
skiing_tracker_update(&my_skiing_tracker, acc_data_g, gyr_data_dps, angle_data, delta_time);
BLE_send_data.skiing_state = my_skiing_tracker.state;
for (int i = 0; i < 3; i++) {
#ifdef XTELL_TEST
BLE_send_data.acc_data[i] = (short)(acc_data_g[i] * 9.8f) * 100; //cm/^s2
BLE_send_data.gyr_data[i] = (short)gyr_data_dps[i]; //dps
BLE_send_data.angle_data[i] = angle_data[i];
#else
BLE_send_data.acc_data[i] = (short)acc_data_buf[i]; //原始adc数据
BLE_send_data.gyr_data[i] = (short)gyr_data_buf[i]; //原始adc数据
BLE_send_data.angle_data[i] = angle_data[i];
#endif
}
BLE_send_data.speed_cms = (int)(my_skiing_tracker.speed * 100);
BLE_send_data.distance_cm = (int)(my_skiing_tracker.distance * 100);
// printf("Calculate the time interval =============== end\n");
return BLE_send_data;
}
#endif

18
apps/earphone/xtell_Sensor/calculate/skiing_tracker.h
View File

@ -42,15 +42,14 @@ typedef struct {
} skiing_tracker_t;
//ble发送的数据
typedef struct __attribute__((packed)){ //该结构体取消内存对齐
typedef struct{ //__attribute__((packed)){ //该结构体取消内存对齐
char sensor_state;
char skiing_state;
int speed_cms; //求出的速度，cm/s
int distance_cm; //求出的距离，cm
#ifndef XTELL_TEST
int acc_original[3]; //直接读取传感器得到的原始三轴加速度
int gyr_original[3]; //直接读取传感器得到的原始三轴陀螺仪
#endif
short acc_data[3]; //三轴加速度, g
short gyr_data[3]; //三轴陀螺仪, dps
float angle_data[3]; //欧若拉角
}BLE_send_data_t;
typedef struct{
@ -74,12 +73,5 @@ typedef struct{
*/
void skiing_tracker_init(skiing_tracker_t *tracker);
/**
* @brief 传感器数据采集与处理任务，外部每10ms调用一次，如果需要更新时间间隔，也需要同步更新宏“ DELTA_TIME ”
*
* @param acc_data_buf 三轴加速度原始数据
* @param gyr_data_buf 三轴陀螺仪原始数据
* @return BLE_send_data_t
*/
BLE_send_data_t sensor_processing_task(signed short * acc_data_buf, signed short * gyr_data_buf) ;
BLE_send_data_t sensor_processing_task(signed short* acc_data_buf, signed short* gyr_data_buf, float* angle_data) ;
#endif // SKIING_TRACKER_H

184
apps/earphone/xtell_Sensor/send_data.c
View File

@ -15,9 +15,8 @@
#include "bt_profile_cfg.h"
#include "dev_manager/dev_manager.h"
#include "update_loader_download.h"
#include "LIS2DH12.h"
#include "circle_buffer.h"
#include "circle_buffer.h"
#include "./sensor/SC7U22.h"
#include "./buffer/circle_buffer.h"
#include "btstack/avctp_user.h"
#include "calculate/skiing_tracker.h"
#include "xtell.h"
@ -58,7 +57,7 @@ static u16 calculate_data_id;
static u8 sensor_data_buffer[SENSOR_DATA_BUFFER_SIZE];
static circle_buffer_t sensor_cb;
BLE_send_data_t BLE_send_data;
//--- test ---
// 全局变量
u16 gsensor_id=0;
@ -145,7 +144,8 @@ void send_sensor_data_task(void) {
// printf("xtell_ble_send\n");
}
#ifdef XTELL_TEST
#if 0
BLE_send_data_t BLE_send_data;
void test(){
signed short acc_data_buf[3] = {0};
signed short gyr_data_buf[3] = {0};
@ -240,28 +240,159 @@ void test(){
// xlog("end============\n");
}
#else
#endif
#define SENSOR_DATA_BUFFER_SIZE 100 // 定义缓冲区可以存储100个sensor_data_t元素
static circle_buffer_t sensor_read; // 环形缓冲区管理结构体
typedef struct {
signed short acc_data[3];
signed short gyr_data[3];
float angle[3];
} sensor_data_t;
static sensor_data_t sensor_read_buffer[SENSOR_DATA_BUFFER_SIZE]; // 存放sensor读到的数据
static circle_buffer_t sensor_send; // 环形缓冲区管理结构体
static BLE_send_data_t sensor_send_buffer[SENSOR_DATA_BUFFER_SIZE]; // 存放ble要发送的数据
/**
* @brief //读取传感器的数据放进缓冲区
*
*/
void sensor_read_data(){
//读取传感器的原始六轴数据放进缓冲区
// xlog("=======sensor_read_data START\n");
static signed short combined_raw_data[6];
static int initialized = 0;
static int calibration_done = 0;
char status = 0;
if(circle_buffer_is_full(&sensor_read)){
// xlog("sensor_read_data: read buffer full\n");
return;
}
static sensor_data_t tmp;
SL_SC7U22_RawData_Read(tmp.acc_data,tmp.gyr_data);
memcpy(&combined_raw_data[0], tmp.acc_data, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], tmp.gyr_data, 3 * sizeof(signed short));
if (!calibration_done) { //第1次启动，开启零漂检测
status = SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
if (status == 1) {
calibration_done = 1;
printf("Sensor calibration successful! Skiing mode is active.\n");
}
} else {
// printf("Calculate the time interval =============== start\n");
status = SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
memcpy(tmp.acc_data, &combined_raw_data[0], 3 * sizeof(signed short));
memcpy(tmp.gyr_data, &combined_raw_data[3], 3 * sizeof(signed short));
circle_buffer_write(&sensor_read, &tmp);
}
// xlog("=======sensor_read_data END\n");
}
void calculate_data(){
//从缓冲区取出原始六轴数据，进行计算得到速度和距离
//计算方面已经实现，直接调用函数就行了
// xlog("=======start\n");
sensor_data_t tmp;
if(circle_buffer_is_empty(&sensor_read)){
// xlog("sensor_read_buffer: read buffer empty\n");
return;
}
circle_buffer_read(&sensor_read, &tmp);
BLE_send_data_t data_by_calculate = sensor_processing_task(tmp.acc_data, tmp.gyr_data,tmp.angle);
if(circle_buffer_is_full(&sensor_send))
return;
circle_buffer_write(&sensor_send, &data_by_calculate);
// extern void BLE_send_data();
// BLE_send_data();
// xlog("=======end\n");
}
static int count = 0;
void BLE_send_data(){
//蓝牙发送数据：计算得到的速度、距离以及读到的原始六轴数据
if(circle_buffer_is_empty(&sensor_send)){
// xlog("sensor_send_buffer: send buffer empty\n");
return;
}
#ifdef XTELL_TEST
BLE_send_data_t tmp;
circle_buffer_read(&sensor_send, &tmp);
if(count >=50){
count = 0;
char* division = "==========\n";
send_data_to_ble_client(division,strlen(division));
char log_buffer[100]; // 100个字符应该足够了
// 使用 snprintf 进行格式化
int num_chars_written = snprintf(
log_buffer, // 目标缓冲区
sizeof(log_buffer), // 目标缓冲区的最大容量
"s %d, %dcm/s, %dcm\n", // 格式化字符串
tmp.skiing_state, // 第一个 %d 的参数
tmp.speed_cms, // 第二个 %d 的参数
tmp.distance_cm // 第三个 %d 的参数
);
send_data_to_ble_client(&log_buffer, strlen(log_buffer));
memset(&log_buffer, 0, 100);
num_chars_written = snprintf(
log_buffer,
sizeof(log_buffer),
"Acc:%d, %d, %d\n",
tmp.acc_data[0],tmp.acc_data[1],tmp.acc_data[2]
);
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
memset(&log_buffer, 0, 100);
num_chars_written = snprintf(
log_buffer,
sizeof(log_buffer),
"Gyr:%d, %d, %d\n",
tmp.gyr_data[0],tmp.gyr_data[1],tmp.gyr_data[2]
);
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
memset(&log_buffer, 0, 100);
num_chars_written = snprintf(
log_buffer,
sizeof(log_buffer),
"Angle:%.1f, %.1f, %1.f\n",
tmp.angle_data[0],tmp.angle_data[1],tmp.angle_data[2]
);
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
short acc_mo_cms = sqrtf(tmp.acc_data[0]*tmp.acc_data[0] + tmp.acc_data[1]*tmp.acc_data[1] + tmp.acc_data[2]*tmp.acc_data[2])-900;
memset(&log_buffer, 0, 100);
num_chars_written = snprintf(
log_buffer,
sizeof(log_buffer),
"acc_cm/s^2:%d\n",
acc_mo_cms
);
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
// xlog("s %d, %dcm/s, %dcm\n",tmp.skiing_state, tmp.speed_cms, tmp.distance_cm);
// xlog("Acc:%d, %d, %d\n", tmp.acc_data[0],tmp.acc_data[1],tmp.acc_data[2]);
// xlog("GYR:%d, %d, %d\n", tmp.gyr_data[0],tmp.gyr_data[1],tmp.gyr_data[2]);
}
count++;
#else
#endif
}
void xtell_task_create(void){
// int ret = hw_iic_init(0);
@ -269,18 +400,32 @@ void xtell_task_create(void){
// //初始化传感器
// SL_SC7U22_Config();
#if TCFG_GSENOR_USER_IIC_TYPE
int ret = hw_iic_init(0);
xlog("init iic result:%d\n", ret); //返回0成功
#else
soft_iic_init(0);
#endif
gpio_set_direction(IO_PORTE_05,0); //设置PE5 输出模式
gpio_set_pull_up(IO_PORTE_05,1);
gpio_direction_output(IO_PORTE_05,1);
os_time_dly(10);
SL_SC7U22_Config();
// extern u8 LIS2DH12_Config(void);
// LIS2DH12_Config();
xlog("xtell_task_create\n");
// 初始化环形缓冲区
circle_buffer_init(&sensor_cb, sensor_data_buffer, SENSOR_DATA_BUFFER_SIZE);
// circle_buffer_init(&sensor_cb, sensor_data_buffer, SENSOR_DATA_BUFFER_SIZE);
circle_buffer_init(&sensor_read, sensor_read_buffer, SENSOR_DATA_BUFFER_SIZE, sizeof(sensor_data_t));
circle_buffer_init(&sensor_send, sensor_send_buffer, SENSOR_DATA_BUFFER_SIZE, sizeof(BLE_send_data_t));
//初始化滑雪追踪器
@ -289,8 +434,11 @@ void xtell_task_create(void){
// create_process(&test_id, "test",NULL, test, (int)(DELTA_TIME*1000));
create_process(&ble_send_data_id, "send",NULL, ble_send_data, (int)(DELTA_TIME*1000));
create_process(&sensor_read_data_id, "read",NULL, sensor_read_data, (int)(DELTA_TIME*1000));
create_process(&calculate_data_id, "calculate",NULL, calculate_data, (int)(DELTA_TIME*1000));
create_process(&sensor_read_data_id, "read",NULL, sensor_read_data, 10);
create_process(&calculate_data_id, "calculate",NULL, calculate_data, 1);
create_process(&ble_send_data_id, "send",NULL, BLE_send_data, 1);
}

147
apps/earphone/xtell_Sensor/sensor/SC7U22.c
View File

@ -119,9 +119,18 @@ unsigned char SL_SC7U22_Config(void)
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7D, 0x0E);//PWR_CTRL ENABLE ACC+GYR+TEMP
os_time_dly(1);//10ms
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0x06);//ACC_CONF 0x07=50Hz 0x06=25Hz
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0x06);//ACC_CONF 0x07=50Hz 0x06=25Hz
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0xA8);//高性能模式，连续4个数据平均1次，100Hz -- lmx
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0xAC);//ACC_CON 高性能模式，1600Hz -- lmx
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x40, 0xBB);//ACC_CON 高性能模式，800Hz -- lmx
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x41, 0x01);//ACC_RANGE 10：±8G 01：±4G
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0x86);//GYR_CONF 0x87=50Hz 0x86=25Hz
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0x86);//GYR_CONF 0x87=50Hz 0x86=25Hz
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0x8C);//GYR_CONF 1600Hz -- lmx
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0xAC);//GYR_CONF 1600Hz -- lmx
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0xAB);//GYR_CONF 800Hz -- lmx
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, 0x00);//GYR_RANGE 2000dps
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, 0x00);//GYR_RANGE 2000dps
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x04, 0x50);//COM_CFG
@ -927,6 +936,140 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short
return 2; // 校准未完成，返回错误状态
}
// =================================================================================
// 关键参数定义 - 需要根据实际情况进行调整
// ---------------------------------------------------------------------------------
// 滤波系数 α (alpha)，决定了对陀螺仪和加速度计的信任程度。
// α 越大，越相信陀螺仪，动态响应越好，但对漂移的修正越慢。
// α 越小，越相信加速度计，对漂移的修正越快，但对运动加速度越敏感。
// 典型值范围：0.95 ~ 0.99
#define FILTER_ALPHA 0.98f
// 采样时间间隔 SAMPLE_INTERVAL (单位：秒)
// 必须与此函数的频率严格对应。例如，100Hz 对应 0.01f。
#define SAMPLE_INTERVAL 0.01f
// =================================================================================
/**
* @brief 不带卡尔曼的欧若拉数据输出，以及消除零点漂移后的六轴数据
*
* @param calibration_en
* @param acc_gyro_input
* @param Angle_output
* @param yaw_rst
* @return unsigned char
*/
unsigned char Original_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst){
unsigned short acc_gyro_delta[2];
unsigned char sl_i = 0;
float angle_acc[3] = {0};
float gyro_val[3] = {0};
// 如果外部强制使能校准，则将标志位置1
if (calibration_en == 0) {
SL_SC7U22_Error_Flag = 1;
}
// =================================================================================
// 步骤 1: 静态校准 (此部分完全保留，不做任何修改)
// ---------------------------------------------------------------------------------
if (SL_SC7U22_Error_Flag == 0) {
// ... (省略与原代码完全相同的部分以节省篇幅) ...
// 计算当前数据与上一帧数据的差值，用于判断是否静止
acc_gyro_delta[0] = 0;
acc_gyro_delta[1] = 0;
for (sl_i = 0; sl_i < 3; sl_i++) {
acc_gyro_delta[0] += SL_GetAbsShort(acc_gyro_input[sl_i] - Temp_Accgyro[sl_i]);
acc_gyro_delta[1] += SL_GetAbsShort(acc_gyro_input[3 + sl_i] - Temp_Accgyro[3 + sl_i]);
}
// 保存当前数据，用于下一帧比较
for (sl_i = 0; sl_i < 6; sl_i++) {
Temp_Accgyro[sl_i] = acc_gyro_input[sl_i];
}
// 判断是否处于静止状态
if ((acc_gyro_delta[0] / 8 < 160) && (acc_gyro_delta[1] < 40) && (SL_GetAbsShort(acc_gyro_input[0]) < 3000) && (SL_GetAbsShort(acc_gyro_input[1]) < 3000) && (SL_GetAbsShort(acc_gyro_input[2] - 8192) < 3000)) {
if (SL_SC7U22_Error_cnt < 200) SL_SC7U22_Error_cnt++;
} else {
SL_SC7U22_Error_cnt = 0;
}
// 如果静止时间足够长
if (SL_SC7U22_Error_cnt > 190) {
for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] += acc_gyro_input[sl_i];
SL_SC7U22_Error_cnt2++;
if (SL_SC7U22_Error_cnt2 > 49) {
SL_SC7U22_Error_Flag = 1;
SL_SC7U22_Error_cnt2 = 0;
SL_SC7U22_Error_cnt = 0;
for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] = Sum_Avg_Accgyro[sl_i] / 50;
Error_Accgyro[0] = 0 - Sum_Avg_Accgyro[0];
Error_Accgyro[1] = 0 - Sum_Avg_Accgyro[1];
Error_Accgyro[2] = 8192 - Sum_Avg_Accgyro[2];
Error_Accgyro[3] = 0 - Sum_Avg_Accgyro[3];
Error_Accgyro[4] = 0 - Sum_Avg_Accgyro[4];
Error_Accgyro[5] = 0 - Sum_Avg_Accgyro[5];
xlog("AVG_Recode AX:%d,AY:%d,AZ:%d,GX:%d,GY:%d,GZ:%d\r\n", Sum_Avg_Accgyro[0], Sum_Avg_Accgyro[1], Sum_Avg_Accgyro[2], Sum_Avg_Accgyro[3], Sum_Avg_Accgyro[4], Sum_Avg_Accgyro[5]);
xlog("Error_Recode AX:%d,AY:%d,AZ:%d,GX:%d,GY:%d,GZ:%d\r\n", Error_Accgyro[0], Error_Accgyro[1], Error_Accgyro[2], Error_Accgyro[3], Error_Accgyro[4], Error_Accgyro[5]);
}
} else {
SL_SC7U22_Error_cnt2 = 0;
for (sl_i = 0; sl_i < 6; sl_i++) Sum_Avg_Accgyro[sl_i] = 0;
}
return 0; // 返回0，表示正在校准
}
// =================================================================================
// 步骤 2: 姿态解算 (使用一阶互补滤波)
// ---------------------------------------------------------------------------------
if (SL_SC7U22_Error_Flag == 1) { // 确认已经校准完成
// --- 2.1 数据预处理 ---
// 应用零点偏移补偿
for (sl_i = 0; sl_i < 6; sl_i++) {
Temp_Accgyro[sl_i] = acc_gyro_input[sl_i] + Error_Accgyro[sl_i];
}
// --- 2.2 使用加速度计计算姿态角 (Pitch 和 Roll) ---
// 注意：这里使用了atan2f来计算Roll角，它比atanf更稳定，能自动处理所有象限，避免了后续的if判断。
float acc_x = (float)Temp_Accgyro[0];
float acc_y = (float)Temp_Accgyro[1];
float acc_z = (float)Temp_Accgyro[2];
// Pitch = arcsin(ax / g)
angle_acc[0] = asinf(acc_x / sqrtf(acc_x*acc_x + acc_y*acc_y + acc_z*acc_z)) * 57.29578f;
// Roll = arctan(ay / az)
angle_acc[1] = atan2f(acc_y, acc_z) * 57.29578f;
// --- 2.3 转换陀螺仪数据单位 ---
// 将陀螺仪原始值（LSB）转换为角速度（度/秒）
gyro_val[0] = Temp_Accgyro[4] * 0.061; // GYR-Y -> Pitch 速率
gyro_val[1] = Temp_Accgyro[3] * 0.061; // GYR-X -> Roll 速率
gyro_val[2] = Temp_Accgyro[5] * 0.061; // GYR-Z -> Yaw 速率
// =================================================================================
// 步骤 2.4: 一阶互补滤波
// ---------------------------------------------------------------------------------
// Pitch 轴融合
Angle_output[0] = FILTER_ALPHA * (Angle_output[0] + gyro_val[0] * SAMPLE_INTERVAL) + (1.0f - FILTER_ALPHA) * angle_acc[0];
// Roll 轴融合
Angle_output[1] = FILTER_ALPHA * (Angle_output[1] + gyro_val[1] * SAMPLE_INTERVAL) + (1.0f - FILTER_ALPHA) * angle_acc[1];
/************** Yaw 轴计算**************/
// Yaw角无法通过加速度计（重力）来校正，因此只能使用陀螺仪进行简单积分。
// 如果需要精确的Yaw角，必须引入磁力计进行修正。
if (yaw_rst == 1) {
Angle_output[2] = 0; // 如果有复位信号，则清零
}
// 增加一个简单的阈值，当角速度较小时，认为没有转动，以减少漂移
if (SL_GetAbsShort(Temp_Accgyro[5]) > 8) {
Angle_output[2] += gyro_val[2] * SAMPLE_INTERVAL;
}
return 1; // 返回1，表示计算成功
}
return 2; // 校准未完成，返回错误状态
}
unsigned char get_calibration_state(void){
return SL_SC7U22_Error_Flag;
}

2
apps/earphone/xtell_Sensor/sensor/SC7U22.h
View File

@ -130,6 +130,8 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en,signed short *
/**output Angle_output[2]: Yaw*******************************/
/**input yaw_rst: reset yaw value***************************/
unsigned char Original_SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short *acc_gyro_input, float *Angle_output, unsigned char yaw_rst);
unsigned char get_calibration_state(void);
/**寄存器宏定义*******************************/
#define SC7U22_WHO_AM_I 0x01

2
apps/earphone/xtell_Sensor/xtell.h
View File

@ -1,7 +1,7 @@
#ifndef XTELL_H
#define XTELL_H
#define KS_BLE 1
// #define KS_BLE 1
#define XTELL_TEST 1