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

cun

Browse Source
This commit is contained in:
lmx
2025-11-03 18:48:15 +08:00
parent 97e85df2f8
commit 671730a351
96 changed files with 170886 additions and 186935 deletions
Download Patch File Download Diff File Expand all files Collapse all files

153
apps/earphone/xtell_Sensor/calculate/skiing_tracker.c Normal file
View File

@ -0,0 +1,153 @@
#include "skiing_tracker.h"
#include <math.h> // 使用 sqrtf, fabsf, atan2f
#include <string.h> // 使用 memset
// ======================= 用户可配置参数 =======================
// IMU的采样率 (Hz)
#define SAMPLE_RATE 100.0f
#define DT (1.0f / SAMPLE_RATE)
// 传感器灵敏度配置 (必须与硬件配置匹配)
// 加速度计量程: ±8G -> 1G = 32768 / 8 = 4096 LSB
#define ACCEL_SENSITIVITY 4096.0f // LSB/g
#define GRAVITY_MSS 9.80665f // 标准重力加速度 (m/s^2)
// 陀螺仪灵敏度 (2000dps)
#define GYRO_SENSITIVITY 16.4f // LSB/(deg/s)
// 状态检测阈值
#define MOTION_ACCEL_THRESHOLD (ACCEL_SENSITIVITY * 0.3f) // 加速度变化超过0.3g认为在运动
#define MOTION_GYRO_THRESHOLD (GYRO_SENSITIVITY * 90.0f) // 角速度超过xx dps认为在运动
#define STILL_SAMPLES_FOR_CALIBRATION 100 // 连续静止1秒 (100个点) 开始校准
#define CALIBRATION_SAMPLE_COUNT 50 // 用于平均的校准样本数 (0.5秒)
#define MOTION_SAMPLES_TO_START_SKIING 10 // 连续运动0.1秒开始滑行
#define STILL_SAMPLES_TO_STOP_SKIING 20 // 连续静止0.2秒停止滑行
// 角度转弧度
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#define RAD_TO_DEG(rad) ((rad) * 180.0f / M_PI)
// ======================= 内部实现 =======================
void SkiingTracker_Init(SkiingTracker* tracker) {
memset(tracker, 0, sizeof(SkiingTracker));
tracker->state = STATE_UNCALIBRATED;
}
// 简单的低通滤波器
static float low_pass_filter(float new_input, float prev_output, float alpha) {
return alpha * new_input + (1.0f - alpha) * prev_output;
}
static void reset_calibration(SkiingTracker* tracker) {
tracker->calib_samples_count = 0;
tracker->calib_acc_sum[0] = 0;
tracker->calib_acc_sum[1] = 0;
tracker->calib_acc_sum[2] = 0;
}
// 核心更新函数
void SkiingTracker_Update(SkiingTracker* tracker, signed short* raw_accel, signed short* raw_gyro) {
// 运动状态检测
// 使用原始数据进行判断，避免校准误差影响
float acc_mag = sqrtf((float)raw_accel[0] * raw_accel[0] + (float)raw_accel[1] * raw_accel[1] + (float)raw_accel[2] * raw_accel[2]);
float gyro_mag = sqrtf((float)raw_gyro[0] * raw_gyro[0] + (float)raw_gyro[1] * raw_gyro[1] + (float)raw_gyro[2] * raw_gyro[2]);
int is_moving = (fabsf(acc_mag - ACCEL_SENSITIVITY) > MOTION_ACCEL_THRESHOLD) || (gyro_mag > MOTION_GYRO_THRESHOLD);
if (is_moving) {
tracker->still_counter = 0;
tracker->motion_counter++;
printf("===motion count===\n");
} else {
tracker->motion_counter = 0;
tracker->still_counter++;
printf("===still count===\n");
}
// 2. 状态机处理
switch (tracker->state) {
case STATE_UNCALIBRATED:
if (tracker->still_counter > STILL_SAMPLES_FOR_CALIBRATION) {
tracker->state = STATE_CALIBRATING;
reset_calibration(tracker);
}
break;
case STATE_CALIBRATING:
if (is_moving) { // 如果在校准时移动了，则校准失败，返回未校准状态
tracker->state = STATE_UNCALIBRATED;
reset_calibration(tracker);
break;
}
// 累加采样数据
tracker->calib_acc_sum[0] += raw_accel[0];
tracker->calib_acc_sum[1] += raw_accel[1];
tracker->calib_acc_sum[2] += raw_accel[2];
tracker->calib_samples_count++;
if (tracker->calib_samples_count >= CALIBRATION_SAMPLE_COUNT) {
// 校准完成，计算平均重力矢量
tracker->static_gravity[0] = (short)(tracker->calib_acc_sum[0] / CALIBRATION_SAMPLE_COUNT);
tracker->static_gravity[1] = (short)(tracker->calib_acc_sum[1] / CALIBRATION_SAMPLE_COUNT);
tracker->static_gravity[2] = (short)(tracker->calib_acc_sum[2] / CALIBRATION_SAMPLE_COUNT);
// 计算坡度（可选，用于显示）
float horiz_g = sqrtf((float)tracker->static_gravity[0] * tracker->static_gravity[0] + (float)tracker->static_gravity[1] * tracker->static_gravity[1]);
float vert_g = fabsf((float)tracker->static_gravity[2]);
tracker->slope_angle_deg = RAD_TO_DEG(atan2f(horiz_g, vert_g));
tracker->state = STATE_READY;
}
break;
case STATE_READY:
if (tracker->motion_counter > MOTION_SAMPLES_TO_START_SKIING) {
tracker->state = STATE_SKIING;
}
break;
case STATE_SKIING:
if (tracker->still_counter > STILL_SAMPLES_TO_STOP_SKIING) {
tracker->state = STATE_STOPPED;
tracker->velocity = 0.0f; // 零速更新
tracker->forward_accel = 0.0f;
break;
}
// 计算线性加速度 (重力抵消)
// 假设传感器的X轴指向滑雪板前进方向
long linear_accel_x_lsb = (long)raw_accel[0] - tracker->static_gravity[0];
// 转换为 m/s^2
float current_accel_mss = (float)linear_accel_x_lsb / ACCEL_SENSITIVITY * GRAVITY_MSS;
// 低通滤波以平滑加速度
tracker->forward_accel = low_pass_filter(current_accel_mss, tracker->forward_accel, 0.3f);
// 积分计算速度和距离 (梯形积分)
float prev_velocity = tracker->velocity;
tracker->velocity += tracker->forward_accel * DT;
// 物理约束：速度不能为负（不能往坡上滑）
if (tracker->velocity < 0) {
tracker->velocity = 0;
}
tracker->distance += (prev_velocity + tracker->velocity) / 2.0f * DT;
break;
case STATE_STOPPED:
// 在停止状态下，如果再次检测到运动，则重新进入滑行状态
if (tracker->motion_counter > MOTION_SAMPLES_TO_START_SKIING) {
tracker->state = STATE_SKIING;
}
// 如果长时间静止，返回未校准状态，以应对更换雪道的情况
// if (tracker->still_counter > 3000) { // e.g., 30 seconds
// tracker->state = STATE_UNCALIBRATED;
// }
break;
}
}

