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 "./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"
///////////////////////////////////////////////////////////////////////////////////////////////////
//宏定义
#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 -- 变量定义
//////////////////////////////////////////////////////////////////////////////////////////////////








#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 500 // 定义缓冲区可以存储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];  //pitch roll yaw
    float quaternion_output[4];  //四元数数据
} 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要发送的数据

static u8 mutex1 = 0;
static u8 mutex2 = 0;
static int count_test1 = 0;
static int count_test2 = 0;
/**
 * @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);
    // xlog("=======sensor_read_data middle 1\n");
    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);
        // status = SIX_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
        // status = Original_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);

        status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0, tmp.quaternion_output);

        int count = 0;
        if(count > 100){
            count = 0;
            char log_buffer[100]; // 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);
        }
        count++;
        
        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);
        // status = SIX_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
        // status = Original_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
        status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0, tmp.quaternion_output);

        memcpy(tmp.acc_data, &combined_raw_data[0], 3 * sizeof(signed short));
        memcpy(tmp.gyr_data, &combined_raw_data[3], 3 * sizeof(signed short));
        if(mutex1 == 0){
            mutex1 = 1;
            // count_test1++;
            // xlog("count_test_1: %d\n",count_test1);
            circle_buffer_write(&sensor_read, &tmp);
            mutex1 = 0;
        }
        extern  void ano_send_attitude_data(float rol, float pit, float yaw, uint8_t fusion_sta) ; 
        ano_send_attitude_data(tmp.angle[0],tmp.angle[1],tmp.angle[2], 1);    
    }
    // 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;
    }

    if(mutex1 == 0){
        mutex1 = 1;
        circle_buffer_read(&sensor_read, &tmp);
        mutex1 = 0;
    }else{
        return;
    }

    
    BLE_send_data_t data_by_calculate = sensor_processing_task(tmp.acc_data, tmp.gyr_data,tmp.angle,tmp.quaternion_output);

    

    if(circle_buffer_is_full(&sensor_send))
        return;
    
    if(mutex2 == 0){
        mutex2 = 1;
        circle_buffer_write(&sensor_send, &data_by_calculate);
        mutex2 = 0;
    }
    

    // extern void BLE_send_data();
    // BLE_send_data();
    // xlog("=======end\n");
}

static int count = 0;
extern char xt_Check_Flag;
void BLE_send_data(){
    //  xlog("=======start\n");
    if(circle_buffer_is_empty(&sensor_send)){
        // xlog("sensor_send_buffer: send buffer empty\n");
        return;
    }
    #ifdef XTELL_TEST
    // #if 0
    BLE_send_data_t tmp;
    if(mutex2 == 0){
        mutex2 = 1;
        circle_buffer_read(&sensor_send, &tmp);
        mutex2 = 0;
    }else{
        return;
    }
    
    
    if(count >=100){ 
        // extern debug_t debug2;
        // 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]);  
        // xlog("debug2.acc_magnitude:%.2f\n", debug2.acc_magnitude);  

        int num_chars_written;
        count = 0;
        char* division = "==========\n";
        send_data_to_ble_client(division,strlen(division));
        char log_buffer[100]; // 100个字符应该足够了

        
        // extern char iic_read_len;
        // extern char iic_write_result;
        // num_chars_written = snprintf(log_buffer, sizeof(log_buffer),"SL_SC7U22_Check=0x%d, %d, %d\n", xt_Check_Flag, iic_read_len, iic_write_result);  
        // send_data_to_ble_client(&log_buffer,strlen(log_buffer));

        memset(&log_buffer, 0, 100);
        #if 1
        // 使用 snprintf 进行格式化
        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));
        #endif
        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++;
    // xlog("=======end\n");
    #else
    #endif
}

//iic测试调用的
#if 0

static u16 xt_iic_test_id;
char log_buffer_1[100]; 
extern char sen_log_buffer_1[100];
extern char sen_log_buffer_2[100];
extern char sen_log_buffer_3[100];
extern char sen_log_buffer_4[100];
extern char sen_log_buffer_5[100];
extern char w_log_buffer_1[100];
extern char w_log_buffer_2[100];
extern char w_log_buffer_3[100];
extern char w_log_buffer_4[100];
extern char w_log_buffer_5[100];
void xt_iic_test(){

    char log_buffer[100]; 
    send_data_to_ble_client(&log_buffer_1,strlen(log_buffer_1));
    extern char iic_read_len;
    extern char iic_write_result;
    int num_chars_written = snprintf(log_buffer, sizeof(log_buffer),"SL_SC7U22_Check=0x%d,%d,%d\n", xt_Check_Flag, iic_read_len, iic_write_result);  
	extern void send_data_to_ble_client(const u8* data, u16 length);
    send_data_to_ble_client(&log_buffer,strlen(log_buffer));

    if(sen_log_buffer_1 != NULL)
        send_data_to_ble_client(&sen_log_buffer_1,strlen(sen_log_buffer_1));
    if(sen_log_buffer_2 != NULL)
        send_data_to_ble_client(&sen_log_buffer_2,strlen(sen_log_buffer_2));
    if(sen_log_buffer_3 != NULL)
        send_data_to_ble_client(&sen_log_buffer_3,strlen(sen_log_buffer_3));
    if(sen_log_buffer_4 != NULL)
        send_data_to_ble_client(&sen_log_buffer_4,strlen(sen_log_buffer_4));
    if(sen_log_buffer_5 != NULL)
        send_data_to_ble_client(&sen_log_buffer_5,strlen(sen_log_buffer_5));

    if(w_log_buffer_1 != NULL)
        send_data_to_ble_client(&w_log_buffer_1,strlen(w_log_buffer_1));
    if(w_log_buffer_2 != NULL)
        send_data_to_ble_client(&w_log_buffer_2,strlen(w_log_buffer_2));
    if(w_log_buffer_3 != NULL)
        send_data_to_ble_client(&w_log_buffer_3,strlen(w_log_buffer_3));
    if(w_log_buffer_4 != NULL)
        send_data_to_ble_client(&w_log_buffer_4,strlen(w_log_buffer_4));
    if(w_log_buffer_5 != NULL)
        send_data_to_ble_client(&w_log_buffer_5,strlen(w_log_buffer_5));

    // SL_SC7U22_Config();
}

#endif

#if 0
u16 xt_iic_test_id;
char hw_iic_init_result;
void xt_hw_iic_test(){
    char log_buffer[100]; 
    extern char iic_read_len;
    extern char iic_write_result;

    int num_chars_written = snprintf(log_buffer, sizeof(log_buffer),"init result:%d, SL_SC7U22_Check=0x%d,%d,%d\n",hw_iic_init_result, xt_Check_Flag, iic_read_len, iic_write_result);  
	extern void send_data_to_ble_client(const u8* data, u16 length);
    send_data_to_ble_client(&log_buffer,strlen(log_buffer));
}
#endif

void sensor_measure(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;
    
    // if(count_test1 >= 100){
    //     count_test1 = 0;
    //     xlog("count_test1\n");
    // }
    // count_test1++;
    
    // static sensor_data_t tmp;
    // SL_SC7U22_RawData_Read(tmp.acc_data,tmp.gyr_data);
    // // xlog("=======sensor_read_data middle 1\n");
    // 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);
    //     // status = SIX_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
    //     // status = Original_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0);
    //     status = Q_SL_SC7U22_Angle_Output(1, combined_raw_data, tmp.angle, 0, tmp.quaternion_output);

    //     int count = 0;
    //     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);
    //     }
    //     count++;
        
    //     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);
    //     // status = SIX_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
    //     // status = Original_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0);
    //     status = Q_SL_SC7U22_Angle_Output(0, combined_raw_data, tmp.angle, 0, tmp.quaternion_output);
    //     memcpy(tmp.acc_data, &combined_raw_data[0], 3 * sizeof(signed short));
    //     memcpy(tmp.gyr_data, &combined_raw_data[3], 3 * sizeof(signed short));
    //     BLE_send_data_t data_by_calculate = sensor_processing_task(tmp.acc_data, tmp.gyr_data,tmp.angle, tmp.quaternion_output);
    //     extern void ano_send_attitude_data(float rol, float pit, float yaw, uint8_t fusion_sta) ; 
    //     ano_send_attitude_data(tmp.angle[0],tmp.angle[1],tmp.angle[2], 1);    

        
    // }

    mmc5603nj_mag_data_t mag_data;
    // mmc5603nj_read_mag_data(&mag_data);

    // if(count_test2 > 100){
    //     count_test2++;
        printf("Mag X: %.4f, Y: %.4f, Z: %.4f Gauss\n", mag_data.x, mag_data.y, mag_data.z);
    // }
    // count_test2++;
    // xlog("=======sensor_read_data END\n");
}


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
    int ret = soft_iic_init(0);
    int num_chars_written = snprintf(log_buffer_1, sizeof(log_buffer_1),"init iic: %d\n", ret);  
    
    #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);
    // delay_2ms(10);

    SL_SC7U22_Config();
    // if (mmc5603nj_init() != 0) {
    //     xlog("MMC5603NJ initialization failed!\n");
    // }
    // xlog("MMC5603NJ PID: 0x%02X\n", mmc5603nj_get_pid());
    // // 启用连续测量模式
    // mmc5603nj_enable_continuous_mode();
    // xlog("Continuous measurement mode enabled.\n");

    //iic总线设备扫描
    extern void i2c_scanner_probe(void);
    i2c_scanner_probe();

    if(bmp280_init() != 0){
        xlog("bmp280 init error\n");
    }
    float temp, press;
    bmp280_read_data(&temp, &press);
    xlog("get temp: %d, get press: %d\n",temp, press);




    xlog("xtell_task_create\n");
    // 初始化环形缓冲区
    // circle_buffer_init(&sensor_cb, sensor_data_buffer, SENSOR_DATA_BUFFER_SIZE);

    
    ano_protocol_init(115200);
    create_process(&calculate_data_id, "calculate",NULL, sensor_measure, 2000);

    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(&sensor_read_data_id, "read",NULL, sensor_read_data, 10);
// 
    // create_process(&calculate_data_id, "calculate",NULL, calculate_data, 4);

    // create_process(&ble_send_data_id, "send",NULL, BLE_send_data, 1);

    #if 0
    hw_iic_init_result = ret;
    create_process(&xt_iic_test_id,"iic_test",NULL,xt_hw_iic_test,1000);
    #endif

}