459 lines
15 KiB
C
459 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 <math.h>
|
||
#include <string.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"
|
||
#include "./ano/ano_protocol.h"
|
||
#include "./sensor/MMC56.h"
|
||
#include "./sensor/BMP280.h"
|
||
#include "./sensor/AK8963.h"
|
||
#include "asm/rtc.h"
|
||
#include "system/timer.h"
|
||
#include "adv_time_stamp_setting.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
|
||
|
||
#define SENSOR_DATA_BUFFER_SIZE 1000 // 定义缓冲区可以存储XXX个sensor_data_t元素
|
||
|
||
//
|
||
///////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||
//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);
|
||
extern void close_process(u16* pid,char* name);
|
||
void start_collect_fuc(void);
|
||
void BLE_send_fuc(void);
|
||
//END -- 函数定义
|
||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
||
|
||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||
//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;
|
||
|
||
} BLE_send_data_t;
|
||
|
||
static int count = 0;
|
||
|
||
// --- 环形缓冲区 ---
|
||
static circle_buffer_t BLE_send_buff; // 环形缓冲区管理结构体
|
||
BLE_send_data_t BLE_send_data[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;
|
||
|
||
//END -- 变量定义
|
||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
||
|
||
// 重置计时器
|
||
void reset_ms_timer(void) {
|
||
timer_offset_ms = sys_timer_get_ms();
|
||
xlog("Timer has been reset.\n");
|
||
}
|
||
|
||
// 获取从上次重置后经过的毫秒数
|
||
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 开始采集传感器数据和计算速度
|
||
*
|
||
*/
|
||
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];
|
||
|
||
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");
|
||
|
||
|
||
}
|
||
|
||
/**
|
||
* @brief ble数据发送函数
|
||
*
|
||
*/
|
||
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;
|
||
|
||
uint8_t imu_packet[IMU_PACKET_LEN];
|
||
|
||
// 填充包头
|
||
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);
|
||
}
|
||
// 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);
|
||
}
|
||
}
|
||
|
||
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);
|
||
}
|
||
|
||
|
||
/**
|
||
* @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发送任务
|
||
}
|
||
|
||
/**
|
||
* @brief 初始化，在app_main.c的app_main函数被调用
|
||
*
|
||
*/
|
||
void xtell_task_create(void){
|
||
|
||
#if TCFG_GSENOR_USER_IIC_TYPE
|
||
int ret = hw_iic_init(0);
|
||
xlog("init iic result:%d\n", ret); //返回0成功
|
||
#else
|
||
int ret = soft_iic_init(0);
|
||
int num_chars_written = snprintf(log_buffer_1, sizeof(log_buffer_1),"init iic: %d\n", ret);
|
||
|
||
#endif
|
||
|
||
// MPU9250_Mag_Init();
|
||
//iic总线设备扫描
|
||
// extern void i2c_scanner_probe(void);
|
||
// i2c_scanner_probe();
|
||
|
||
xlog("xtell_task_create\n");
|
||
|
||
circle_buffer_init(&BLE_send_buff, BLE_send_data, SENSOR_DATA_BUFFER_SIZE, sizeof(BLE_send_data_t));
|
||
|
||
|
||
|
||
}
|
||
|
||
|
||
|
||
//////////////////////////////////////////////////////////////////////////////
|
||
//test
|
||
//
|
||
|
||
void bmp280_test(void){
|
||
|
||
}
|
||
|
||
|
||
|
||
void gsensor_test(void){
|
||
#if 1
|
||
if(count >= 5){
|
||
xlog("==============time============\n");
|
||
|
||
u32 current_time = get_ms_timer();
|
||
xlog("Time since last reset: %u ms\n", current_time);
|
||
|
||
// 为了演示，我们在这里调用重置
|
||
if (current_time > 5000) { // 比如超过5秒就重置一次
|
||
reset_ms_timer();
|
||
}
|
||
|
||
count = 0;
|
||
}
|
||
count++;
|
||
#endif
|
||
} |