app_Calibration.c 4.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137
  1. #include "app_Calibration.h"
  2. #include "bsp_time.h"
  3. #include "system.h"
  4. #include "hal_imu.h"
  5. #include "hal_flash.h"
  6. #include "hal_ble_client.h"
  7. #include "arm_math.h"
  8. #include "drv_calibration.h"
  9. #include "nrf_gpio.h"
  10. #include "app_charge.h"
  11. static int16_t Acc[3]={0};
  12. static int16_t Gry[3]={0};
  13. static int16_t FrontMag[3]={0};
  14. /***********************************
  15. *未校准:灭灯
  16. *校准中:指示灯常亮
  17. *校准完成:指示灯慢闪,亮10ms,灭灯2S
  18. *校准错误:指示灯快闪,亮10ms,灭灯100ms
  19. ************************************/
  20. static void app_calibration_CalLed_Process(void){
  21. static uint32_t tim =0;
  22. if(tim == 0){
  23. tim = TIME_GetTicks();
  24. }else if(TIME_GetTicks()-tim >= 60000){ //亮1分钟
  25. tim = 0;
  26. Process_Stop(app_calibration_CalLed_Process); //停止该任务
  27. }
  28. nrf_gpio_pin_write(PIN_LED_RUN,0);
  29. nrf_delay_ms(10);
  30. nrf_gpio_pin_write(PIN_LED_RUN,1);
  31. char printdata[200]={0};
  32. 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]);
  33. DEBUG_LOG("%s\n",printdata);
  34. 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]);
  35. }
  36. #define USED_MAG_CHECK_BAREBOARD_VALUE_START 0
  37. #define USED_MAG_CHECK_BAREBOARD_VALUE_END 300
  38. void app_calibration_is_BareBoard_Process(void)
  39. {
  40. if(IMU_GetCurrentMode() == STATE_IDLE_MODE)
  41. {
  42. IMU_GetMagFront(FrontMag);
  43. if(USED_MAG_CHECK_BAREBOARD_VALUE_START < FrontMag[0] && FrontMag[0] < USED_MAG_CHECK_BAREBOARD_VALUE_END){
  44. IMU_SetBareBoardMode(1); //进入板子模式
  45. }else{
  46. IMU_SetBareBoardMode(0); //不进入板子模式
  47. }
  48. }
  49. }
  50. void app_calibration_is_Calibration_Process(void)
  51. {
  52. if(IMU_GetCurrentMode() == STATE_BAREBOARD_MODE)
  53. {
  54. if(IMU_GetBareBoardMode() == 1)IMU_SetBareBoardMode(0); //进入板子模式后,将进入板子模式的标志位清零。
  55. IMU_GetAcc(Acc);
  56. IMU_GetGry(Gry);
  57. IMU_GetMagFront(FrontMag);
  58. ImuCalibration_pcs(Acc, Gry, FrontMag);
  59. if(ImuCalibration_GetState() == ImuCal_GetData){
  60. nrf_gpio_pin_write(PIN_LED_RUN,0);
  61. IMU_SetCalibrationMode(1); //进入校准模式
  62. }else{
  63. if(FrontMag[0] >= USED_MAG_CHECK_BAREBOARD_VALUE_END || USED_MAG_CHECK_BAREBOARD_VALUE_START >= FrontMag[0]){
  64. IMU_SetLowPowerMode(1); //进入低功耗模式。
  65. }
  66. }
  67. }
  68. }
  69. void app_calibration_Calibration_in_Process(void)
  70. {
  71. if(IMU_GetCurrentMode() == STATE_CALIBRATION_MODE)
  72. {
  73. if(IMU_GetCalibrationMode() == 1)IMU_SetCalibrationMode(0); //进入板子校准模式后,将进入板子校准模式的标志位清零。
  74. IMU_GetAcc(Acc);
  75. IMU_GetGry(Gry);
  76. IMU_GetMagFront(FrontMag);
  77. ImuCalibration_pcs(Acc, Gry, FrontMag);
  78. if(ImuCalibration_GetState() == ImuCal_finish){
  79. Flash_SaveBackup();
  80. Mahony_PRINT("ImuCal_state ImuCal_finish");
  81. Process_Start(2000,"app_calibration_CalLed_Process",app_calibration_CalLed_Process);
  82. Process_UpdatePeroid(app_calibration_CalLed_Process,2000);
  83. IMU_SetLowPowerMode(1); //进入低功耗模式。
  84. }else if(ImuCalibration_GetState() == ImuCal_error || ImuCalibration_GetState() == ImuCal_quiet){
  85. Mahony_PRINT("ImuCal_state ImuCal_error");
  86. if(app_charge_Getstate() == BLE_Client_T_CHARGE_INSERT || app_charge_Getstate() == BLE_Client_T_CHARGE_DONE){ //若是充电,重新校准
  87. ImuCalibration_SetState(ImuCal_init);
  88. IMU_SetBareBoardMode(1); //进入板子模式
  89. }else{
  90. Process_Start(100,"app_calibration_CalLed_Process",app_calibration_CalLed_Process);
  91. Process_UpdatePeroid(app_calibration_CalLed_Process,100);
  92. IMU_SetLowPowerMode(1); //进入低功耗模式。
  93. }
  94. }
  95. }
  96. }
  97. void app_calibration_Init(void)
  98. {
  99. Process_Start(1000,"app_calibration_is_BareBoard_Process",app_calibration_is_BareBoard_Process); //是否进入板子模式
  100. Process_Start(200,"app_calibration_is_Calibration_Process",app_calibration_is_Calibration_Process); //是否进入板子校准模式
  101. Process_Start(10,"app_calibration_Calibration_in_Process",app_calibration_Calibration_in_Process); //校准处理中
  102. }