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-12-05 19:06:54 +08:00
parent 89f1c93f74
commit 138275a04b
15 changed files with 432 additions and 450 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@ -42,7 +42,10 @@
#define xlog(format, ...) ((void)0)
#endif
#define SENSOR_DATA_BUFFER_SIZE 1000 // 定义缓冲区可以存储XXX个sensor_data_t元素
#define SENSOR_DATA_BUFFER_SIZE 200 // 定义缓冲区可以存储XXX个sensor_data_t元素
#define MPU_FIFO_INTERVAL 4 //隔多久读取六轴fifo，单位10ms
#define MPU_FIFO_LEN 16 //(MPU_FIFO_INTERVAL*10/2.5) //400hz采用频率，那每40ms的时间，fifo就有16组六轴数据
//
///////////////////////////////////////////////////////////////////////////////////////////////////
@ -52,7 +55,6 @@
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);
extern void close_process(u16* pid,char* name);
void start_collect_fuc(void);
void BLE_send_fuc(void);
//END -- 函数定义
//////////////////////////////////////////////////////////////////////////////////////////////////
@ -62,35 +64,26 @@ void BLE_send_fuc(void);
//START -- 变量定义
static u32 timer_offset_ms = 0;
typedef struct {
// -- 六轴 --
signed short SC7U22_data[6];
// -- 磁力计 --
uint8_t mmc5603nj_buffer[9];
// -- 速度 --
uint16_t speed_cms;
// -- 气压计 --
int16_t temperature;
uint32_t pressure;
// -- 左/右腿 --
uint8_t foot_state; //1：左脚；2：右脚
// -- 时间 --
u32 timestamp_ms;
typedef struct{
signed short SC7U22_data[6]; //12字节
int mmc5603nj_buffer[3]; //12字节
int16_t temperature; //2
uint32_t pressure; //4
}sensor_package_t __attribute__((packed));
} BLE_send_data_t;
static int count = 0;
typedef struct{
uint8_t checkout_1;
uint8_t checkout_2;
uint8_t index;
sensor_package_t sensor_package[MPU_FIFO_LEN];//一次蓝牙发送MPU_FIFO_LEN组传感器数据
}ble_send_data_t; //一次蓝牙发送的数据内容
// --- 环形缓冲区 ---
static circle_buffer_t BLE_send_buff; // 环形缓冲区管理结构体
BLE_send_data_t BLE_send_data[SENSOR_DATA_BUFFER_SIZE];
static circle_buffer_t g_ble_send_cb; // 环形缓冲区管理结构体
static ble_send_data_t g_sensor_data_storage[SENSOR_DATA_BUFFER_SIZE]; //缓冲区
// -- 任务id --
u16 SC7U22_calibration_id;
u16 start_collect_fuc_id;
u16 BLE_send_fuc_id;
static u8 stop_ble_send_fuc_id;
static OS_SEM receiver_ready_sem; // 用于启动同步的信号量
//END -- 变量定义
//////////////////////////////////////////////////////////////////////////////////////////////////
@ -106,110 +99,154 @@ u32 get_ms_timer(void) {
return sys_timer_get_ms() - timer_offset_ms;
}
// ----------------------------------------------------------------------------------------------------------------
// --------------------------------------------定时器回调函数----------------------------------------------------------
/**
* @brief 六轴静态校准
*
*/
void SC7U22_static_calibration(void){
signed short acc_data_buf[3];
signed short gyr_data_buf[3];
float angle[3];
float quaternion_output[3];
static signed short combined_raw_data[6];
static int calibration_done = 0;
char status = 0;
static first_set_flag = 0;
if(first_set_flag == 0){
first_set_flag = 1;
set_SC7U22_Error_Flag(0);
}
SL_SC7U22_RawData_Read(acc_data_buf,gyr_data_buf);
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, angle,NULL, 0, quaternion_output);
if(status == 1){ //校准完成
extern u16 SC7U22_calibration_id;
extern u8 SC7U22_init;
first_set_flag = 0;
SC7U22_init = 0x11;
close_process(&SC7U22_calibration_id, "SC7U22_calibration");
u8 send2_1[5] = {0xBB,0xBE,0x02,0x00,0x00};
send2_1[4] = SC7U22_init;
send_data_to_ble_client(&send2_1,5);
}
if(count > 100){
count = 0;
char log_buffer[100];
// snprintf( log_buffer, sizeof(log_buffer),"status:%d\n",status);
// send_data_to_ble_client(&log_buffer,strlen(log_buffer));
xlog("status:%d\n", status);
xlog("RawData:AX=%d,AY=%d,AZ=%d,GX=%d,GY=%d,GZ=%d\r\n",combined_raw_data[0],combined_raw_data[1],combined_raw_data[2],combined_raw_data[3],combined_raw_data[4],combined_raw_data[5]);
uint8_t send[5] = {0xBB, 0xBE, 0x02, 0x00, 0x12};
send_data_to_ble_client(&send,5); //正在校验中
}
count++;
}
/**
* @brief 开始采集传感器数据和计算速度
* @brief 传感器采集任务
*
*/
void start_collect_fuc(void){
// 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;
BLE_send_data_t BLE_send_data_tmp;
uint8_t mmc5603nj_buffer[9];
signed short acc_data_buf[3];
signed short gyr_data_buf[3];
float angle[3];
float quaternion_output[3];
void sensor_collect_task(void){
ble_send_data_t send_data;
mmc5603nj_mag_data_t mmc5603nj_buffer;
float temperature = 0;
float pressure = 0;
int interval = 0;
signed short accx_buf[100];
signed short accy_buf[100];
signed short accz_buf[100];
signed short gyrx_buf[100];
signed short gyry_buf[100];
signed short gyrz_buf[100];
int fifo_num = 0;
u8 index = 1;
// -- 读数据 --
SL_SC7U22_RawData_Read(acc_data_buf,gyr_data_buf);
mmc5603nj_read_origin_data(mmc5603nj_buffer);
// bmp280_read_originanl_data(&BLE_send_data_tmp.adc_P, &BLE_send_data_tmp.adc_T);
bmp280_read_data(&temperature, &pressure);
while(1){//4组地磁数据、16组六轴数据、1组气压计数据
interval++;
mmc5603nj_read_mag_data(&mmc5603nj_buffer);
for(int i = (interval-1)*4; i < interval*4; i++){
send_data.sensor_package[i].mmc5603nj_buffer[0] = (int32_t)(mmc5603nj_buffer.x * 1000.0f);
send_data.sensor_package[i].mmc5603nj_buffer[1] = (int32_t)(mmc5603nj_buffer.y * 1000.0f);
send_data.sensor_package[i].mmc5603nj_buffer[2] = (int32_t)(mmc5603nj_buffer.z * 1000.0f);
}
// xlog("MAG x: %.2f,y: %.2f,z: %.2f\n",mmc5603nj_buffer.x,mmc5603nj_buffer.y,mmc5603nj_buffer.z);
if(interval >= 4){
interval = 0;
memset(&accx_buf, 0, sizeof(accx_buf));
memset(&accy_buf, 0, sizeof(accy_buf));
memset(&accz_buf, 0, sizeof(accz_buf));
memset(&gyrx_buf, 0, sizeof(gyrx_buf));
memset(&gyry_buf, 0, sizeof(gyry_buf));
memset(&gyrz_buf, 0, sizeof(gyrz_buf));
fifo_num = SL_SC7U22_FIFO_Read(accx_buf,accy_buf,accz_buf,gyrx_buf,gyry_buf,gyrz_buf); //40ms一次性读取内置fifo的数据
bmp280_read_data(&temperature, &pressure);//每40ms读取一次
for(int i = 0;i<MPU_FIFO_LEN;i++){
send_data.sensor_package[i].SC7U22_data[0] = accx_buf[i]; //acc_x
send_data.sensor_package[i].SC7U22_data[1] = accy_buf[i]; //acc_y
send_data.sensor_package[i].SC7U22_data[2] = accz_buf[i]; //acc_z
send_data.sensor_package[i].SC7U22_data[3] = gyrx_buf[i]; //gyr_x
send_data.sensor_package[i].SC7U22_data[4] = gyry_buf[i]; //gyr_y
send_data.sensor_package[i].SC7U22_data[5] = gyrz_buf[i]; //gyr_z
send_data.sensor_package[i].temperature = (int16_t)(temperature * 1000.0f);
send_data.sensor_package[i].pressure = (int32_t)(pressure * 1000.0f);
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
#if 0
float acc_g[3];
float gyr_dps[3];
acc_g[0] = (float)send_data.sensor_package[i].SC7U22_data[0] / 2048.0f;
acc_g[1] = (float)send_data.sensor_package[i].SC7U22_data[1] / 2048.0f;
acc_g[2] = (float)send_data.sensor_package[i].SC7U22_data[2] / 2048.0f;
gyr_dps[0] = (float)send_data.sensor_package[i].SC7U22_data[3] * 0.061f;
gyr_dps[1] = (float)send_data.sensor_package[i].SC7U22_data[4] * 0.061f;
gyr_dps[2] = (float)send_data.sensor_package[i].SC7U22_data[5] * 0.061f;
printf(" ACC(g): x=%.3f, y=%.3f, z=%.3f\n", acc_g[0], acc_g[1], acc_g[2]);
printf(" GYR(dps):x=%.3f, y=%.3f, z=%.3f\n", gyr_dps[0], gyr_dps[1], gyr_dps[2]);
#endif
}
// -- 四元数 --
status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, angle,NULL, 0, quaternion_output);
// xlog("temperature: %.2f,pressure: %.2f\n",temperature,pressure);
// xlog("fifo_num:%d\n",fifo_num);
// -- 速度计算 --
memcpy(acc_data_buf, &combined_raw_data[0], 3 * sizeof(signed short));
memcpy(gyr_data_buf, &combined_raw_data[3], 3 * sizeof(signed short));
uint16_t speed = sensor_processing_task(acc_data_buf,gyr_data_buf,angle, quaternion_output);
// -- 数据包装进结构体 --
memcpy(&BLE_send_data_tmp.SC7U22_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&BLE_send_data_tmp.SC7U22_data[3], gyr_data_buf, 3 * sizeof(signed short));
memcpy(BLE_send_data_tmp.mmc5603nj_buffer, mmc5603nj_buffer, 9);
BLE_send_data_tmp.temperature = (int16_t)(temperature * 1000.0f);
BLE_send_data_tmp.pressure = (int32_t)(pressure * 1000.0f);
BLE_send_data_tmp.speed_cms = speed;
extern u8 foot_init;
BLE_send_data_tmp.foot_state = foot_init;
BLE_send_data_tmp.timestamp_ms = get_ms_timer();
send_data.checkout_1 = 0xBE;
send_data.checkout_2 = 0xBB;
send_data.index = index;
// circle_buffer_write(&g_ble_send_cb, &send_data);
// os_sem_post(&receiver_ready_sem); //通知另一个发送任务
// -- 放进缓冲区 --
if(circle_buffer_is_full(&BLE_send_buff) == 0){
circle_buffer_write(&BLE_send_buff, &BLE_send_data_tmp);
memset(&send_data, 0, sizeof(ble_send_data_t));
index++;
if(index >= 0xFF) index = 1;
// xlog("=====================%d============================\n",get_ms_timer());
}
os_time_dly(1); //10ms为单位
}
// xlog("=======sensor_read_data END\n");
}
static int random = 0;
void data_log(uint8_t* data){
// 检查数据包头部
if (data[0] != 0xBE || data[1] != 0xBB) {
printf("Error: Invalid data packet header.\n");
return;
}
// 解析包索引
uint8_t index = data[2];
printf("--- Parsing Data Packet Index: %d ---\n", index);
uint8_t* p = &data[3]; // 指向数据负载的起始位置
// 循环解析16组数据
for (int i = 0; i < MPU_FIFO_LEN; i++) {
// 1. 解析六轴传感器数据 (12 bytes)
int16_t imu_raw[6];
for (int j = 0; j < 6; j++) {
// 小端模式: 低字节在前, 高字节在后
imu_raw[j] = (int16_t)(((uint16_t)p[1] << 8) | (uint16_t)p[0]);
p += 2;
}
float acc_g[3];
float gyr_dps[3];
acc_g[0] = (float)imu_raw[0] / 2048.0f;
acc_g[1] = (float)imu_raw[1] / 2048.0f;
acc_g[2] = (float)imu_raw[2] / 2048.0f;
gyr_dps[0] = (float)imu_raw[3] * 0.061f;
gyr_dps[1] = (float)imu_raw[4] * 0.061f;
gyr_dps[2] = (float)imu_raw[5] * 0.061f;
// 2. 解析地磁传感器数据 (12 bytes)
int32_t mag_raw[3];
for (int j = 0; j < 3; j++) {
// 小端模式
mag_raw[j] = (int32_t)(((uint32_t)p[3] << 24) | ((uint32_t)p[2] << 16) | ((uint32_t)p[1] << 8) | (uint32_t)p[0]);
p += 4;
}
float mag_gauss[3];
mag_gauss[0] = (float)mag_raw[0] / 1000.0f;
mag_gauss[1] = (float)mag_raw[1] / 1000.0f;
mag_gauss[2] = (float)mag_raw[2] / 1000.0f;
// 3. 解析温度数据 (2 bytes)
int16_t temp_raw = (int16_t)(((uint16_t)p[1] << 8) | (uint16_t)p[0]);
p += 2;
float temperature = (float)temp_raw / 1000.0f;
// 4. 解析气压数据 (4 bytes)
uint32_t press_raw = (uint32_t)(((uint32_t)p[3] << 24) | ((uint32_t)p[2] << 16) | ((uint32_t)p[1] << 8) | (uint32_t)p[0]);
p += 4;
float pressure = (float)press_raw / 1000.0f;
// 打印解析后的数据
// if(i % 4 == 0){
printf("Package[%d]:====================\n", i);
// printf(" ACC(g): x=%.3f, y=%.3f, z=%.3f\n", acc_g[0], acc_g[1], acc_g[2]);
// printf(" GYR(dps):x=%.3f, y=%.3f, z=%.3f\n", gyr_dps[0], gyr_dps[1], gyr_dps[2]);
printf(" MAG(Gs): x=%.3f, y=%.3f, z=%.3f\n", mag_gauss[0], mag_gauss[1], mag_gauss[2]);
printf(" TEMP(C): %.3f, PRESS(Pa): %.3f\n", temperature, pressure);
// }
}
printf("--- End of Packet ---\n\n");
}
/**
@ -217,187 +254,89 @@ void start_collect_fuc(void){
*
*/
void BLE_send_fuc(void){
BLE_send_data_t data_to_send;
if (circle_buffer_is_empty(&BLE_send_buff) == 0) {
circle_buffer_read(&BLE_send_buff, &data_to_send);
} else {
// 缓冲区为空，直接返回
return;
}
// --- 封装并发送六轴传感器数据 ---
{
// 协议定义: 包头(2) + 长度(1) + 类型(1) + 数据(12) = 16字节
const uint8_t IMU_PACKET_LEN = 16;
const uint8_t IMU_PAYLOAD_LEN = 13; // 类型(1) + 数据(12)
const uint8_t IMU_TYPE = 0x01;
ble_send_data_t send_data;
uint8_t send_buffer[483];
while(1){
os_sem_pend(&receiver_ready_sem, 0); //阻塞等待
circle_buffer_read(&g_ble_send_cb, &send_data);
uint8_t imu_packet[IMU_PACKET_LEN];
// 逐字节打包数据到 send_buffer, 采用小端模式
uint8_t *p = send_buffer;
*p++ = send_data.checkout_1;
*p++ = send_data.checkout_2;
*p++ = send_data.index;
// 填充包头
imu_packet[0] = 0xBB;
imu_packet[1] = 0xBE;
imu_packet[2] = IMU_PAYLOAD_LEN;
imu_packet[3] = IMU_TYPE;
// 拷贝六轴数据
// memcpy(&imu_packet[4], data_to_send.SC7U22_data, sizeof(data_to_send.SC7U22_data));
for (int i = 0; i < 6; i++) {
// SC7U22_data[i] 是一个 signed short (2字节)
// 将其低字节放在前面
imu_packet[4 + i * 2] = (uint8_t)(data_to_send.SC7U22_data[i] & 0xFF);
// 将其高字节放在后面
imu_packet[4 + i * 2 + 1] = (uint8_t)((data_to_send.SC7U22_data[i] >> 8) & 0xFF);
for (int i = 0; i < MPU_FIFO_LEN; i++) {
sensor_package_t *pkg = &send_data.sensor_package[i];
// 1. 打包六轴数据 (6 * int16_t)
for (int j = 0; j < 6; j++) {
*p++ = (uint8_t)(pkg->SC7U22_data[j] & 0xFF);
*p++ = (uint8_t)((pkg->SC7U22_data[j] >> 8) & 0xFF);
}
// 2. 打包地磁数据 (3 * int32_t)
for (int j = 0; j < 3; j++) {
*p++ = (uint8_t)(pkg->mmc5603nj_buffer[j] & 0xFF);
*p++ = (uint8_t)((pkg->mmc5603nj_buffer[j] >> 8) & 0xFF);
*p++ = (uint8_t)((pkg->mmc5603nj_buffer[j] >> 16) & 0xFF);
*p++ = (uint8_t)((pkg->mmc5603nj_buffer[j] >> 24) & 0xFF);
}
// 3. 打包温度数据 (int16_t)
*p++ = (uint8_t)(pkg->temperature & 0xFF);
*p++ = (uint8_t)((pkg->temperature >> 8) & 0xFF);
// 4. 打包气压数据 (uint32_t)
*p++ = (uint8_t)(pkg->pressure & 0xFF);
*p++ = (uint8_t)((pkg->pressure >> 8) & 0xFF);
*p++ = (uint8_t)((pkg->pressure >> 16) & 0xFF);
*p++ = (uint8_t)((pkg->pressure >> 24) & 0xFF);
#if 0
float acc_g[3];
float gyr_dps[3];
acc_g[0] = (float)send_data.sensor_package[i].SC7U22_data[0] / 2048.0f;
acc_g[1] = (float)send_data.sensor_package[i].SC7U22_data[1] / 2048.0f;
acc_g[2] = (float)send_data.sensor_package[i].SC7U22_data[2] / 2048.0f;
gyr_dps[0] = (float)send_data.sensor_package[i].SC7U22_data[3] * 0.061f;
gyr_dps[1] = (float)send_data.sensor_package[i].SC7U22_data[4] * 0.061f;
gyr_dps[2] = (float)send_data.sensor_package[i].SC7U22_data[5] * 0.061f;
printf(" ACC(g): x=%.3f, y=%.3f, z=%.3f\n", acc_g[0], acc_g[1], acc_g[2]);
printf(" GYR(dps):x=%.3f, y=%.3f, z=%.3f\n", gyr_dps[0], gyr_dps[1], gyr_dps[2]);
#endif
}
// xlog("imu %d\n",data_to_send.SC7U22_data[0]);
// xlog("imu_packet: 0x%x 0x%x 0x%x\n",imu_packet[4],imu_packet[5],imu_packet[6]);
send_data_to_ble_client(&imu_packet, IMU_PACKET_LEN);
}
// --- 封装并发送磁力计数据 ---
{
// 协议定义: 包头(2) + 长度(1) + 类型(1) + 数据(9) = 13字节
const uint8_t MAG_PACKET_LEN = 13;
const uint8_t MAG_PAYLOAD_LEN = 10; // 类型(1) + 数据(9)
const uint8_t MAG_TYPE = 0x02;
uint8_t mag_packet[MAG_PACKET_LEN];
// 填充包头
mag_packet[0] = 0xBB;
mag_packet[1] = 0xBE;
mag_packet[2] = MAG_PAYLOAD_LEN;
mag_packet[3] = MAG_TYPE;
// 拷贝磁力计数据
// memcpy(&mag_packet[4], data_to_send.mmc5603nj_buffer, sizeof(data_to_send.mmc5603nj_buffer));
for (int i = 0; i < 9; i++) {
mag_packet[4 + i] = data_to_send.mmc5603nj_buffer[i];
}
// xlog("mag: 0x%x 0x%x 0x%x\n",mag_packet[4],mag_packet[5],mag_packet[6]);
send_data_to_ble_client(&mag_packet, MAG_PACKET_LEN);
}
// --- 封装并发送压力机计数据 ---
{
// 协议定义: 包头(2) + 长度(1) + 类型(1) + 数据(6) = 10字节
const uint8_t PT_PACKET_LEN = 10;
const uint8_t PT_PAYLOAD_LEN = 7; // 类型(1) + 数据(6)
const uint8_t PT_TYPE = 0x03;
uint8_t pt_packet[PT_PACKET_LEN];
// 填充包头
pt_packet[0] = 0xBB;
pt_packet[1] = 0xBE;
pt_packet[2] = PT_PAYLOAD_LEN;
pt_packet[3] = PT_TYPE;
// 直接发送 int16_t 的二进制补码
pt_packet[4] = (uint8_t)(data_to_send.temperature & 0xFF); // 低字节
pt_packet[5] = (uint8_t)((data_to_send.temperature >> 8) & 0xFF); // 高字节
// 气压 (保持不变)
pt_packet[6] = (uint8_t)(data_to_send.pressure & 0xFF);
pt_packet[7] = (uint8_t)((data_to_send.pressure >> 8) & 0xFF);
pt_packet[8] = (uint8_t)((data_to_send.pressure >> 16) & 0xFF);
pt_packet[9] = (uint8_t)((data_to_send.pressure >> 24) & 0xFF);
send_data_to_ble_client(&pt_packet, PT_PACKET_LEN);
}
// --- 封装并发送速度数据 ---
{
// 协议定义: 包头(2) + 长度(1) + 类型(1) + 数据(2) = 6字节
const uint8_t SPEED_PACKET_LEN = 6;
const uint8_t SPEED_PAYLOAD_LEN = 3; // 类型(1) + 数据(2)
const uint8_t SPEED_TYPE = 0x04;
uint8_t speed_packet[SPEED_PACKET_LEN];
// 填充包头
speed_packet[0] = 0xBB;
speed_packet[1] = 0xBE;
// 填充长度
speed_packet[2] = SPEED_PAYLOAD_LEN;
// 填充类型
speed_packet[3] = SPEED_TYPE;
// 小端模式
speed_packet[4] = (uint8_t)(data_to_send.speed_cms & 0xFF); // 低字节
speed_packet[5] = (uint8_t)((data_to_send.speed_cms >> 8) & 0xFF); // 高字节
send_data_to_ble_client(&speed_packet, SPEED_PACKET_LEN);
}
// --- 封装并发送数据 ---
{
// 协议定义: 包头(2) + 长度(1) + 类型(1) + 数据(5) = 9字节
const uint8_t OTHER_PACKET_LEN = 9;
const uint8_t OTHER_PAYLOAD_LEN = 6; // 类型(1) + 数据(5)
const uint8_t OTHER_TYPE = 0x05;
uint8_t oher_packet[OTHER_PACKET_LEN];
// 填充包头
oher_packet[0] = 0xBB;
oher_packet[1] = 0xBE;
// 填充长度
oher_packet[2] = OTHER_PAYLOAD_LEN;
// 填充类型
oher_packet[3] = OTHER_TYPE;
// 小端模式
oher_packet[4] = (uint8_t)data_to_send.foot_state; // 数据来源
oher_packet[5] = (uint8_t)((data_to_send.timestamp_ms >> 0) & 0xFF); // LSB
oher_packet[6] = (uint8_t)((data_to_send.timestamp_ms >> 8) & 0xFF);
oher_packet[7] = (uint8_t)((data_to_send.timestamp_ms >> 16) & 0xFF);
oher_packet[8] = (uint8_t)((data_to_send.timestamp_ms >> 24) & 0xFF); // MSB
send_data_to_ble_client(&oher_packet, OTHER_PACKET_LEN);
// send_data_to_ble_client(send_buffer, 3 + sizeof(send_data.sensor_package)); // 发送数据
data_log(send_buffer);
}
}
void stop_BLE_send_fuc(void){
if (circle_buffer_is_empty(&BLE_send_buff)) {
close_process(&BLE_send_fuc_id,"BLE_send_fuc");
close_process(&stop_ble_send_fuc_id,"stop_BLE_send_fuc");
}
}
// ------------------------------------------------------------------------------------------------------
// ------------------------------------------------------------------------------------------------------
/**
* @brief 六轴静态校验
*
*/
void start_calibration(void){
create_process(&SC7U22_calibration_id,"SC7U22_calibration",NULL,SC7U22_static_calibration,10);
}
void stop_calibration(void){
close_process(&SC7U22_calibration_id, "SC7U22_calibration");
}
/**
* @brief 开始采集传感器数据并通过ble发送
*
*/
void start_clloct(void){
reset_ms_timer();
create_process(&start_collect_fuc_id,"start_collect",NULL,start_collect_fuc,10);
create_process(&BLE_send_fuc_id,"BLE_send_fuc",NULL,BLE_send_fuc,5);
os_task_create(sensor_collect_task,NULL,5,1024,32,"sensor_collect_task");
os_task_create(BLE_send_fuc,NULL,5,1024,32,"BLE_send_fuc");
}
/**
* @brief 停止采集和ble发送
*
*/
void stop_clloct(void){
close_process(&start_collect_fuc_id,"start_collect");
close_process(&BLE_send_fuc_id,"BLE_send_fuc");
// create_process(&stop_ble_send_fuc_id,"stop_BLE_send_fuc",NULL,stop_BLE_send_fuc,500); //等缓冲区内容发送完，才停止ble发送任务
os_task_del_req("sensor_collect_task");
os_task_del_req("BLE_send_fuc");
}
/**
@ -422,9 +361,9 @@ void xtell_task_create(void){
xlog("xtell_task_create\n");
circle_buffer_init(&BLE_send_buff, BLE_send_data, SENSOR_DATA_BUFFER_SIZE, sizeof(BLE_send_data_t));
circle_buffer_init(&g_ble_send_cb, g_sensor_data_storage, SENSOR_DATA_BUFFER_SIZE, sizeof(ble_send_data_t));
os_sem_create(&receiver_ready_sem, 0);
}
@ -434,66 +373,6 @@ void xtell_task_create(void){
//test
//
/**
* @brief 开始采集传感器数据和计算速度
*
*/
void sensor_task(void){
while(1){
// 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;
BLE_send_data_t BLE_send_data_tmp;
uint8_t mmc5603nj_buffer[9];
signed short acc_data_buf[3];
signed short gyr_data_buf[3];
float angle[3];
float quaternion_output[3];
float temperature = 0;
float pressure = 0;
// -- 读数据 --
SL_SC7U22_RawData_Read(acc_data_buf,gyr_data_buf);
mmc5603nj_read_origin_data(mmc5603nj_buffer);
// bmp280_read_originanl_data(&BLE_send_data_tmp.adc_P, &BLE_send_data_tmp.adc_T);
bmp280_read_data(&temperature, &pressure);
memcpy(&combined_raw_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&combined_raw_data[3], gyr_data_buf, 3 * sizeof(signed short));
// -- 四元数 --
status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, angle,NULL, 0, quaternion_output);
// -- 速度计算 --
memcpy(acc_data_buf, &combined_raw_data[0], 3 * sizeof(signed short));
memcpy(gyr_data_buf, &combined_raw_data[3], 3 * sizeof(signed short));
uint16_t speed = sensor_processing_task(acc_data_buf,gyr_data_buf,angle, quaternion_output);
// -- 数据包装进结构体 --
memcpy(&BLE_send_data_tmp.SC7U22_data[0], acc_data_buf, 3 * sizeof(signed short));
memcpy(&BLE_send_data_tmp.SC7U22_data[3], gyr_data_buf, 3 * sizeof(signed short));
memcpy(BLE_send_data_tmp.mmc5603nj_buffer, mmc5603nj_buffer, 9);
BLE_send_data_tmp.temperature = (int16_t)(temperature * 1000.0f);
BLE_send_data_tmp.pressure = (int32_t)(pressure * 1000.0f);
BLE_send_data_tmp.speed_cms = speed;
extern u8 foot_init;
BLE_send_data_tmp.foot_state = foot_init;
BLE_send_data_tmp.timestamp_ms = get_ms_timer();
// -- 放进缓冲区 --
if(circle_buffer_is_full(&BLE_send_buff) == 0){
circle_buffer_write(&BLE_send_buff, &BLE_send_data_tmp);
}
// xlog("=======sensor_read_data END\n");
os_time_dly(1);
}
}
#define BUFF_LEN 500
static signed char acc_data_buf[BUFF_LEN] = {0};
// 1. 定义一个全局的信号量
@ -556,6 +435,12 @@ void test_func(void){
for(int i = 0;i<BUFF_LEN;i++){
acc_data_buf[i] = i;
}
os_task_create(data_send_task,NULL,5,1024,32,"data_send_task");
SL_SC7U22_Config();
mmc5603nj_init();
bmp280_init();
os_task_create(BLE_send_fuc,NULL,5,1024,32,"BLE_send_fuc");
os_task_create(sensor_collect_task,NULL,5,1024,32,"sensor_collect_task");
// data_send_task();
}