#include "usr_config.h" #include "hal_battery.h" #include "nrf_drv_saadc.h" #include "bsp_time.h" #include "system.h" #include "hal_led.h" #include "app_overturn.h" #include "hal_wearshoes.h" #include "bll_imu.h" #include "ble_comm.h" #define SLIDE_WINDOW_LEN 5 //全功率 static const fml_imu_param_t game_front_param={ .acc_power_mode = FML_IMU_ACC_POWER_MODE_NORMAL, //前脚 - 加速度正常模式 .gry_power_mode = FML_IMU_GRY_POWER_MODE_NORMAL, //前脚 - 陀螺仪正常模式 .timestamp_resolution = FML_IMU_TIMESTAMP_25US, //前脚 - 时间戳25US精度 .timestamp_switch = FML_IMU_TIMESTAMP_ON, //前脚 - 时间戳开启 .acc_fs = FML_IMU_ACC_FS_16G, //前脚 - 加速度量程 - 16G .gry_fs = FML_IMU_GRY_FS_2000DPS, //前脚 - 陀螺仪量程 - 2000DPS .mag_fs = FML_IMU_MAG_FS_30GS, //前脚 - 地磁计量程 - 30GS .acc_odr = FML_IMU_ACC_ODR_104HZ, //前脚 - 加速度采样频率 - 104HZ .gry_odr = FML_IMU_GRY_ODR_104HZ, //前脚 - 陀螺仪采样频率 - 104HZ .mag_odr = FML_IMU_MAG_ODR_200HZ, //前脚 - 地磁计采样频率 - 200HZ .fifo_odr = FML_IMU_FIFO_ODR_104HZ, }; static const fml_imu_param_t game_back_param={ .acc_power_mode = FML_IMU_ACC_POWER_MODE_NORMAL, //后脚 - 加速度正常模式 .gry_power_mode = FML_IMU_GRY_POWER_MODE_NORMAL, //后脚 - 陀螺仪正常模式 .timestamp_resolution = FML_IMU_TIMESTAMP_25US, //后脚 - 时间戳25US精度 .timestamp_switch = FML_IMU_TIMESTAMP_OFF, //后脚 - 时间戳关闭 .acc_fs = FML_IMU_ACC_FS_16G, //后脚 - 加速度量程 - 16G .gry_fs = FML_IMU_GRY_FS_2000DPS, //后脚 - 陀螺仪量程 - 2000DPS .mag_fs = FML_IMU_MAG_FS_30GS, //后脚 - 地磁计量程 - 30GS .acc_odr = FML_IMU_ACC_ODR_OFF, //后脚 - 加速度采样频率 - 关闭 .gry_odr = FML_IMU_GRY_ODR_OFF, //后脚 - 陀螺仪采样频率 - 关闭 .mag_odr = FML_IMU_MAG_ODR_200HZ, //后脚 - 地磁计采样频率 - 200HZ .fifo_odr = FML_IMU_FIFO_ODR_OFF, }; //低功耗 static const fml_imu_param_t shoes_front_param={ .acc_power_mode = FML_IMU_ACC_POWER_MODE_NORMAL, //前脚 - 加速度正常模式 .gry_power_mode = FML_IMU_GRY_POWER_MODE_NORMAL, //前脚 - 陀螺仪正常模式 .timestamp_resolution = FML_IMU_TIMESTAMP_25US, //前脚 - 时间戳25US精度 .timestamp_switch = FML_IMU_TIMESTAMP_OFF, //前脚 - 时间戳关闭 .acc_fs = FML_IMU_ACC_FS_16G, //前脚 - 加速度量程 - 16G .gry_fs = FML_IMU_GRY_FS_2000DPS, //前脚 - 陀螺仪量程 - 2000DPS .mag_fs = FML_IMU_MAG_FS_30GS, //前脚 - 地磁计量程 - 30GS .acc_odr = FML_IMU_ACC_ODR_104HZ, //前脚 - 加速度采样频率 - 104HZ .gry_odr = FML_IMU_GRY_ODR_OFF, //前脚 - 陀螺仪采样频率 关闭 .mag_odr = FML_IMU_MAG_ODR_100HZ, //前脚 - 地磁计采样频率 - 100HZ .fifo_odr = FML_IMU_FIFO_ODR_104HZ, }; static const fml_imu_param_t shoes_back_param={ .acc_power_mode = FML_IMU_ACC_POWER_MODE_NORMAL, //后脚 - 加速度正常模式 .gry_power_mode = FML_IMU_GRY_POWER_MODE_NORMAL, //后脚 - 陀螺仪正常模式 .timestamp_resolution = FML_IMU_TIMESTAMP_25US, //后脚 - 时间戳25US精度 .timestamp_switch = FML_IMU_TIMESTAMP_OFF, //后脚 - 时间戳关闭 .acc_fs = FML_IMU_ACC_FS_16G, //后脚 - 加速度量程 - 16G .gry_fs = FML_IMU_GRY_FS_2000DPS, //后脚 - 陀螺仪量程 - 2000DPS .mag_fs = FML_IMU_MAG_FS_30GS, //后脚 - 地磁计量程 - 30GS .acc_odr = FML_IMU_ACC_ODR_OFF, //后脚 - 加速度采样频率 - 关闭 .gry_odr = FML_IMU_GRY_ODR_OFF, //后脚 - 陀螺仪采样频率 - 关闭 .mag_odr = FML_IMU_MAG_ODR_OFF, //后脚 - 地磁计采样频率 - 200HZ .fifo_odr = FML_IMU_FIFO_ODR_OFF, }; //待机模式 static const fml_imu_param_t standby_front_param={ .acc_power_mode = FML_IMU_ACC_POWER_MODE_NORMAL, //前脚 - 加速度正常模式 .gry_power_mode = FML_IMU_GRY_POWER_MODE_NORMAL, //前脚 - 陀螺仪正常模式 .timestamp_resolution = FML_IMU_TIMESTAMP_25US, //前脚 - 时间戳25US精度 .timestamp_switch = FML_IMU_TIMESTAMP_OFF, //前脚 - 时间戳关闭 .acc_fs = FML_IMU_ACC_FS_16G, //前脚 - 加速度量程 - 16G .gry_fs = FML_IMU_GRY_FS_2000DPS, //前脚 - 陀螺仪量程 - 2000DPS .mag_fs = FML_IMU_MAG_FS_30GS, //前脚 - 地磁计量程 - 30GS .acc_odr = FML_IMU_ACC_ODR_12HZ5, //前脚 - 加速度采样频率 - 12.5HZ .gry_odr = FML_IMU_GRY_ODR_OFF, //前脚 - 陀螺仪采样频率 - 关闭 .mag_odr = FML_IMU_MAG_ODR_OFF, //前脚 - 地磁计采样频率 - 关闭 .fifo_odr = FML_IMU_FIFO_ODR_OFF, //前脚 - FIFO采样频率 - 104HZ }; static const fml_imu_param_t standby_param={ .acc_power_mode = FML_IMU_ACC_POWER_MODE_NORMAL, //后脚 - 加速度正常模式 .gry_power_mode = FML_IMU_GRY_POWER_MODE_NORMAL, //后脚 - 陀螺仪正常模式 .timestamp_resolution = FML_IMU_TIMESTAMP_25US, //后脚 - 时间戳25US精度 .timestamp_switch = FML_IMU_TIMESTAMP_OFF, //后脚 - 时间戳关闭 .acc_fs = FML_IMU_ACC_FS_16G, //后脚 - 加速度量程 - 16G .gry_fs = FML_IMU_GRY_FS_2000DPS, //后脚 - 陀螺仪量程 - 2000DPS .mag_fs = FML_IMU_MAG_FS_30GS, //后脚 - 地磁计量程 - 30GS .acc_odr = FML_IMU_ACC_ODR_OFF, //后脚 - 加速度采样频率 - 关闭 .gry_odr = FML_IMU_GRY_ODR_OFF, //后脚 - 陀螺仪采样频率 - 关闭 .mag_odr = FML_IMU_MAG_ODR_10HZ, //后脚 - 地磁计采样频率 - 10HZ .fifo_odr = FML_IMU_FIFO_ODR_OFF, //后脚 - FIFO采样频率 - 关闭 }; typedef struct { int16_t max_temp; uint16_t time_count; }ACC_TIME_TYPE; typedef struct { ACC_TIME_TYPE slide_window[SLIDE_WINDOW_LEN]; int window_length; }SLIDE_WINDOW; int cal_unsigned_D_value(uint16_t left_value, uint16_t right_value) { if(left_value <= right_value) { return right_value - left_value; } else { return 65535-left_value + 1 + right_value; } } static uint8_t flag_move =0; void sort_silde_window(SLIDE_WINDOW *max_slide_window, SLIDE_WINDOW *min_slide_window, int16_t acc_val, uint16_t time_count) { ACC_TIME_TYPE acc_time_type_temp = {acc_val, time_count}; //检测当前的值是是否是最大值 if(max_slide_window->window_length == 0) { max_slide_window->slide_window[0] = acc_time_type_temp; max_slide_window->window_length = 1; } else { //先判断长度是否溢出来了 while(cal_unsigned_D_value(max_slide_window->slide_window[0].time_count, time_count)>= SLIDE_WINDOW_LEN) { memcpy(max_slide_window->slide_window, max_slide_window->slide_window + 1, (SLIDE_WINDOW_LEN - 1) *sizeof(ACC_TIME_TYPE)); max_slide_window->window_length --; } //再检测新的元素需要前面的元素小 while(max_slide_window->window_length > 0) { if(max_slide_window->slide_window[max_slide_window->window_length - 1].max_temp < acc_val) { max_slide_window->window_length --; } else { break; } } max_slide_window->slide_window[max_slide_window->window_length] = acc_time_type_temp; max_slide_window->window_length ++; } //检测当前的值是是否是最大值 if(min_slide_window->window_length == 0) { min_slide_window->slide_window[0] = acc_time_type_temp; min_slide_window->window_length = 1; } else { //先判断长度是否溢出来了 while(cal_unsigned_D_value(min_slide_window->slide_window[0].time_count, time_count)>= SLIDE_WINDOW_LEN) { memcpy(min_slide_window->slide_window, min_slide_window->slide_window + 1, (SLIDE_WINDOW_LEN - 1) *sizeof(ACC_TIME_TYPE)); min_slide_window->window_length --; } //再检测新的元素需要前面的元素小 while(min_slide_window->window_length > 0) { if(min_slide_window->slide_window[min_slide_window->window_length - 1].max_temp > acc_val) { min_slide_window->window_length --; } else { break; } } min_slide_window->slide_window[min_slide_window->window_length] = acc_time_type_temp; min_slide_window->window_length ++; } } /************************ 踮脚显示电量 ***********************************/ //没穿鞋的情况下 void app_BatDispaly_Process_N(void) { ser_imu_data_t data= {0}; static uint8_t lED_LIFE_SIGNAL = 0; static uint8_t lED_LIFE_TIME = 0; // DEBUG_LOG("app_BatDispaly_Process_N;\n"); if(-1 == hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_STANDBY)) { return;//停止状态 } //获取最新一组前脚加速度 int16_t data_size = 0; data_size = hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_FRONT); if(data_size > 0){ hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_FRONT, data_size -1 , &data); int32_t acc_norm = (data.acc[0] * data.acc[0] +data.acc[1] * data.acc[1] + data.acc[2] * data.acc[2]); if(abs(acc_norm - 4194304) < 362144 && data.acc[2] > 1536) { lED_LIFE_SIGNAL = 3; } else if(lED_LIFE_SIGNAL > 0)lED_LIFE_SIGNAL --; if(abs(acc_norm - 4194304) < 362144 && data.acc[2] < -1024) { lED_LIFE_SIGNAL = 0; } // DEBUG_LOG("acc_norm:%d,lED_LIFE_SIGNAL:%d,data.acc2:%d,lED_LIFE_TIME:%d\n",abs(acc_norm - 4194304),lED_LIFE_SIGNAL,data.acc[2],lED_LIFE_TIME); if(lED_LIFE_SIGNAL > 0) { // DEBUG_LOG("unwearshoes LED_Start(LED_OVERTURN,COLOR_GREEN);;\n"); if(lED_LIFE_TIME == 0){ lED_LIFE_TIME = (10000/StandByPower_Interval); if(GetBatteryPersent()>20){ LED_Start(LED_OVERTURN,COLOR_GREEN); }else{ LED_Start(LED_OVERTURN,COLOR_ORANGE); } Process_SetHoldOn(app_BatDispaly_Process_N,1); } else if (2 == lED_LIFE_TIME){ LED_Stop(LED_OVERTURN); Process_SetHoldOn(app_BatDispaly_Process_N,0); } if (lED_LIFE_TIME > 1)lED_LIFE_TIME--; } else { // DEBUG_LOG("unwearshoes LED_Stop(LED_OVERTURN);\n"); if(0 != lED_LIFE_TIME ){lED_LIFE_TIME =0; LED_Stop(LED_OVERTURN); Process_SetHoldOn(app_BatDispaly_Process_N,0); } if(1 == flag_move){flag_move =0; LED_Stop(LED_OVERTURN); Process_SetHoldOn(app_BatDispaly_Process_N,0); } } } } //穿鞋的情况下 void app_BatDispaly_Process(void) { ser_imu_data_t data={0}; static uint16_t time_count = 0; static uint16_t station_count =0; static uint16_t overturn_180_count = 0; static uint8_t lED_LIFE_SIGNAL = 0; static uint8_t lED_LIFE_TIME = 0; static SLIDE_WINDOW max_slide_window = { {-3096,0},0 }; static SLIDE_WINDOW min_slide_window = { {3096,0},0 }; if(-1 == hal_ser_imu_mode_manage_get_ready(HAL_SER_IMU_MODE_MANAGE_NORMAL)) { return;//走路状态 } // DEBUG_LOG("app_BatDispaly_Process;\n"); //获取最新一组前脚加速度, 有fifo int16_t data_size = 0; data_size = hal_ser_imu_mode_manage_get_data_num(SER_IMU_DIR_FRONT); if(data_size > 0){ // DEBUG_LOG("data_size : %d, hal_wearshoes_is_wearshoes: %d\n", data_size,hal_wearshoes_is_wearshoes()); hal_ser_imu_mode_manage_get_data(SER_IMU_DIR_FRONT, data_size-1 , &data); //耗时较少的计算滑动窗口的最大值、最小值 sort_silde_window(&max_slide_window, &min_slide_window, data.acc[0], time_count); if(time_count > SLIDE_WINDOW_LEN) { if(data.acc[0] - min_slide_window.slide_window[0].max_temp < 256 && max_slide_window.slide_window[0].max_temp - data.acc[0] < 256 && max_slide_window.slide_window[0].max_temp - min_slide_window.slide_window[0].max_temp < 256 && data.acc[0] < -1400) { station_count++; } else station_count =0; } time_count++; //在穿鞋状态下，被翻转了 int32_t acc_norm = (data.acc[0] * data.acc[0] +data.acc[1] * data.acc[1] + data.acc[2] * data.acc[2]); if(abs(acc_norm - 4194304) < 362144 && data.acc[2] > 1536) { //DEBUG_LOG(" wear shoes if(abs(acc_norm - 4194304) < 262144 && data.acc[2] > 1536);\n"); overturn_180_count ++; } else { overturn_180_count = 0; } if(overturn_180_count >= 5 || station_count > 4) { // DEBUG_LOG(" overturn_180_count > 5;\n"); lED_LIFE_SIGNAL = 6; } if(lED_LIFE_SIGNAL > 0)lED_LIFE_SIGNAL --; if(lED_LIFE_SIGNAL > 0) { // DEBUG_LOG("wearshoes LED_Start(LED_OVERTURN,COLOR_GREEN);;\n"); if(lED_LIFE_TIME == 0){ lED_LIFE_TIME = (10000/LowPower_Interval); if(GetBatteryPersent()>20){ LED_Start(LED_OVERTURN,COLOR_GREEN); }else{ LED_Start(LED_OVERTURN,COLOR_ORANGE); } Process_SetHoldOn(app_BatDispaly_Process,1); } else if (2 == lED_LIFE_TIME){ LED_Stop(LED_OVERTURN); Process_SetHoldOn(app_BatDispaly_Process,0); } if (lED_LIFE_TIME > 1)lED_LIFE_TIME--; } else { // DEBUG_LOG("wearshoes LED_Stop(LED_OVERTURN);\n"); if(0 != lED_LIFE_TIME ){lED_LIFE_TIME =0; LED_Stop(LED_OVERTURN); Process_SetHoldOn(app_BatDispaly_Process,0); } if(0 == flag_move){flag_move =1; LED_Stop(LED_OVERTURN); Process_SetHoldOn(app_BatDispaly_Process_N,0); } } } } void app_overturn_Init(void) { Process_Start(LowPower_Interval,"BatDispaly",app_BatDispaly_Process); Process_Start(StandByPower_Interval,"BatDispaly_N",app_BatDispaly_Process_N); fml_imu_mode_manage_imu_full_power_init(&app1_config_param.config_param[BLL_IMU_DIR_FRONT], &app1_config_param.config_param[BLL_IMU_DIR_BACK]); }