53
apps/earphone/xtell_Sensor/calculate/skiing_tracker.h Normal file
View File

@ -0,0 +1,53 @@
#ifndef SKIING_TRACKER_H
#define SKIING_TRACKER_H
// 定义滑雪者的运动状态
typedef enum {
STATE_UNCALIBRATED, // 未校准，等待在斜坡上静止
STATE_CALIBRATING, // 正在校准重力矢量
STATE_READY, // 校准完成，准备滑行
STATE_SKIING, // 正在滑行
STATE_STOPPED // 在斜坡上中途停止
} MotionState;
// 存储所有运动学数据
typedef struct {
// 最终输出
float velocity; // 沿斜坡方向的速度 (m/s)
float distance; // 沿斜坡方向的滑行距离 (m)
float slope_angle_deg; // 动态计算出的坡度 (度)
// 内部状态变量
MotionState state; // 当前运动状态
// 校准相关
short static_gravity[3]; // 校准后得到的静态重力矢量 (LSB)
long calib_acc_sum[3]; // 用于计算平均值的累加器
int calib_samples_count; // 校准采样计数
// 运动检测
int motion_counter;
int still_counter; // 静止状态计数器
// 物理量
float forward_accel; // 沿滑行方向的加速度 (m/s^2)
} SkiingTracker;
/**
* @brief 初始化滑雪追踪器
* @param tracker 指向 SkiingTracker 实例的指针
*/
void SkiingTracker_Init(SkiingTracker* tracker);
/**
* @brief 处理IMU数据，自动校准并计算速度和距离
* @details 这是核心处理函数，应在每次获取新的IMU数据后调用。
*
* @param tracker 指向 SkiingTracker 实例的指针
* @param raw_accel 未经校准的原始加速度数据 [X, Y, Z]，单位是 LSB
* @param raw_gyro 未经校准的原始陀螺仪数据 [X, Y, Z]，单位是 LSB
*/
void SkiingTracker_Update(SkiingTracker* tracker, signed short* raw_accel, signed short* raw_gyro);
#endif // SKIING_TRACKER_H

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

