123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705 |
- #include "tool.h"
- #include "MahonyAHRS.h"
- #include "ble_comm.h"
- #include "nrf_delay.h"
- #include "bsp_pwm.h"
- #include "bsp_time.h"
- #include "fml_adc.h"
- #include "bsp_wdt.h"
- #include "exception.h"
- #include "selfcheck.h"
- #include "system.h"
- #include "drv_qma7981.h"
- #include "hal_led.h"
- #include "hal_battery.h"
- #include "bll_imu.h"
- #include "app_flash.h"
- #include "system.h"
- #define SELFCHECK_SCAN_DEVICE_RSSI_MIN_THRESHOLD -60
- #define SELFCHECK_CHARGE_CHIP_PIN_ADC_MIN_THRESHOLD 80
- #define SELFCHECK_BATTERY_PIN_ADC_MIN_THRESHOLD 3300
- #define SELFCHECK_BATTERY_PIN_ADC_MAX_THRESHOLD 4300
- #define SELFCHECK_MIDDLE_ACC_PROS_ROLL_MIN_THRESHOLD 0
- #define SELFCHECK_MIDDLE_ACC_PROS_ROLL_MAX_THRESHOLD 35
- #define SELFCHECK_MIDDLE_ACC_CONS_ROLL_MIN_THRESHOLD 160
- #define SELFCHECK_MIDDLE_ACC_CONS_ROLL_MAX_THRESHOLD 180
- #define SELFCHECK_WEAR_INSOLE_MAG_NORM_MIN_THRESHOLD 5000
- #define SELFCHECK_SENSOR_MAG_SHAKE_TRIGGER_THRESHOLD 18000
- #define SELFCHECK_SENSOR_MAG_NO_WELDING_CAPACITOR_MIN_THRESHOLD 200
- #define SELFCHECK_SENSOR_MAG_SHAKE_THRESHOLD 200
- typedef enum {
- SELFCHECK_RESULT_SUCCESS = 0,
-
- SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_SIX_AXIS,
-
- SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_MAG,
-
- SELFCHECK_RESULT_ERR_FRONT_SIX_AXIS_NO_DATA_OR_EXCP_DATA,
-
- SELFCHECK_RESULT_ERR_FRONT_MAG_NO_DATA_OR_EXCP_DATA,
-
- SELFCHECK_RESULT_ERR_BACK_SENSOR_CONFIG,
-
- SELFCHECK_RESULT_ERR_BACK_NO_DATA_OR_EXCP_DATA,
-
- SELFCHECK_RESULT_ERR_MIDDLE_SENSOR_CONFIG,
-
- SELFCHECK_RESULT_ERR_MIDDLE_NO_DATA_OR_EXCP_DATA,
-
- SELFCHECK_RESULT_ERR_CHARGE_CHIP_PIN_ADC,
-
- SELFCHECK_RESULT_ERR_BATTERY_PIN_ADC,
-
- SELFCHECK_RESULT_ERR_RSSI,
-
- } SELFCHECK_RESULT_e;
- typedef struct _selfcheck
- {
- uint32_t selfcheck_result;
-
- uint32_t selfcheck_result_led_color;
-
- uint32_t selfcheck_result_flash_num;
-
- bool selfcheck_is_led_display;
-
- int16_t max_rssi;
-
- fml_imu_data_t f_data;
-
- fml_imu_data_t b_data;
-
- qma_data_t m_data;
-
- bool f_is_read_data;
-
- bool b_is_read_data;
-
- } SelfCheck_t;
- static SelfCheck_t ob_selfcheck;
- static const bll_imu_one_way_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,
- .timestamp_switch = FML_IMU_TIMESTAMP_ON,
- .acc_fs = FML_IMU_ACC_FS_16G,
- .gry_fs = FML_IMU_GRY_FS_2000DPS,
- .mag_fs = FML_IMU_MAG_FS_30GS,
- .acc_odr = FML_IMU_ACC_ODR_416HZ,
- .gry_odr = FML_IMU_GRY_ODR_416HZ,
- .mag_odr = FML_IMU_MAG_ODR_200HZ,
- .fifo_odr = FML_IMU_FIFO_ODR_416HZ,
- };
- static const bll_imu_one_way_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,
- .timestamp_switch = FML_IMU_TIMESTAMP_OFF,
- .acc_fs = FML_IMU_ACC_FS_16G,
- .gry_fs = FML_IMU_GRY_FS_2000DPS,
- .mag_fs = FML_IMU_MAG_FS_30GS,
- .acc_odr = FML_IMU_ACC_ODR_OFF,
- .gry_odr = FML_IMU_GRY_ODR_OFF,
- .mag_odr = FML_IMU_MAG_ODR_200HZ,
- .fifo_odr = FML_IMU_FIFO_ODR_OFF,
- };
- static const bll_imu_param_t game_bll_imu_param_t={
- .config_param[FML_IMU_DIR_FRONT] = &game_front_param,
- .config_param[FML_IMU_DIR_BACK] = &game_back_param,
- };
- static void fb_data_notify_cb(uint32_t dir_bit)
- {
- int data_len;
- if((dir_bit >> BLL_IMU_DIR_FRONT) & 0x01)
- {
- memset(&ob_selfcheck.f_data,0,sizeof(ob_selfcheck.f_data));
- data_len = bll_imu_get_data_num(BLL_IMU_DIR_FRONT);
- for(int i=0; i<data_len;i++)
- {
- bll_imu_get_data(BLL_IMU_DIR_FRONT, i, &ob_selfcheck.f_data);
- }
- }
-
- if((dir_bit >> BLL_IMU_DIR_BACK) & 0x01)
- {
- memset(&ob_selfcheck.b_data,0,sizeof(ob_selfcheck.b_data));
- data_len = bll_imu_get_data_num(BLL_IMU_DIR_BACK);
- for(int i=0; i<data_len;i++)
- {
- bll_imu_get_data(BLL_IMU_DIR_BACK, i, &ob_selfcheck.b_data);
- }
- }
-
- if(((dir_bit >> BLL_IMU_DIR_FRONT) & 0x01))
- {
- ob_selfcheck.f_is_read_data = true;
- }
-
- if((dir_bit >> BLL_IMU_DIR_BACK))
- {
- ob_selfcheck.b_is_read_data = true;
- }
-
- }
- static void scan_report_cb(uint8_t *adv_data, uint16_t adv_data_len,int8_t rssi)
- {
- ob_selfcheck.max_rssi = (ob_selfcheck.max_rssi > rssi)?ob_selfcheck.max_rssi:rssi;
- }
- static void selfcheck_led_display_process(void)
- {
- static uint32_t cur_flash_num = 0;
- static uint8_t level = LED_ENABLE;
- static uint8_t red_off_led_count = 0;
- static uint32_t tim = 0;
-
-
- feed_watchdog();
-
- if(ob_selfcheck.selfcheck_is_led_display == true)
- {
- if(cur_flash_num == 0)tim = TIME_GetTicks();
-
-
- if(level == LED_ENABLE)cur_flash_num++;
- if(cur_flash_num > ob_selfcheck.selfcheck_result_flash_num)
- {
- nrf_gpio_pin_write(PIN_LED_ENABLE,LED_DISABLE);
- if(ob_selfcheck.selfcheck_result_led_color == COLOR_RED && (TIME_GetTicks() - tim < 5000))return;
- cur_flash_num = 0;
- level = LED_ENABLE;
- red_off_led_count = 0;
- ob_selfcheck.selfcheck_is_led_display = false;
- Process_Stop(selfcheck_led_display_process);
- return;
- }
-
-
- if(ob_selfcheck.selfcheck_result_led_color == COLOR_RED)
- {
- Process_Start(50,"selfcheck_led_display_process",selfcheck_led_display_process);
- nrf_gpio_pin_write(PIN_LED_ENABLE,level);nrf_delay_ms(5);
- if(level == LED_ENABLE){WS2812_DisplayDot(ob_selfcheck.selfcheck_result_led_color);WS2812_Pwm_Play();}
- }
- else
- {
- Process_Start(500,"selfcheck_led_display_process",selfcheck_led_display_process);
- nrf_gpio_pin_write(PIN_LED_ENABLE,level);nrf_delay_ms(5);
- if(level == LED_ENABLE){WS2812_DisplayDot(ob_selfcheck.selfcheck_result_led_color);WS2812_Pwm_Play();}
- }
-
-
- if(ob_selfcheck.selfcheck_result_led_color == COLOR_RED)
- {
- if(level == LED_ENABLE){level = LED_DISABLE;red_off_led_count = 0;}
- if(level == LED_DISABLE){red_off_led_count++;if(red_off_led_count >= 6)level = LED_ENABLE;}
- }
- else
- {
- level = (level == LED_ENABLE)?LED_DISABLE:LED_ENABLE;
- }
- }
- }
- static void selfcheck_result_display_process(void)
- {
- uint16_t front_mag_norm;
-
-
- feed_watchdog();
-
- if(ob_selfcheck.selfcheck_is_led_display == false)
- {
-
-
-
-
-
-
-
-
-
-
- if(ob_selfcheck.selfcheck_result == 0)
- {
- front_mag_norm = sqrt((double)(ob_selfcheck.f_data.mag[0]*ob_selfcheck.f_data.mag[0]) + (double)(ob_selfcheck.f_data.mag[1]*ob_selfcheck.f_data.mag[1]) + (double)(ob_selfcheck.f_data.mag[2]*ob_selfcheck.f_data.mag[2]));
- if(SELFCHECK_WEAR_INSOLE_MAG_NORM_MIN_THRESHOLD <= front_mag_norm)
- {
- ob_selfcheck.selfcheck_result_led_color = COLOR_GREEN;
- ob_selfcheck.selfcheck_result_flash_num = 1;
- }
- else
- {
- ob_selfcheck.selfcheck_result_led_color = COLOR_BLUE;
- ob_selfcheck.selfcheck_result_flash_num = 1;
- }
- }
- else
- {
- ob_selfcheck.selfcheck_result_led_color = COLOR_RED;
- for(int i=0;i<sizeof(ob_selfcheck.selfcheck_result)*8;i++)
- {
- if((ob_selfcheck.selfcheck_result & (1 << i)) != 0)
- {
- ob_selfcheck.selfcheck_result_flash_num = i;
- break;
- }
- }
- }
- Process_Start(0,"selfcheck_led_display_process",selfcheck_led_display_process);
- ob_selfcheck.selfcheck_is_led_display = true;
- }
- }
- static void selfcheck_mt_process(void)
- {
-
- feed_watchdog();
-
- nrf_gpio_pin_toggle(PIN_MT_EN);
- }
- static void selfcheck_process(void)
- {
- BLL_IMU_CONFIG_RESULT f_config_result,b_config_result;
- int16_t adc_value = 0;
- uint16_t front_mag_norm;
- uint16_t back_mag_norm;
- int16_t roll;
- uint8_t buf[256];
- uint8_t L=0;
- char mac_buf[16];
- static uint32_t charge_chip_adc_max = 0;
- static int16_t battery_adc_max = 0;
- static uint32_t t_count = 0;
- static uint32_t battery_adc_t_count = 0;
- static uint32_t roll_t_count = 0;
- static uint32_t sensor_t_count = 0;
- static uint32_t last_tim = 0;
- static uint32_t continue_trigger = 0;
- static uint32_t front_mag_norm_max = 0;
- static uint32_t back_mag_norm_max = 0;
- static uint32_t front_mag_norm_min = 0xffffffff;
- static uint32_t back_mag_norm_min = 0xffffffff;
- static bool front_mag_shake_trigger = false;
- static bool back_mag_shake_trigger = false;
- t_count++;
-
-
- feed_watchdog();
-
-
- memset(&ob_selfcheck.m_data,0,sizeof(ob_selfcheck.m_data));
- if(drv_qma_get_acc_data(&ob_selfcheck.m_data) != 0)
- {
- ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_MIDDLE_NO_DATA_OR_EXCP_DATA);
-
- drv_qma_set_acc_odr(QMA_ACC_ODR_104HZ);
- }
- Mahony_process(0,0,0,ob_selfcheck.m_data.acc[0],ob_selfcheck.m_data.acc[1],ob_selfcheck.m_data.acc[2],0,0,0);
-
-
-
- fml_adc_get_value(PIN_ADC_BAT_CHANNEL,&adc_value);
- adc_value = ADC_RESULT_IN_MILLI_VOLTS(adc_value) * 5 / 3;
- battery_adc_max = adc_value;
-
-
- if(t_count - battery_adc_t_count > (3000/100))
- {
- if(!(SELFCHECK_BATTERY_PIN_ADC_MIN_THRESHOLD < battery_adc_max && battery_adc_max < SELFCHECK_BATTERY_PIN_ADC_MAX_THRESHOLD))ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_BATTERY_PIN_ADC);
- else ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_BATTERY_PIN_ADC);
-
- battery_adc_t_count = t_count;
- }
-
-
- fml_adc_get_value(PIN_ADC_CHARGMEASURE_CHANNEL,&adc_value);
- if(SELFCHECK_CHARGE_CHIP_PIN_ADC_MIN_THRESHOLD < adc_value)
- {
- charge_chip_adc_max++;
- }
- if(charge_chip_adc_max > 10)
- {
- ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_CHARGE_CHIP_PIN_ADC);
- }
- else
- {
- ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_CHARGE_CHIP_PIN_ADC);
- }
-
-
-
- if(t_count - roll_t_count > (3000/100))
- {
- roll = (int16_t)(getRoll()*100);
- if(roll < 0)roll *= -1;
- roll = (roll > 0 && roll < 100)?1:roll/100;
- if(!( \
- (SELFCHECK_MIDDLE_ACC_PROS_ROLL_MIN_THRESHOLD < roll && roll < SELFCHECK_MIDDLE_ACC_PROS_ROLL_MAX_THRESHOLD) || \
- (SELFCHECK_MIDDLE_ACC_CONS_ROLL_MIN_THRESHOLD < roll && roll < SELFCHECK_MIDDLE_ACC_CONS_ROLL_MAX_THRESHOLD) \
- ))ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_MIDDLE_NO_DATA_OR_EXCP_DATA);
- else
- ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_MIDDLE_NO_DATA_OR_EXCP_DATA);
-
- roll_t_count = t_count;
- }
-
- if(t_count - sensor_t_count > (1000/100))
- {
-
- f_config_result = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_FRONT,&game_bll_imu_param_t);
- if(f_config_result != BLL_IMU_CONFIG_FINISH)
- {
- if(f_config_result == BLL_IMU_CONFIG_FAIL_FRONT_MAG)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_MAG);
- if(f_config_result == BLL_IMU_CONFIG_FAIL_FRONT_SIX_AXIS)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_SIX_AXIS);
- }
- else
- {
-
- ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_MAG);
- ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_FRONT_SENSOR_CONFIG_SIX_AXIS);
-
- if(ob_selfcheck.f_is_read_data != true)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_SIX_AXIS_NO_DATA_OR_EXCP_DATA);
- else
- {
-
- ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_FRONT_SIX_AXIS_NO_DATA_OR_EXCP_DATA);
- ob_selfcheck.f_is_read_data = false;
-
- front_mag_norm = sqrt((double)(ob_selfcheck.f_data.mag[0]*ob_selfcheck.f_data.mag[0]) + (double)(ob_selfcheck.f_data.mag[1]*ob_selfcheck.f_data.mag[1]) + (double)(ob_selfcheck.f_data.mag[2]*ob_selfcheck.f_data.mag[2]));
-
-
- if(front_mag_norm < SELFCHECK_SENSOR_MAG_NO_WELDING_CAPACITOR_MIN_THRESHOLD)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_MAG_NO_DATA_OR_EXCP_DATA);
- else ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_FRONT_MAG_NO_DATA_OR_EXCP_DATA);
- }
- }
-
-
-
- b_config_result = bll_imu_query_config_param_is_ready(BLL_IMU_DIR_BACK,&game_bll_imu_param_t);
- if(b_config_result != BLL_IMU_CONFIG_FINISH)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_BACK_SENSOR_CONFIG);
- else
- {
-
- ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_BACK_SENSOR_CONFIG);
-
- if(ob_selfcheck.b_is_read_data != true)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_BACK_NO_DATA_OR_EXCP_DATA);
- else
- {
-
- ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_BACK_NO_DATA_OR_EXCP_DATA);
- ob_selfcheck.b_is_read_data = false;
-
- back_mag_norm = sqrt((double)(ob_selfcheck.b_data.mag[0]*ob_selfcheck.b_data.mag[0]) + (double)(ob_selfcheck.b_data.mag[1]*ob_selfcheck.b_data.mag[1]) + (double)(ob_selfcheck.b_data.mag[2]*ob_selfcheck.b_data.mag[2]));
-
-
- if(back_mag_norm < SELFCHECK_SENSOR_MAG_NO_WELDING_CAPACITOR_MIN_THRESHOLD)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_BACK_NO_DATA_OR_EXCP_DATA);
- else ob_selfcheck.selfcheck_result &= ~(1 << SELFCHECK_RESULT_ERR_BACK_NO_DATA_OR_EXCP_DATA);
- }
-
- }
-
- if(f_config_result != BLL_IMU_CONFIG_FINISH || b_config_result != BLL_IMU_CONFIG_FINISH || ob_selfcheck.b_is_read_data != true || ob_selfcheck.f_is_read_data != true)
- {
- bll_imu_Resume_config_param(&game_bll_imu_param_t);
- }
-
- sensor_t_count = t_count;
- }
-
-
- if(t_count > (2000/100))
- {
- front_mag_norm = sqrt((double)(ob_selfcheck.f_data.mag[0]*ob_selfcheck.f_data.mag[0]) + (double)(ob_selfcheck.f_data.mag[1]*ob_selfcheck.f_data.mag[1]) + (double)(ob_selfcheck.f_data.mag[2]*ob_selfcheck.f_data.mag[2]));
- back_mag_norm = sqrt((double)(ob_selfcheck.b_data.mag[0]*ob_selfcheck.b_data.mag[0]) + (double)(ob_selfcheck.b_data.mag[1]*ob_selfcheck.b_data.mag[1]) + (double)(ob_selfcheck.b_data.mag[2]*ob_selfcheck.b_data.mag[2]));
-
- if(front_mag_norm >= SELFCHECK_SENSOR_MAG_SHAKE_TRIGGER_THRESHOLD)
- {
-
- front_mag_norm_max = (front_mag_norm_max > front_mag_norm)?front_mag_norm_max:front_mag_norm;
-
- front_mag_norm_min = (front_mag_norm_min > front_mag_norm)?front_mag_norm:front_mag_norm_min;
-
- front_mag_shake_trigger = true;
- }
-
- if(back_mag_norm >= SELFCHECK_SENSOR_MAG_SHAKE_TRIGGER_THRESHOLD)
- {
-
- back_mag_norm_max = (back_mag_norm_max > back_mag_norm)?back_mag_norm_max:back_mag_norm;
-
- back_mag_norm_min = (back_mag_norm_min > back_mag_norm)?back_mag_norm:back_mag_norm_min;
-
- back_mag_shake_trigger = true;
- }
-
- DEBUG_LOG("front_mag_norm_max:%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n",front_mag_norm,back_mag_norm,front_mag_norm_max,back_mag_norm_max, \
- front_mag_norm_min,back_mag_norm_min,front_mag_norm_max - front_mag_norm_min, \
- back_mag_norm_max - back_mag_norm_min,front_mag_shake_trigger,back_mag_shake_trigger);
-
- if(t_count >= (5000/100))
- {
- if(front_mag_norm_max - front_mag_norm_min >= SELFCHECK_SENSOR_MAG_SHAKE_THRESHOLD && front_mag_shake_trigger)
- {
- ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_FRONT_MAG_NO_DATA_OR_EXCP_DATA);
- }
-
- if(back_mag_norm_max - back_mag_norm_min >= SELFCHECK_SENSOR_MAG_SHAKE_THRESHOLD && back_mag_shake_trigger)
- {
- ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_BACK_NO_DATA_OR_EXCP_DATA);
- }
- }
- }
-
- if(t_count == (5000/100))
- {
- if(SELFCHECK_SCAN_DEVICE_RSSI_MIN_THRESHOLD >= ob_selfcheck.max_rssi)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_RSSI);
- }
-
- if(t_count == (5000/100))
- {
- nrf_gpio_pin_write(PIN_MT_EN,0);
- Process_Stop(selfcheck_mt_process);
- Process_Start(0,"selfcheck_result_display_process",selfcheck_result_display_process);
- Process_SetHoldOn(selfcheck_result_display_process,1);
- }
-
- L=0;
- int16_t rol = (int16_t)(getRoll()*100);
- int16_t pitch = (int16_t)(getPitch()*100);
- int16_t yaw = (int16_t)(getYaw()*100);
- buf[L++] = (uint8_t)(rol>>8);
- buf[L++] = (uint8_t)(rol>>0);
- buf[L++] = (uint8_t)(pitch>>8);
- buf[L++] = (uint8_t)(pitch>>0);
- buf[L++] = (uint8_t)(yaw>>8);
- buf[L++] = (uint8_t)(yaw>>0);
- buf[L++] = 0;
- buf[L++] = 0;
- buf[L++] = 0;
- buf[L++] = 0;
- buf[L++] = 0;
- buf[L++] = 0;
- buf[L++] = 0;
- Mahony_send_ANO(0x01,buf,L);
- L=0;
- int32_t middle_acc_x = ob_selfcheck.m_data.acc[0];
- int32_t middle_acc_y = ob_selfcheck.m_data.acc[1];
- int16_t middle_acc_z = ob_selfcheck.m_data.acc[2];
- middle_acc_x *= 100;
- middle_acc_y *= 100;
- middle_acc_z *= 10;
- buf[L++] = (uint8_t)(middle_acc_x>>24);
- buf[L++] = (uint8_t)(middle_acc_x>>16);
- buf[L++] = (uint8_t)(middle_acc_x>>8);
- buf[L++] = (uint8_t)(middle_acc_x>>0);
- buf[L++] = (uint8_t)(middle_acc_y>>24);
- buf[L++] = (uint8_t)(middle_acc_y>>16);
- buf[L++] = (uint8_t)(middle_acc_y>>8);
- buf[L++] = (uint8_t)(middle_acc_y>>0);
- buf[L++] = (uint8_t)(middle_acc_z>>8);
- buf[L++] = (uint8_t)(middle_acc_z>>0);
- Mahony_send_ANO(0x07,buf,L);
-
-
-
- if(TIME_GetTicks()-last_tim >= 1000)
- {
- last_tim = TIME_GetTicks();
-
- ob_selfcheck.max_rssi = -120;
- }
- L=0;
- buf[L++] = (uint8_t)(ob_selfcheck.f_data.acc[0]>>8);
- buf[L++] = (uint8_t)(ob_selfcheck.f_data.acc[0]>>0);
- buf[L++] = (uint8_t)(ob_selfcheck.f_data.acc[1]>>8);
- buf[L++] = (uint8_t)(ob_selfcheck.f_data.acc[1]>>0);
- buf[L++] = (uint8_t)(ob_selfcheck.f_data.acc[2]>>8);
- buf[L++] = (uint8_t)(ob_selfcheck.f_data.acc[2]>>0);
- buf[L++] = (uint8_t)(ob_selfcheck.f_data.gry[0]>>8);
- buf[L++] = (uint8_t)(ob_selfcheck.f_data.gry[0]>>0);
- buf[L++] = (uint8_t)(ob_selfcheck.f_data.gry[1]>>8);
- buf[L++] = (uint8_t)(ob_selfcheck.f_data.gry[1]>>0);
- buf[L++] = (uint8_t)(ob_selfcheck.f_data.gry[2]>>8);
- buf[L++] = (uint8_t)(ob_selfcheck.f_data.gry[2]>>0);
- front_mag_norm = sqrt((double)(ob_selfcheck.f_data.mag[0]*ob_selfcheck.f_data.mag[0]) + (double)(ob_selfcheck.f_data.mag[1]*ob_selfcheck.f_data.mag[1]) + (double)(ob_selfcheck.f_data.mag[2]*ob_selfcheck.f_data.mag[2]));
- back_mag_norm = sqrt((double)(ob_selfcheck.b_data.mag[0]*ob_selfcheck.b_data.mag[0]) + (double)(ob_selfcheck.b_data.mag[1]*ob_selfcheck.b_data.mag[1]) + (double)(ob_selfcheck.b_data.mag[2]*ob_selfcheck.b_data.mag[2]));
- buf[L++] = (uint8_t)(front_mag_norm>>8);
- buf[L++] = (uint8_t)(front_mag_norm>>0);
- buf[L++] = (uint8_t)(back_mag_norm>>8);
- buf[L++] = (uint8_t)(back_mag_norm>>0);
- buf[L++] = (uint8_t)(ob_selfcheck.max_rssi>> 8);
- buf[L++] = (uint8_t)(ob_selfcheck.max_rssi>>0);
- Mahony_send_ANO(0x02,buf,L);
-
- fml_adc_get_value(PIN_CHARGING_CHANNEL,&adc_value);
- if(adc_value >= 2000)
- {
- if(continue_trigger <= 50)continue_trigger++;
- }
- else
- {
- if(continue_trigger != 0)continue_trigger--;
- }
-
-
- if(t_count >= (30000/100) && !slave_isconnect() && continue_trigger==0)
- {
- memset(mac_buf, 0, sizeof(mac_buf));
- sprintf(mac_buf, "%02X%02X%02X%02X%02X%02X", mFlash.macHost[0], mFlash.macHost[1], mFlash.macHost[2], mFlash.mClient.macAddr[3], mFlash.mClient.macAddr[4], mFlash.mClient.macAddr[5]);
- ST_scan_stop();
- host_set_scan_name(mac_buf, strlen(mac_buf));
-
- if(Flash_SaveStep() != ZONE_OP_SUCCESS)Except_TxError(EXCEPT_Power,"save step fail");
- extern battercb_t battery_record;
- memcpy(&mFlash.mbattercb_t,&battery_record,sizeof(battercb_t));
- mBackup.RestartCnt =0;
- if(Flash_SaveBackup() != ZONE_OP_SUCCESS)Except_TxError(EXCEPT_Power,"pwr save backup fail");
- if(Flash_SaveInfomation() != ZONE_OP_SUCCESS)Except_TxError(EXCEPT_Power,"pwr save information fail");
- NVIC_SystemReset();
- }
- }
- static void adc_callback(uint32_t sample_point, Fml_Adc_All_Channel_Adc_Value_t all_adc_value)
- {
-
- }
- void selfcheck_trigger_callback(char order)
- {
- int error;
- uint32_t last_tim = TIME_GetTicks();
-
-
- feed_watchdog();
-
-
- memset(&ob_selfcheck,0,sizeof(ob_selfcheck));
- ob_selfcheck.max_rssi = -120;
- ob_selfcheck.selfcheck_result = SELFCHECK_RESULT_SUCCESS;
- ob_selfcheck.selfcheck_result_led_color = COLOR_BLACK;
- ob_selfcheck.f_is_read_data = false;
- ob_selfcheck.b_is_read_data = false;
- ob_selfcheck.selfcheck_is_led_display = false;
-
-
- Process_All_Stop();
- Process_Start(100,"selfcheck_process",selfcheck_process);
- Process_SetHoldOn(selfcheck_process,1);
-
- while(TIME_GetTicks() - last_tim <= 1000)
- {
- host_disconnect();
- if(host_isconnect() == 0)break;
- }
- ST_scan_stop();
- host_set_scan_name((char *)"***********",sizeof("***********"));
- advdata_report_Evt_Regist(scan_report_cb);
- error = ST_scan_start();
- if(error != APP_SUCCESS)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_RSSI);
-
- nrf_gpio_cfg(
- PIN_LED_ENABLE,
- NRF_GPIO_PIN_DIR_OUTPUT,
- NRF_GPIO_PIN_INPUT_DISCONNECT,
- NRF_GPIO_PIN_NOPULL,
- NRF_GPIO_PIN_S0D1,
- NRF_GPIO_PIN_NOSENSE);
-
- nrf_gpio_cfg_output(PIN_MT_EN);
- nrf_gpio_pin_write(PIN_MT_EN,0);
-
-
- ADC_SetPinChannel(PIN_ADC_CHARGMEASURE, PIN_ADC_CHARGMEASURE_CHANNEL,NRF_GPIO_PIN_NOPULL);
- ADC_SetPinChannel(PIN_ADC_BAT_IN, PIN_ADC_BAT_CHANNEL,NRF_GPIO_PIN_NOPULL);
- ADC_SetPinChannel(PIN_CHARGING, PIN_CHARGING_CHANNEL,NRF_GPIO_PIN_NOPULL);
- fml_adc_sample_update_notify_register(adc_callback);
-
- bll_imu_Init();
- bll_imu_register_data_notify_callback(BLL_IMU_DATA_NOTIFY_CB_PRIORITY_1, fb_data_notify_cb);
- bll_imu_Resume_config_param(&game_bll_imu_param_t);
- error = drv_qma_Init();
- if(error == -1)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_MIDDLE_SENSOR_CONFIG);
- nrf_delay_ms(20);
- error = drv_qma_set_acc_odr(QMA_ACC_ODR_104HZ);
- if(error == -1)ob_selfcheck.selfcheck_result |= (1 << SELFCHECK_RESULT_ERR_MIDDLE_SENSOR_CONFIG);
-
-
- Process_Start(500,"selfcheck_mt_process",selfcheck_mt_process);
- Process_SetHoldOn(selfcheck_mt_process,1);
-
- Mahony_Init(10);
-
-
- feed_watchdog();
-
- }
|