445 lines
15 KiB
C
445 lines
15 KiB
C
#include "system/includes.h"
|
||
#include "btstack/btstack_task.h"
|
||
#include "app_config.h"
|
||
#include "app_action.h"
|
||
#include "asm/pwm_led.h"
|
||
#include "tone_player.h"
|
||
#include "ui_manage.h"
|
||
#include "gpio.h"
|
||
#include "app_main.h"
|
||
#include "asm/charge.h"
|
||
#include "update.h"
|
||
#include "app_power_manage.h"
|
||
#include "audio_config.h"
|
||
#include "app_charge.h"
|
||
#include "bt_profile_cfg.h"
|
||
#include "dev_manager/dev_manager.h"
|
||
#include "update_loader_download.h"
|
||
#include "./sensor/SC7U22.h"
|
||
#include "./buffer/circle_buffer.h"
|
||
#include "btstack/avctp_user.h"
|
||
#include "calculate/skiing_tracker.h"
|
||
#include "xtell.h"
|
||
///////////////////////////////////////////////////////////////////////////////////////////////////
|
||
//宏定义
|
||
#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
|
||
//
|
||
///////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||
//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 -- 函数定义
|
||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
||
|
||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||
//START -- 变量定义
|
||
|
||
// --- 任务ID ---
|
||
static u16 xtell_i2c_test_id;
|
||
static u16 collect_data_id;
|
||
static u16 ble_send_data_id;
|
||
static u16 sensor_read_data_id;
|
||
static u16 calculate_data_id;
|
||
|
||
// --- 环形缓冲区 ---
|
||
#define SENSOR_DATA_BUFFER_SIZE 512
|
||
static u8 sensor_data_buffer[SENSOR_DATA_BUFFER_SIZE];
|
||
static circle_buffer_t sensor_cb;
|
||
|
||
|
||
//--- test ---
|
||
// 全局变量
|
||
u16 gsensor_id=0;
|
||
u16 test_id=0;
|
||
//END -- 变量定义
|
||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
||
|
||
// /**
|
||
// * @brief 向匿名上位机发送数据帧
|
||
// * @param function_id 功能码 (例如 0x01, 0x03)
|
||
// * @param data 指向 int16_t 数据数组的指针 (例如加速度、欧拉角)
|
||
// * @param data_len int16_t 数据的个数 (例如发送6轴数据时为6，发送3个欧拉角时为3)
|
||
// * @param status_byte 附加的状态字节 (例如 SHOCK_STA 或 FUSION_STA)
|
||
// */
|
||
// void send_data_anotc(uint8_t function_id, int16_t* data, uint8_t data_len, uint8_t status_byte) {
|
||
// // 定义一个足够大的缓冲区来构建数据帧
|
||
// // 最大长度(ID 0x01): 1(HEAD)+1(D_ADDR)+1(ID)+1(LEN)+13(DATA)+1(SC)+1(AC) = 19字节
|
||
// uint8_t buffer[32];
|
||
|
||
// uint8_t data_payload_len = data_len * sizeof(int16_t) + sizeof(uint8_t);
|
||
|
||
// // 1. 填充帧头和地址
|
||
// buffer[0] = 0xAA; // 帧头 HEAD
|
||
// buffer[1] = 0xFF; // 目标地址 D_ADDR
|
||
|
||
// // 2. 填充功能码和数据长度
|
||
// buffer[2] = function_id;
|
||
// buffer[3] = data_payload_len;
|
||
|
||
// // 3. 填充数据内容 (DATA)
|
||
// // 首先使用 memcpy 拷贝主要的 int16_t 数组数据
|
||
// // &buffer[4] 是数据区的起始地址
|
||
// memcpy(&buffer[4], (uint8_t*)data, data_len * sizeof(int16_t));
|
||
// // 然后在数据区末尾填充状态字节
|
||
// buffer[4 + data_len * sizeof(int16_t)] = status_byte;
|
||
|
||
// // 4. 计算校验和 (SC 和 AC)
|
||
// uint8_t sum_check = 0;
|
||
// uint8_t add_check = 0;
|
||
|
||
// // SC: 和校验 (从帧头到数据区最后一个字节)
|
||
// for (int i = 0; i < 4 + data_payload_len; ++i) {
|
||
// sum_check += buffer[i];
|
||
// }
|
||
|
||
// // 将SC填充到缓冲区
|
||
// buffer[4 + data_payload_len] = sum_check;
|
||
|
||
// // AC: 附加校验 (从帧头到SC)
|
||
// for (int i = 0; i < 4 + data_payload_len + 1; ++i) {
|
||
// add_check += buffer[i];
|
||
// }
|
||
|
||
// // 将AC填充到缓冲区
|
||
// buffer[4 + data_payload_len + 1] = add_check;
|
||
|
||
// // 5. 发送整个数据帧
|
||
// uint16_t frame_length = 4 + data_payload_len + 2;
|
||
// // Serial_Send_Buffer(buffer, frame_length);
|
||
// for (int i = 0; i < frame_length; ++i) {
|
||
// // 使用 %c 格式化字符来发送单个字节的原始值
|
||
// printf("%c", buffer[i]);
|
||
// }
|
||
// printf("\n");
|
||
// }
|
||
|
||
void ble_send_data(signed short *acc_gyro_input, float *Angle_output){
|
||
char buffer[50]; //一次最多发送50字节
|
||
u8 len = 0;
|
||
//AA FF 01 六轴数据 EE
|
||
//TO DO
|
||
send_data_to_ble_client(&buffer,len);
|
||
|
||
|
||
//AA FF 02 欧若拉角数据 EE
|
||
// TO DO
|
||
send_data_to_ble_client(&buffer,len);
|
||
}
|
||
|
||
|
||
// 从环形缓冲区读取数据并发送
|
||
void send_sensor_data_task(void) {
|
||
// printf("xtell_ble_send\n");
|
||
}
|
||
|
||
#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};
|
||
signed short acc_gyro_input[6] = {0};
|
||
float Angle_output[3] = {0};
|
||
// xlog("============start\n");
|
||
|
||
SL_SC7U22_RawData_Read(acc_data_buf,gyr_data_buf);
|
||
|
||
BLE_send_data = sensor_processing_task(acc_data_buf, gyr_data_buf);
|
||
u8 data[50];
|
||
data[0] = 0xBB;
|
||
data[1] = 0xBE;
|
||
data[2] = 0x01;
|
||
data[3] = sizeof(BLE_send_data_t); //后续包的数据长度
|
||
// send_data_to_ble_client(&data,sizeof(BLE_send_data_t)+4);
|
||
memcpy(&data[4], &BLE_send_data, sizeof(BLE_send_data_t));
|
||
|
||
static int count = 0;
|
||
if(count >=10){
|
||
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", // 格式化字符串
|
||
BLE_send_data.skiing_state, // 第一个 %d 的参数
|
||
BLE_send_data.speed_cms, // 第二个 %d 的参数
|
||
BLE_send_data.distance_cm // 第三个 %d 的参数
|
||
);
|
||
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
|
||
|
||
extern BLE_KS_send_data_t KS_data;
|
||
memset(&log_buffer, 0, 100);
|
||
num_chars_written = snprintf(
|
||
log_buffer,
|
||
sizeof(log_buffer),
|
||
"Acc:%d, %d, %d\n",
|
||
KS_data.acc_KS[0],KS_data.acc_KS[1],KS_data.acc_KS[2]
|
||
); // cm/s^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_dps:%d, %d, %d\n",
|
||
KS_data.gyr_KS_dps[0],
|
||
KS_data.gyr_KS_dps[1],
|
||
KS_data.gyr_KS_dps[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: %d, %d, %d\n",
|
||
KS_data.angle_KS[0],
|
||
KS_data.angle_KS[1],
|
||
KS_data.angle_KS[2]
|
||
);
|
||
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
|
||
|
||
extern debug_t debug1;
|
||
extern debug_t debug2;
|
||
memset(&log_buffer, 0, 100);
|
||
num_chars_written = snprintf(
|
||
log_buffer,
|
||
sizeof(log_buffer),
|
||
"debug:%.2f,%.2f,%.2f(%.2f),%.2f\n",
|
||
debug1.acc_variance,
|
||
debug1.gyr_variance,
|
||
debug1.acc_magnitude,
|
||
debug2.acc_magnitude, //滤波后的加速度
|
||
debug1.gyr_magnitude
|
||
);
|
||
send_data_to_ble_client(&log_buffer,strlen(log_buffer));
|
||
|
||
xlog("Call interval\n");
|
||
}
|
||
count++;
|
||
|
||
memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
|
||
memset(&data, 0, 50);
|
||
// xlog("end============\n");
|
||
|
||
}
|
||
#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);
|
||
// xlog("hw_iic_init result:%d\n",ret);
|
||
// //初始化传感器
|
||
// 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_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));
|
||
|
||
|
||
//初始化滑雪追踪器
|
||
// SkiingTracker_Init(&skiing_data);
|
||
xlog("SkiingTracker_Init\n");
|
||
|
||
// create_process(&test_id, "test",NULL, test, (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);
|
||
|
||
|
||
}
|