123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137 |
- #include "app_Calibration.h"
- #include "bsp_time.h"
- #include "system.h"
- #include "hal_imu.h"
- #include "hal_flash.h"
- #include "hal_ble_client.h"
- #include "arm_math.h"
- #include "drv_calibration.h"
- #include "nrf_gpio.h"
- #include "app_charge.h"
- static int16_t Acc[3]={0};
- static int16_t Gry[3]={0};
- static int16_t FrontMag[3]={0};
- static void app_calibration_CalLed_Process(void){
-
- static uint32_t tim =0;
-
- if(tim == 0){
- tim = TIME_GetTicks();
- }else if(TIME_GetTicks()-tim >= 60000){
- tim = 0;
- Process_Stop(app_calibration_CalLed_Process);
- }
-
-
-
- nrf_gpio_pin_write(PIN_LED_RUN,0);
- nrf_delay_ms(10);
- nrf_gpio_pin_write(PIN_LED_RUN,1);
-
- char printdata[200]={0};
- sprintf(printdata,"caldata:%f,%f,%f,%f,%f,%f",mBackup.cal[0],mBackup.cal[1],mBackup.cal[2],mBackup.cal[3],mBackup.cal[4],mBackup.cal[5]);
- DEBUG_LOG("%s\n",printdata);
- Mahony_PRINT("caldata:%f,%f,%f,%f,%f,%f",mBackup.cal[0],mBackup.cal[1],mBackup.cal[2],mBackup.cal[3],mBackup.cal[4],mBackup.cal[5]);
-
-
- }
- #define USED_MAG_CHECK_BAREBOARD_VALUE_START 0
- #define USED_MAG_CHECK_BAREBOARD_VALUE_END 300
- void app_calibration_is_BareBoard_Process(void)
- {
- if(IMU_GetCurrentMode() == STATE_IDLE_MODE)
- {
- IMU_GetMagFront(FrontMag);
- if(USED_MAG_CHECK_BAREBOARD_VALUE_START < FrontMag[0] && FrontMag[0] < USED_MAG_CHECK_BAREBOARD_VALUE_END){
- IMU_SetBareBoardMode(1);
- }else{
- IMU_SetBareBoardMode(0);
- }
- }
- }
- void app_calibration_is_Calibration_Process(void)
- {
- if(IMU_GetCurrentMode() == STATE_BAREBOARD_MODE)
- {
-
- if(IMU_GetBareBoardMode() == 1)IMU_SetBareBoardMode(0);
-
- IMU_GetAcc(Acc);
- IMU_GetGry(Gry);
- IMU_GetMagFront(FrontMag);
-
- ImuCalibration_pcs(Acc, Gry, FrontMag);
- if(ImuCalibration_GetState() == ImuCal_GetData){
- nrf_gpio_pin_write(PIN_LED_RUN,0);
- IMU_SetCalibrationMode(1);
- }else{
- if(FrontMag[0] >= USED_MAG_CHECK_BAREBOARD_VALUE_END || USED_MAG_CHECK_BAREBOARD_VALUE_START >= FrontMag[0]){
- IMU_SetLowPowerMode(1);
- }
- }
- }
- }
- void app_calibration_Calibration_in_Process(void)
- {
- if(IMU_GetCurrentMode() == STATE_CALIBRATION_MODE)
- {
-
- if(IMU_GetCalibrationMode() == 1)IMU_SetCalibrationMode(0);
-
- IMU_GetAcc(Acc);
- IMU_GetGry(Gry);
- IMU_GetMagFront(FrontMag);
-
- ImuCalibration_pcs(Acc, Gry, FrontMag);
- if(ImuCalibration_GetState() == ImuCal_finish){
- Flash_SaveBackup();
- Mahony_PRINT("ImuCal_state ImuCal_finish");
- Process_Start(2000,"app_calibration_CalLed_Process",app_calibration_CalLed_Process);
- Process_UpdatePeroid(app_calibration_CalLed_Process,2000);
- IMU_SetLowPowerMode(1);
- }else if(ImuCalibration_GetState() == ImuCal_error || ImuCalibration_GetState() == ImuCal_quiet){
- Mahony_PRINT("ImuCal_state ImuCal_error");
- if(app_charge_Getstate() == BLE_Client_T_CHARGE_INSERT || app_charge_Getstate() == BLE_Client_T_CHARGE_DONE){
- ImuCalibration_SetState(ImuCal_init);
- IMU_SetBareBoardMode(1);
- }else{
- Process_Start(100,"app_calibration_CalLed_Process",app_calibration_CalLed_Process);
- Process_UpdatePeroid(app_calibration_CalLed_Process,100);
- IMU_SetLowPowerMode(1);
- }
- }
- }
-
- }
- void app_calibration_Init(void)
- {
- Process_Start(1000,"app_calibration_is_BareBoard_Process",app_calibration_is_BareBoard_Process);
- Process_Start(200,"app_calibration_is_Calibration_Process",app_calibration_is_Calibration_Process);
- Process_Start(10,"app_calibration_Calibration_in_Process",app_calibration_Calibration_in_Process);
- }
|