#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" #include "./ano/ano_protocol.h" #include "./sensor/MMC56.h" #include "./sensor/BMP280.h" #include "./sensor/AK8963.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 500 // 定义缓冲区可以存储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); //END -- 函数定义 ////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////// //START -- 变量定义 typedef struct { // -- 六轴 -- signed short SC7U22_data[6]; // -- 磁力计 -- uint8_t mmc5603nj_buffer[9]; // -- 速度 -- uint16_t speed_cms; // -- 气压计 -- int adc_P; int adc_T; } 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]; //END -- 变量定义 ////////////////////////////////////////////////////////////////////////////////////////////////// /** * @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]); } 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]; // -- 读数据 -- 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); 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.speed_cms = speed; // -- 放进缓冲区 -- if(circle_buffer_is_full(&BLE_send_buff) == 0){ circle_buffer_write(&BLE_send_buff, &BLE_send_data_tmp); } } /** * @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) + 数据(8) = 12字节 const uint8_t PT_PACKET_LEN = 12; const uint8_t PT_PAYLOAD_LEN = 9; // 类型(1) + 数据(8) 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; // 打包压力数据 data_to_send.adc_P (占 pt_packet[4] 到 pt_packet[7]) pt_packet[4] = (uint8_t)(data_to_send.adc_P & 0xFF); // 最低字节 (LSB) pt_packet[5] = (uint8_t)((data_to_send.adc_P >> 8) & 0xFF); pt_packet[6] = (uint8_t)((data_to_send.adc_P >> 16) & 0xFF); pt_packet[7] = (uint8_t)((data_to_send.adc_P >> 24) & 0xFF); // 最高字节 (MSB) // 打包温度数据 data_to_send.adc_T (占 pt_packet[8] 到 pt_packet[11]) pt_packet[8] = (uint8_t)(data_to_send.adc_T & 0xFF); // 最低字节 (LSB) pt_packet[9] = (uint8_t)((data_to_send.adc_T >> 8) & 0xFF); pt_packet[10] = (uint8_t)((data_to_send.adc_T >> 16) & 0xFF); pt_packet[11] = (uint8_t)((data_to_send.adc_T >> 24) & 0xFF); // 最高字节 (MSB) 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); } } static u8 bmp280_test_id = 0; 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)); bmp280_init(); extern void bmp280_test(void); xlog("barometer start measeure\n"); // create_process(&bmp280_test_id,"bmp280_test",NULL, bmp280_test, 100); float Temp = 0; float Press = 0; xlog("test_func\n"); bmp280_read_data(&Temp, &Press); xlog("Temp:%.2f, Press:%.2f\n",Temp,Press); } ////////////////////////////////////////////////////////////////////////////// //test // void bmp280_test(void){ }