@ -19,7 +19,7 @@
#include "circle_buffer.h"
#include "circle_buffer.h"
#include "btstack/avctp_user.h"
#include "calculate/skiing_tracker.h"
///////////////////////////////////////////////////////////////////////////////////////////////////
//宏定义
#define ENABLE_XLOG 1
@ -37,6 +37,7 @@
//////////////////////////////////////////////////////////////////////////////////////////////////
//START -- 函数定义
void send_data_to_ble_client(const u8* data, u16 length);
extern void create_process(u16* pid, const char* name, void *priv, void (*func)(void *priv), u32 msec);
//END -- 函数定义
//////////////////////////////////////////////////////////////////////////////////////////////////
@ -53,76 +54,136 @@ static u16 send_data_id;
#define SENSOR_DATA_BUFFER_SIZE 512
static u8 sensor_data_buffer[SENSOR_DATA_BUFFER_SIZE];
static circle_buffer_t sensor_cb;
//--- test ---
static SkiingTracker skiing_data;
static char ble_send_data[50];
//END -- 变量定义
//////////////////////////////////////////////////////////////////////////////////////////////////
// 采集传感器数据并存入环形缓冲区的任务
void collect_and_buffer_sensor_data_task(void) {
motion_data_t current_motion;
axis_info_xtell total_accel;
axis_info_xtell linear_accel;
char data_string[256];
// 从驱动获取最新的运动数据
get_motion_data(&current_motion);
total_accel = get_current_accel_mss();
linear_accel = get_linear_accel_mss();
// 将浮点数据转换为整数（乘以100以保留两位小数）进行格式化
int len = snprintf(data_string, sizeof(data_string),
"T:%d,%d,%d;L:%d,%d,%d;V:%d,%d,%d;D:%d,%d,%d\n",
(int)(total_accel.x * 100), (int)(total_accel.y * 100), (int)(total_accel.z * 100),
(int)(linear_accel.x * 100), (int)(linear_accel.y * 100), (int)(linear_accel.z * 100),
(int)(current_motion.velocity.x * 100), (int)(current_motion.velocity.y * 100), (int)(current_motion.velocity.z * 100),
(int)(current_motion.distance.x * 100), (int)(current_motion.distance.y * 100), (int)(current_motion.distance.z * 100));
// 写入环形缓冲区
u16 written = circle_buffer_write(&sensor_cb, (u8*)data_string, len);
if (written < len) {
xlog("The circular buffer is full!\n");
}
}
// 定义数组大小
#define ARRAY_SIZE (178)
// 在 main 函数外部声明为全局变量，它将被存储在静态数据区
unsigned char global_data_array[ARRAY_SIZE];
// 从环形缓冲区读取数据并发送
void send_sensor_data_task(void) {
// printf("xtell_ble_send\n");
send_data_to_ble_client(&global_data_array,ARRAY_SIZE);
}
extern void create_process(u16* pid, const char* name, void *priv, void (*func)(void *priv), u32 msec);
void rcsp_adv_fill_mac_addr(u8 *mac_addr_buf) //by xtell
{
swapX(bt_get_mac_addr(), mac_addr_buf, 6);
}
void xtell_task_create(void){
void sensor_test(){
xlog("xtell_task_create\n");
//写入测试数据
for (int i = 0; i < ARRAY_SIZE; i++) { //ARRAY_SIZE字节的数组
global_data_array[i] = i % 256;
// signed short acc_raw[3], gyr_raw[3];
// // 读取原始数据
// SL_SC7U22_RawData_Read(acc_raw, gyr_raw);
// // 将原始数据送入追踪器进行处理
// SkiingTracker_Update(&skiing_data, acc_raw, gyr_raw);
// // c. 打印状态和结果
const char* state_str = "UNKNOWN";
switch(skiing_data.state) {
case STATE_UNCALIBRATED: state_str = "UNCALIBRATED"; break;
case STATE_CALIBRATING: state_str = "CALIBRATING..."; break;
case STATE_READY: state_str = "READY"; break;
case STATE_SKIING: state_str = "SKIING"; break;
case STATE_STOPPED: state_str = "STOPPED"; break;
}
// // 转换为 km/h
// float velocity_kmh = skiing_data.velocity * 3.6f;
// printf("State: %s, Slope: %.1f deg, Vel: %.2f km/h, Dist: %.2f m\n",
// state_str,
// skiing_data.slope_angle_deg,
// velocity_kmh,
// skiing_data.distance);
signed short acc_raw[3], gyr_raw[3];
// 读取原始数据
SL_SC7U22_RawData_Read(acc_raw, gyr_raw);
// 将原始数据送入追踪器进行处理
SkiingTracker_Update(&skiing_data, acc_raw, gyr_raw);
// 清空发送缓冲区
memset(ble_send_data, 0, sizeof(ble_send_data));
// 准备要填充的数据
// 状态
char state_code = (char)skiing_data.state;
// 坡度 (舍弃小数)
char slope_angle = (char)skiing_data.slope_angle_deg;
// 速度 km/h (舍弃小数)
float velocity_kmh_float = skiing_data.velocity * 3.6f;
// 使用 uint16_t 来存储速度，因为它可能超过255
uint16_t velocity_kmh_int = (uint16_t)velocity_kmh_float;
// 距离 (m)
// 使用 uint32_t 来存储距离，因为它会变得很大
uint32_t distance_int = (uint32_t)skiing_data.distance;
// 按顺序填充到 ble_send_data 数组中
int index = 0;
// 字节 0: 状态码 (1 byte)
ble_send_data[index++] = state_code;
// 字节 1: 坡度 (1 byte)
ble_send_data[index++] = slope_angle;
// 字节 2-3: 速度 (2 bytes, 小端模式)
// 将 16 位的速度拆分成两个 8 位的 char
ble_send_data[index++] = (char)(velocity_kmh_int & 0xFF); // 低8位
ble_send_data[index++] = (char)((velocity_kmh_int >> 8) & 0xFF); // 高8位
// 字节 4-7: 距离 (4 bytes, 小端模式)
// 将 32 位的距离拆分成四个 8 位的 char
ble_send_data[index++] = (char)(distance_int & 0xFF); // 最低8位
ble_send_data[index++] = (char)((distance_int >> 8) & 0xFF);
ble_send_data[index++] = (char)((distance_int >> 16) & 0xFF);
ble_send_data[index++] = (char)((distance_int >> 24) & 0xFF); // 最高8位
send_data_to_ble_client(ble_send_data, index);
printf("State: %s, Slope: %.1f deg, Vel: %.2f km/h, Dist: %.2f m\n",
state_str,
skiing_data.slope_angle_deg,
velocity_kmh_float,
skiing_data.distance);
}
u16 test_id=0;
void xtell_task_create(void){
// int ret = hw_iic_init(0);
// xlog("hw_iic_init result:%d\n",ret);
// //初始化传感器
// SL_SC7U22_Config();
soft_iic_init(0);
gpio_set_direction(IO_PORTE_05,0); //设置PE5 输出模式
gpio_set_pull_up(IO_PORTE_05,1);
gpio_direction_output(IO_PORTE_05,1);
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);
// 创建一个定时器，每200ms调用一次核心计算任务
// create_process(&xtell_i2c_test_id, "xtell_i2c_test", NULL, xtell_i2c_test, (u32)(SAMPLING_PERIOD_S * 1000));
//初始化滑雪追踪器
// SkiingTracker_Init(&skiing_data);
xlog("SkiingTracker_Init\n");
// 创建一个定时器，每1000ms采集一次数据
// create_process(&collect_data_id, "collect_data", NULL, collect_and_buffer_sensor_data_task, 1000);
create_process(&test_id, "test",NULL, sensor_test, 10);
// 创建一个定时器，每200ms尝试发送一次数据
create_process(&send_data_id, "send_data", NULL, send_sensor_data_task, 1);
}

8
apps/earphone/xtell_Sensor/sensor/LIS2DH12.c
View File

@ -33,7 +33,7 @@
#define LIS2DH12_R_ADDR 0x33
// --- IIC 寄存器地址宏定义 ---
#define LIS2DH12_WHO_AM_I 0x0F
#define LIS2DH12_WHO_AM_I 0x01 //0F
#define LIS2DH12_CTRL_REG1 0x20
#define LIS2DH12_CTRL_REG4 0x23
#define LIS2DH12_CTRL_REG5 0x24
@ -93,7 +93,7 @@ static u8 SL_MEMS_i2cWrite(u8 addr, u8 reg, u8 data) {
char LIS2DH12_Check() {
u8 reg_value = 0;
SL_MEMS_i2cRead(LIS2DH12_R_ADDR, LIS2DH12_WHO_AM_I, 1, &reg_value);
if (reg_value == 0x33) {
if (reg_value == 0x6A) { //0x33
return 0x01;
}
return 0x00;
@ -150,7 +150,7 @@ void LIS2DH12_calibrate() {
// 初始化并配置LIS2DH12传感器
u8 LIS2DH12_Config(void) {
if (LIS2DH12_Check() != 1) {
xlog("LIS2DH12 I2C检查失败\n");
xlog("LIS2DH12 I2C error\n");
return -1;
}
@ -163,7 +163,7 @@ u8 LIS2DH12_Config(void) {
// 执行开机校准
LIS2DH12_calibrate();
xlog("LIS2DH12 I2C检查成功\n");
xlog("LIS2DH12 I2C success\n");
return 0;
}

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

@ -1,12 +1,18 @@
#include "SCU722.h"
#include "SC7U22.h"
#include "math.h"
#include "os/os_api.h"
#if SL_Sensor_Algo_Release_Enable==0x00
#include "printf.h"
#endif
#define ENABLE_XLOG 1
#ifdef xlog
#undef xlog
#endif
#if ENABLE_XLOG
#define xlog(format, ...) printf("[XT:%s] " format, __func__, ##__VA_ARGS__)
#else
#define xlog(format, ...) ((void)0)
#endif
//I2C SPI选择
//#define SL_SC7U22_SPI_EN_I2C_DISABLE 0x00 //需要配合SL_SPI_IIC_INTERFACE使用
@ -61,13 +67,11 @@ static void sl_delay(unsigned char sl_i)
unsigned char SL_SC7U22_Check(void)
{
unsigned char reg_value=0;
xlog("SL_SC7U22_Check\n");
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, SC7U22_WHO_AM_I, 1, &reg_value);
#if SL_Sensor_Algo_Release_Enable==0x00
printf("0x%x=0x%x\r\n",SC7U22_WHO_AM_I,reg_value);
#endif
if(reg_value==0x6A)
xlog("0x%x=0x%x\r\n",SC7U22_WHO_AM_I,reg_value);
if(reg_value==0x6A) //设备的id
return 0x01;//SC7U22
else
return 0x00;//通信异常
@ -75,6 +79,7 @@ unsigned char SL_SC7U22_Check(void)
unsigned char SL_SC7U22_Config(void)
{
xlog("SL_SC7U22_Config\n");
unsigned char Check_Flag=0;
unsigned char reg_value=0;
@ -95,23 +100,18 @@ unsigned char SL_SC7U22_Config(void)
Check_Flag=SL_SC7U22_Check();
// Check_Flag= SL_SC7U22_SOFT_RESET();
// Check_Flag=1;//强制初始化
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_SC7U22_Check=0x%x\r\n",Check_Flag);
#endif
xlog("SL_SC7U22_Check=0x%x\r\n",Check_Flag);
if(Check_Flag==1)
{
Check_Flag= SL_SC7U22_POWER_DOWN();
}
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_SC7U22_POWER_DOWN=0x%x\r\n",Check_Flag);
#endif
xlog("SL_SC7U22_POWER_DOWN=0x%x\r\n",Check_Flag);
if(Check_Flag==1)
{
Check_Flag= SL_SC7U22_SOFT_RESET();
}
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_SC7U22_SOFT_RESET=0x%x\r\n",Check_Flag);
#endif
xlog("SL_SC7U22_SOFT_RESET=0x%x\r\n",Check_Flag);
if(Check_Flag==1)
{
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
@ -119,9 +119,9 @@ 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
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, 0x41, 0x01);//ACC_RANGE ±8G
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x42, 0x86);//GYR_CONF 0x87=50Hz
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, 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
@ -218,24 +218,23 @@ void SL_SC7U22_RawData_Read(signed short * acc_data_buf,signed short * gyr_data_
if(drdy_cnt>30000) break;
}
#if SL_Sensor_Algo_Release_Enable==0x00
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x30, 1, &drdy_satus);
// printf("RawData:0x40=%x\r\n",drdy_satus);
// xlog("RawData:0x40=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x40, 1, &drdy_satus);
// printf("RawData:0x40=%x\r\n",drdy_satus);
// xlog("RawData:0x40=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x06, 1, &drdy_satus);
// printf("RawData:0x06=%x\r\n",drdy_satus);
// xlog("RawData:0x06=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x07, 1, &drdy_satus);
// printf("RawData:0x07=%x\r\n",drdy_satus);
// xlog("RawData:0x07=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1, &drdy_satus);
// printf("RawData:0x7D=%x\r\n",drdy_satus);
// xlog("RawData:0x7D=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x31, 1, &drdy_satus);
// printf("RawData:0x31=%x\r\n",drdy_satus);
// xlog("RawData:0x31=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x02, 1, &drdy_satus);
// printf("RawData:0x02=%x\r\n",drdy_satus);
// xlog("RawData:0x02=%x\r\n",drdy_satus);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x03, 1, &drdy_satus);
// printf("RawData:0x03=%x\r\n",drdy_satus);
#endif
// xlog("RawData:0x03=%x\r\n",drdy_satus);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x0C, 12, &raw_data[0]);
@ -246,9 +245,7 @@ void SL_SC7U22_RawData_Read(signed short * acc_data_buf,signed short * gyr_data_
gyr_data_buf[1] =(signed short)((((unsigned char)raw_data[8])* 256) + ((unsigned char)raw_data[9]));//GYRY-16位
gyr_data_buf[2] =(signed short)((((unsigned char)raw_data[10])* 256) + ((unsigned char)raw_data[11]));//GYRZ-16位
#if SL_Sensor_Algo_Release_Enable==0x00
printf("RawData:AX=%d,AY=%d,AZ=%d,GX=%d,GY=%d,GZ=%d\r\n",acc_data_buf[0],acc_data_buf[1],acc_data_buf[2],gyr_data_buf[0],gyr_data_buf[1],gyr_data_buf[2]);
#endif
xlog("RawData:AX=%d,AY=%d,AZ=%d,GX=%d,GY=%d,GZ=%d\r\n",acc_data_buf[0],acc_data_buf[1],acc_data_buf[2],gyr_data_buf[0],gyr_data_buf[1],gyr_data_buf[2]);
}
#else
@ -322,10 +319,10 @@ unsigned short SL_SC7U22_FIFO_Read(signed short *accx_buf,signed short *accy_buf
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x21, fifo_num*2, SL_SC7U22_FIFO_DATA);//读取FIFO数据 BYTE NUM
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1D, 0x00);//BY PASS MODE
// SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x1D, 0x20);//Stream MODE
printf("SC7U22_FIFO_NUM1:%d\n",fifo_num);
xlog("SC7U22_FIFO_NUM1:%d\n",fifo_num);
#if SL_Sensor_Algo_Release_Enable==0x00
// printf("0x1F:0x%x 0x20:0x%x\n",fifo_num1,fifo_num2);
// printf("SC7U22_FIFO_NUM1:%d\n",fifo_num);
// xlog("0x1F:0x%x 0x20:0x%x\n",fifo_num1,fifo_num2);
// xlog("SC7U22_FIFO_NUM1:%d\n",fifo_num);
#endif
fifo_len=0;
@ -350,7 +347,7 @@ unsigned short SL_SC7U22_FIFO_Read(signed short *accx_buf,signed short *accy_buf
accx_buf[Acc_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 0] * 256 + SL_SC7U22_FIFO_DATA[i + 1])) ;
accy_buf[Acc_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 2] * 256 + SL_SC7U22_FIFO_DATA[i + 3])) ;
accz_buf[Acc_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 4] * 256 + SL_SC7U22_FIFO_DATA[i + 5])) ;
printf("AccNum : %d ,Acc_x : %4d, Acc_y : %4d, Acc_z : %4d,\r\n",Acc_FIFO_Num, accx_buf[Acc_FIFO_Num], accy_buf[Acc_FIFO_Num], accz_buf[Acc_FIFO_Num]);
xlog("AccNum : %d ,Acc_x : %4d, Acc_y : %4d, Acc_z : %4d,\r\n",Acc_FIFO_Num, accx_buf[Acc_FIFO_Num], accy_buf[Acc_FIFO_Num], accz_buf[Acc_FIFO_Num]);
i = i + 6;
Acc_FIFO_Num++;
}
@ -360,7 +357,7 @@ unsigned short SL_SC7U22_FIFO_Read(signed short *accx_buf,signed short *accy_buf
gyrx_buf[Gyr_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 0] * 256 + SL_SC7U22_FIFO_DATA[i + 1])) ;
gyry_buf[Gyr_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 2] * 256 + SL_SC7U22_FIFO_DATA[i + 3])) ;
gyrz_buf[Gyr_FIFO_Num] = ((s16)(SL_SC7U22_FIFO_DATA[i + 4] * 256 + SL_SC7U22_FIFO_DATA[i + 5])) ;
printf("GyrNum : %d, Gyr_x : %4d, Gyr_y : %4d, Gyr_z : %4d,\r\n",Gyr_FIFO_Num, gyrx_buf[Gyr_FIFO_Num], gyry_buf[Gyr_FIFO_Num], gyrz_buf[Gyr_FIFO_Num]);
xlog("GyrNum : %d, Gyr_x : %4d, Gyr_y : %4d, Gyr_z : %4d,\r\n",Gyr_FIFO_Num, gyrx_buf[Gyr_FIFO_Num], gyry_buf[Gyr_FIFO_Num], gyrz_buf[Gyr_FIFO_Num]);
i = i + 6;
Gyr_FIFO_Num++;
}
@ -398,21 +395,16 @@ unsigned char SL_SC7U22_SOFT_RESET(void)
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x7F, 0x00);//goto 0x00
os_time_dly(1);
#if SL_Sensor_Algo_Release_Enable==0x00
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x04, 1,&SL_Read_Reg);
printf("SL_SC7U22_SOFT_RESET1 0x04=0x%x\r\n",SL_Read_Reg);
xlog("SL_SC7U22_SOFT_RESET1 0x04=0x%x\r\n",SL_Read_Reg);
SL_Read_Reg = 0xff;
#endif
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x04, 0x10);//BOOT
#if SL_Sensor_Algo_Release_Enable==0x00
#endif
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x4A, 0xA5);//SOFT_RESET
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x4A, 0xA5);//SOFT_RESET
os_time_dly(20);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x04, 1,&SL_Read_Reg);
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_SC7U22_SOFT_RESET2 0x08=0x%x\r\n",SL_Read_Reg);
#endif
xlog("SL_SC7U22_SOFT_RESET2 0x08=0x%x\r\n",SL_Read_Reg);
if(SL_Read_Reg==0x50) return 1;
else return 0;
@ -453,9 +445,7 @@ unsigned char SL_SC7U22_Open_Close_SET(unsigned char acc_enable,unsigned char gy
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x7D, 1,&SL_Read_Check);
if(SL_Read_Reg!=SL_Read_Check)
{
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_Read_Reg=0x%x SL_Read_Check=0x%x\r\n",SL_Read_Reg,SL_Read_Check);
#endif
xlog("SL_Read_Reg=0x%x SL_Read_Check=0x%x\r\n",SL_Read_Reg,SL_Read_Check);
return 0;
}
return 1;
@ -513,9 +503,7 @@ unsigned char SL_SC7U22_IN_SLEEP_SET(unsigned char acc_odr,unsigned char vth,uns
if(SL_Read_Reg!=0x04)
{
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_Read_Reg=0x%x 0x04\r\n",SL_Read_Reg);
#endif
xlog("SL_Read_Reg=0x%x 0x04\r\n",SL_Read_Reg);
return 0;
}
return 1;
@ -617,23 +605,20 @@ unsigned char SL_SC7U22_WakeUp_SET(unsigned char odr_mode,unsigned char acc_rang
}
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, SL_gyro_range_Reg);//GYR_RANGE 2000dps
SL_SC7U22_I2c_Spi_Write(SL_SPI_IIC_INTERFACE, 0x43, SL_gyro_range_Reg);//GYR_RANGE 2000dps
#if SL_Sensor_Algo_Release_Enable==0x00
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x40, 1, &SL_Read_Check);
// printf("RawData:0x40=%x\r\n",SL_Read_Check);
// xlog("RawData:0x40=%x\r\n",SL_Read_Check);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x41, 1, &SL_Read_Check);
// printf("RawData:0x41=%x\r\n",SL_Read_Check);
// xlog("RawData:0x41=%x\r\n",SL_Read_Check);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x42, 1, &SL_Read_Check);
// printf("RawData:0x42=%x\r\n",SL_Read_Check);
// xlog("RawData:0x42=%x\r\n",SL_Read_Check);
// SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x43, 1, &SL_Read_Check);
// printf("RawData:0x43=%x\r\n",SL_Read_Check);
#endif
// xlog("RawData:0x43=%x\r\n",SL_Read_Check);
SL_SC7U22_I2c_Spi_Read(SL_SPI_IIC_INTERFACE, 0x43, 1,&SL_Read_Check);
if(SL_Read_Check!=SL_gyro_range_Reg)
{
#if SL_Sensor_Algo_Release_Enable==0x00
printf("SL_Read_Check=0x%x SL_gyro_range_Reg=0x%x\r\n",SL_Read_Check,SL_gyro_range_Reg);
#endif
xlog("SL_Read_Check=0x%x SL_gyro_range_Reg=0x%x\r\n",SL_Read_Check,SL_gyro_range_Reg);
return 0;
}
return 1;
@ -773,17 +758,14 @@ unsigned char SL_SC7U22_Angle_Output(unsigned char calibration_en, signed short
}
// 计算零点偏移：理想值 - 实际平均值
// 加速度Z轴的理想值是8192，对应1g（假设量程为±8g）
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];
#if SL_Sensor_Algo_Release_Enable == 0x00
printf("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]);
printf("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]);
#endif
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 {
// 如果在累加过程中发生移动，则重新开始
@ -982,13 +964,13 @@ void sensor_processing_task(void *priv)
// 7. 检查函数返回的状态
if (status == 1) {
// 计算成功，final_angle_data 中的数据有效
printf("Pitch: %.2f, Roll: %.2f, Yaw: %.2f\n", final_angle_data[0], final_angle_data[1], final_angle_data[2]);
xlog("Pitch: %.2f, Roll: %.2f, Yaw: %.2f\n", final_angle_data[0], final_angle_data[1], final_angle_data[2]);
} else if (status == 0) {
// 传感器正在进行静态校准，此时角度数据可能不准确
printf("Sensor is calibrating...\n");
xlog("Sensor is calibrating...\n");
} else {
// status == 2, 表示校准未完成或发生错误
printf("Angle calculation error or calibration not finished.\n");
xlog("Angle calculation error or calibration not finished.\n");
}
// 延时一段时间，例如10ms (对应100Hz)
@ -1005,11 +987,11 @@ void app_main()
unsigned char init_success = SL_SC7U22_Config();
if (init_success) {
printf("SCU722 初始化成功!\n");
xlog("SCU722 初始化成功!\n");
// 3. 创建一个任务来周期性地读取和处理数据
task_create(sensor_processing_task, NULL, "sensor_task");
} else {
printf("SCU722 初始化失败!\n");
xlog("SCU722 初始化失败!\n");
}
// ...

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

@ -11,7 +11,7 @@ Copyright (c) 2022 Silan MEMS. All Rights Reserved.
#include "printf.h"
//是否使能串口打印调试
#define SL_Sensor_Algo_Release_Enable 0x01
#define SL_Sensor_Algo_Release_Enable 0x00
//是否开启FIFO模式，默认STREAM模式
#define SL_SC7U22_FIFO_ENABLE 0x00

2
apps/earphone/xtell_Sensor/sensor/SC7U22_Ma.c
View File

@ -657,7 +657,7 @@ static float invSqrt(float x) {
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1); // 神奇的魔术数
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y)); // 牛顿迭代法，提高精度
return y;

6
apps/earphone/xtell_Sensor/xtell_app_main.c
View File

@ -148,10 +148,10 @@ void xtell_app_main()
create_process(&close_BL_number, "close_BL",NULL, close_BL, 3000);
soft_iic_init(0);
extern u8 LIS2DH12_Config(void);
LIS2DH12_Config();
u8 mac_data[6];
extern void rcsp_adv_fill_mac_addr(u8 *mac_addr_buf);
rcsp_adv_fill_mac_addr(mac_data); //读取MAC地址
xlog("xtell BT mac data:%x:%x:%x:%x:%x:%x",mac_data[0],mac_data[1],mac_data[2],mac_data[3],mac_data[4],mac_data[5]);