99_7018_lmx/apps/earphone/xtell_Sensor/send_data.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 "LIS2DH12.h"
#include "circle_buffer.h"
#include "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 send_data_id;

// --- 环形缓冲区 ---
#define SENSOR_DATA_BUFFER_SIZE 512
static u8 sensor_data_buffer[SENSOR_DATA_BUFFER_SIZE];
static circle_buffer_t sensor_cb;

BLE_send_data_t BLE_send_data;
//--- 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");
}

#ifdef XTELL_TEST
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};

    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));
        
       
    }
    count++;

    memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
    memset(&data, 0, 50);

}
#else

void BLE_send_data(){
    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};

    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;
        send_data_to_ble_client(&data,sizeof(BLE_send_data_t)+4);
    }
    count++;

    memset(&BLE_send_data, 0, sizeof(BLE_send_data_t));
    memset(&data, 0, 50);
}
#endif

void gsensor_test(){

    sys_timer_del(gsensor_id);

}


void xtell_task_create(void){

    // int ret = hw_iic_init(0);
    // xlog("hw_iic_init result:%d\n",ret);
    // //初始化传感器
    // SL_SC7U22_Config();

    soft_iic_init(0);
    gpio_set_direction(IO_PORTE_05,0);          //设置PE5 输出模式
    gpio_set_pull_up(IO_PORTE_05,1);
    gpio_direction_output(IO_PORTE_05,1);

    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);


    //初始化滑雪追踪器
    // SkiingTracker_Init(&skiing_data); 
    xlog("SkiingTracker_Init\n");

    // create_process(&gsensor_id, "gsensor",NULL, gsensor_test, 1000);
    create_process(&test_id, "test",NULL, test, (int)(DELTA_TIME*1000));
    
}