486 lines
17 KiB
C
486 lines
17 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 "./sensor/WF282A.h"
|
||
#include "asm/rtc.h"
|
||
#include "system/timer.h"
|
||
#include "adv_time_stamp_setting.h"
|
||
#include "btstack/le/le_user.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 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组六轴数据
|
||
|
||
//
|
||
///////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||
//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 BLE_send_fuc(void);
|
||
//END -- 函数定义
|
||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
||
|
||
//////////////////////////////////////////////////////////////////////////////////////////////////
|
||
//START -- 变量定义
|
||
static u32 timer_offset_ms = 0;
|
||
|
||
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));
|
||
|
||
typedef struct{
|
||
uint8_t checkout_1;
|
||
uint8_t checkout_2;
|
||
uint8_t foot; //1：左脚，2：右脚
|
||
uint8_t package_index;
|
||
sensor_package_t sensor_package[MPU_FIFO_LEN];//一次蓝牙发送MPU_FIFO_LEN组传感器数据
|
||
}ble_send_data_t; //一次蓝牙发送的数据内容
|
||
|
||
// --- 环形缓冲区 ---
|
||
static circle_buffer_t g_ble_send_cb; // 环形缓冲区管理结构体
|
||
static ble_send_data_t g_sensor_data_storage[SENSOR_DATA_BUFFER_SIZE]; //缓冲区
|
||
|
||
extern u8 foot_init;
|
||
static OS_SEM receiver_ready_sem; // 用于启动同步的信号量
|
||
|
||
static const uart_bus_t *uart_bus = NULL;
|
||
|
||
static u16 test_id = 0;
|
||
//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 sensor_collect_task(void){
|
||
static 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;
|
||
static int SL_data_index = 0;
|
||
u8 package_index = 1;
|
||
int tmp_index = 0;
|
||
|
||
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);
|
||
|
||
SL_SC7U22_FIFO_Read(accx_buf,accy_buf,accz_buf,gyrx_buf,gyry_buf,gyrz_buf); //一次性读取内置fifo的数据
|
||
for(int i = 0; i < MPU_FIFO_LEN/4; i++){
|
||
tmp_index = SL_data_index + i;
|
||
// if(tmp_index >= MPU_FIFO_LEN-1) tmp_index = MPU_FIFO_LEN-1;
|
||
send_data.sensor_package[tmp_index].SC7U22_data[0] = accx_buf[i]; //acc_x
|
||
send_data.sensor_package[tmp_index].SC7U22_data[1] = accy_buf[i]; //acc_y
|
||
send_data.sensor_package[tmp_index].SC7U22_data[2] = accz_buf[i]; //acc_z
|
||
send_data.sensor_package[tmp_index].SC7U22_data[3] = gyrx_buf[i]; //gyr_x
|
||
send_data.sensor_package[tmp_index].SC7U22_data[4] = gyry_buf[i]; //gyr_y
|
||
send_data.sensor_package[tmp_index].SC7U22_data[5] = gyrz_buf[i]; //gyr_z
|
||
|
||
// xlog(" Acc_x : %4d, Acc_y : %4d, Acc_z : %4d,\r\n", send_data.sensor_package[tmp_index].SC7U22_data[0], send_data.sensor_package[tmp_index].SC7U22_data[1], send_data.sensor_package[tmp_index].SC7U22_data[2]);
|
||
#if 0
|
||
float acc_g[3];
|
||
float gyr_dps[3];
|
||
acc_g[0] = (float)send_data.sensor_package[tmp_index].SC7U22_data[0] / 2048.0f;
|
||
acc_g[1] = (float)send_data.sensor_package[tmp_index].SC7U22_data[1] / 2048.0f;
|
||
acc_g[2] = (float)send_data.sensor_package[tmp_index].SC7U22_data[2] / 2048.0f;
|
||
gyr_dps[0] = (float)send_data.sensor_package[tmp_index].SC7U22_data[3] * 0.061f;
|
||
gyr_dps[1] = (float)send_data.sensor_package[tmp_index].SC7U22_data[4] * 0.061f;
|
||
gyr_dps[2] = (float)send_data.sensor_package[tmp_index].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
|
||
}
|
||
SL_data_index += MPU_FIFO_LEN/4;
|
||
|
||
|
||
if(interval >= 4){
|
||
interval = 0;
|
||
SL_data_index = 0;
|
||
#if BMP280
|
||
bmp280_read_data(&temperature, &pressure);//每40ms读取一次
|
||
#else
|
||
WF_GET_Temperature_Pressure(&temperature, &pressure);
|
||
#endif
|
||
for(int i = 0;i<MPU_FIFO_LEN;i++){
|
||
send_data.sensor_package[i].temperature = (int16_t)(temperature * 1000.0f);
|
||
send_data.sensor_package[i].pressure = (int32_t)(pressure * 1000.0f);
|
||
}
|
||
|
||
xlog("temperature: %.2f,pressure: %.2f\n",temperature,pressure);
|
||
// xlog("fifo_num:%d\n",fifo_num);
|
||
|
||
send_data.checkout_1 = 0xBE;
|
||
send_data.checkout_2 = 0xBB;
|
||
send_data.foot = foot_init;
|
||
send_data.package_index = package_index;
|
||
// circle_buffer_write(&g_ble_send_cb, &send_data);
|
||
// os_sem_post(&receiver_ready_sem); //通知另一个发送任务
|
||
|
||
memset(&send_data, 0, sizeof(ble_send_data_t));
|
||
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));
|
||
|
||
package_index++;
|
||
if(package_index >= 0xFF) package_index = 1;
|
||
// xlog("=====================%d============================\n",get_ms_timer());
|
||
}
|
||
os_time_dly(1); //10ms为单位
|
||
}
|
||
}
|
||
|
||
|
||
void data_log(uint8_t* data){
|
||
static u8 imu_airplane[MPU_FIFO_LEN][12];
|
||
// 检查数据包头部
|
||
if (data[0] != 0xBE || data[1] != 0xBB) {
|
||
printf("Error: Invalid data packet header.\n");
|
||
return;
|
||
}
|
||
|
||
//左右脚
|
||
uint8_t package_foot = data[2];
|
||
|
||
// 解析包索引
|
||
uint8_t package_index = data[3];
|
||
printf("--- Parsing Data Packet Index: %d ---\n", package_index);
|
||
|
||
uint8_t* p = &data[4]; // 指向数据负载的起始位置
|
||
|
||
// 循环解析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_airplane[i][2*j] = p[0];
|
||
imu_airplane[i][2*j+1] = p[1];
|
||
// 小端模式: 低字节在前, 高字节在后
|
||
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 % 8 == 0){
|
||
printf(" ==================ble index: %d\n", *p);
|
||
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");
|
||
|
||
extern void uartSendData(void *buf, u16 len) ; // 确保u16是uint16_t或unsigned short
|
||
// uartSendData(imu_airplane, sizeof(imu_airplane));
|
||
uartSendData(data, 484); // 发送总共17字节
|
||
}
|
||
|
||
/**
|
||
* @brief ble数据发送函数
|
||
*
|
||
*/
|
||
void BLE_send_fuc(void){
|
||
ble_send_data_t send_data;
|
||
uint8_t send_buffer[484];
|
||
while(1){
|
||
os_sem_pend(&receiver_ready_sem, 0); //阻塞等待
|
||
circle_buffer_read(&g_ble_send_cb, &send_data);
|
||
|
||
// 逐字节打包数据到 send_buffer, 采用小端模式
|
||
uint8_t *p = send_buffer;
|
||
*p++ = send_data.checkout_1;
|
||
*p++ = send_data.checkout_2;
|
||
*p++ = send_data.foot;
|
||
*p++ = send_data.package_index;
|
||
|
||
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
|
||
}
|
||
|
||
|
||
extern void uartSendData(void *buf, u16 len) ; // 确保u16是uint16_t或unsigned short
|
||
uartSendData(send_buffer, 484); // 发送总共17字节
|
||
send_data_to_ble_client(send_buffer, 484); // 发送数据
|
||
|
||
// data_log(send_buffer);
|
||
}
|
||
}
|
||
|
||
|
||
|
||
// ------------------------------------------------------------------------------------------------------
|
||
// ------------------------------------------------------------------------------------------------------
|
||
|
||
|
||
|
||
/**
|
||
* @brief 开始采集传感器数据并通过ble发送
|
||
*
|
||
*/
|
||
void start_clloct(void){
|
||
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){
|
||
os_task_del("sensor_collect_task");
|
||
os_task_del("BLE_send_fuc");
|
||
}
|
||
|
||
/**
|
||
* @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(&g_ble_send_cb, g_sensor_data_storage, SENSOR_DATA_BUFFER_SIZE, sizeof(ble_send_data_t));
|
||
|
||
os_sem_create(&receiver_ready_sem, 0);
|
||
|
||
extern void test_uart_init(void);
|
||
test_uart_init();
|
||
|
||
}
|
||
|
||
|
||
/**
|
||
* @brief 发给上位机
|
||
*
|
||
*/
|
||
void test_uart_init(void){
|
||
#if TCFG_UART0_ENABLE == 0
|
||
static u8 buff[40];
|
||
struct uart_platform_data_t u_arg = {0};
|
||
u_arg.tx_pin = IO_PORT_DP;
|
||
u_arg.rx_cbuf = buff;
|
||
u_arg.rx_cbuf_size = 32;
|
||
u_arg.frame_length = 6;
|
||
u_arg.rx_timeout = 100;
|
||
u_arg.isr_cbfun = NULL;
|
||
u_arg.baud = 1000000;
|
||
u_arg.is_9bit = 0;
|
||
uart_bus = uart_dev_open(&u_arg);
|
||
#endif
|
||
}
|
||
|
||
void uartSendData(void *buf, u16 len) //发送数据的接口。
|
||
{
|
||
#if TCFG_UART0_ENABLE == 0
|
||
if (uart_bus) {
|
||
uart_bus->write(buf, len); //把数据写到DMA
|
||
}
|
||
#endif
|
||
}
|
||
//////////////////////////////////////////////////////////////////////////////
|
||
//test
|
||
//
|
||
|
||
#define BUFF_LEN 500
|
||
static signed char acc_data_buf[BUFF_LEN] = {0};
|
||
// 1. 定义一个全局的信号量
|
||
static OS_SEM ble_send_sem;
|
||
|
||
|
||
int j = 0;
|
||
void sensor_test_task(void){
|
||
float temperature = 0;
|
||
float pressure = 0;
|
||
while(1){
|
||
WF_GET_Temperature_Pressure(&temperature, &pressure);
|
||
xlog("temperature: %.3f,pressure: %.3f\n",temperature,pressure);
|
||
os_time_dly(100);
|
||
}
|
||
|
||
}
|
||
static u16 gtest_id = 0;
|
||
void test_func(void){
|
||
// a. 初始化信号量，初始值为0
|
||
// os_sem_create(&ble_send_sem, 0);
|
||
|
||
// b. 注册回调函数，让协议栈知道在准备好时该调用谁
|
||
// struct ble_server_operation_t *ble_ops;
|
||
// ble_get_server_operation_table(&ble_ops);
|
||
// ble_ops->regist_wakeup_send(NULL, on_ble_can_send);
|
||
for(int i = 0;i<BUFF_LEN;i++){
|
||
acc_data_buf[i] = i;
|
||
}
|
||
|
||
// SL_SC7U22_Config();
|
||
// mmc5603nj_init();
|
||
#if BMP280
|
||
BMP280_init();
|
||
#else
|
||
WF_Init();
|
||
#endif
|
||
// 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");
|
||
os_task_create(sensor_test_task,NULL,5,1024,32,"sensor_test");
|
||
// data_send_task();
|
||
}
|
||
|
||
|