app_ImuCalibration.c 15 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488
  1. #include <math.h>
  2. #include "stdio.h"
  3. #include "ble_comm.h"
  4. #include "hal_flash.h"
  5. #include "hal_imu.h"
  6. #include "system.h"
  7. #include "bsp_time.h"
  8. #include "app_ImuCalibration.h"
  9. #include "app_charge.h"
  10. #include "hal_ble_client.h"
  11. #if CALIBRATION_ENANBLE
  12. enum{
  13. ImuCal_init,
  14. ImuCal_GetData,
  15. ImuCal_Analyze,
  16. ImuCal_finish,
  17. ImuCal_error,
  18. ImuCal_quiet,
  19. };
  20. char printfbuf[256];
  21. void send_ANO(unsigned char fun, unsigned char* p, int len);
  22. #define Mahony_PRINT(...) send_ANO(0,(unsigned char*)printfbuf,sprintf(printfbuf,__VA_ARGS__))
  23. #define ERROR_PIN_ON nrf_gpio_pin_write(PIN_LED_RUN,0);
  24. #define ERROR_PIN_OFF nrf_gpio_pin_write(PIN_LED_RUN,1);
  25. static float invSampleFreq = 0.010f; //采样率(Hz)
  26. //匿名四轴上位机api
  27. void send_ANO(unsigned char fun, unsigned char* p, int len){
  28. unsigned char buf[256];
  29. int L = 0;
  30. unsigned char ver = 0;
  31. buf[L] = 0xAA;
  32. ver += buf[L++];
  33. buf[L] = 0x05;
  34. ver += buf[L++];
  35. buf[L] = 0xAF;
  36. ver += buf[L++];
  37. buf[L] = fun;
  38. ver += buf[L++];
  39. buf[L] = len;
  40. ver += buf[L++];
  41. for (int i = 0; i < len; i++)
  42. {
  43. buf[L] = p[i];
  44. ver += buf[L++];
  45. }
  46. buf[L++] = ver;
  47. // extern void send_bytes_client(unsigned char* bytes, int len);
  48. send_bytes_client(buf, L);
  49. // SEGGER_RTT_Write(0,buf, L);
  50. // ESB_SendBuff(buf,L);
  51. }
  52. void send_ANO_Quaternion(float* Q){
  53. unsigned char buf[256];
  54. unsigned char L = 0;
  55. int quat[4];
  56. quat[0] = Q[0] * 10000;
  57. quat[1] = Q[1] * 10000;
  58. quat[2] = Q[2] * 10000;
  59. quat[3] = Q[3] * 10000;
  60. buf[L++] = (unsigned char)(quat[0] >> 24);
  61. buf[L++] = (unsigned char)(quat[0] >> 16);
  62. buf[L++] = (unsigned char)(quat[0] >> 8);
  63. buf[L++] = (unsigned char)(quat[0] >> 0);
  64. buf[L++] = (unsigned char)(quat[1] >> 24);
  65. buf[L++] = (unsigned char)(quat[1] >> 16);
  66. buf[L++] = (unsigned char)(quat[1] >> 8);
  67. buf[L++] = (unsigned char)(quat[1] >> 0);
  68. buf[L++] = (unsigned char)(quat[2] >> 24);
  69. buf[L++] = (unsigned char)(quat[2] >> 16);
  70. buf[L++] = (unsigned char)(quat[2] >> 8);
  71. buf[L++] = (unsigned char)(quat[2] >> 0);
  72. buf[L++] = (unsigned char)(quat[3] >> 24);
  73. buf[L++] = (unsigned char)(quat[3] >> 16);
  74. buf[L++] = (unsigned char)(quat[3] >> 8);
  75. buf[L++] = (unsigned char)(quat[3] >> 0);
  76. buf[L++] = 0;
  77. send_ANO(0x03, buf, L);
  78. }
  79. void send_ANO_STATUS(float _roll, float _pitch, float _yaw, float _posx, float _posy, float _posz){
  80. unsigned char buf[256];
  81. unsigned char L = 0;
  82. short roll = _roll * 100;
  83. short pitch = _pitch * 100;
  84. short yaw = _yaw * 100;
  85. short posx = _posx * 100;
  86. short posy = _posy * 100;
  87. short posz = _posz * 100;
  88. buf[L++] = (unsigned char)(roll >> 8);
  89. buf[L++] = (unsigned char)(roll >> 0);
  90. buf[L++] = (unsigned char)(pitch >> 8);
  91. buf[L++] = (unsigned char)(pitch >> 0);
  92. buf[L++] = (unsigned char)(yaw >> 8);
  93. buf[L++] = (unsigned char)(yaw >> 0);
  94. buf[L++] = (unsigned char)(posx >> 8);
  95. buf[L++] = (unsigned char)(posx >> 0);
  96. buf[L++] = (unsigned char)(posy >> 8);
  97. buf[L++] = (unsigned char)(posy >> 0);
  98. buf[L++] = (unsigned char)(posz >> 8);
  99. buf[L++] = (unsigned char)(posz >> 0);
  100. buf[L++] = 0;
  101. send_ANO(0x01, buf, L);
  102. }
  103. void send_ANO_SENSER(short gx, short gy, short gz, short ax, short ay, short az, short mx, short my, short mz){
  104. unsigned char buf[256];
  105. unsigned char L = 0;
  106. buf[L++] = (unsigned char)(ax >> 8);
  107. buf[L++] = (unsigned char)(ax >> 0);
  108. buf[L++] = (unsigned char)(ay >> 8);
  109. buf[L++] = (unsigned char)(ay >> 0);
  110. buf[L++] = (unsigned char)(az >> 8);
  111. buf[L++] = (unsigned char)(az >> 0);
  112. buf[L++] = (unsigned char)(gx >> 8);
  113. buf[L++] = (unsigned char)(gx >> 0);
  114. buf[L++] = (unsigned char)(gy >> 8);
  115. buf[L++] = (unsigned char)(gy >> 0);
  116. buf[L++] = (unsigned char)(gz >> 8);
  117. buf[L++] = (unsigned char)(gz >> 0);
  118. buf[L++] = (unsigned char)(mx >> 8);
  119. buf[L++] = (unsigned char)(mx >> 0);
  120. buf[L++] = (unsigned char)(my >> 8);
  121. buf[L++] = (unsigned char)(my >> 0);
  122. buf[L++] = (unsigned char)(mz >> 8);
  123. buf[L++] = (unsigned char)(mz >> 0);
  124. send_ANO(0x02, buf, L);
  125. }
  126. //LDLT分解法解线性方程组,和LDLTBKSB_6一起用
  127. char LDLTDCMP_6(int n, float (*a)[6]){
  128. int k;
  129. int m;
  130. int i;
  131. for (k = 0; k < n; k++){
  132. for (m = 0; m < k; m++){
  133. a[k][k] = a[k][k] - a[m][k] * a[k][m];
  134. }
  135. if (a[k][k] == 0){
  136. return 1;//error
  137. }
  138. for(i = k + 1; i < n; i++){
  139. for (m = 0; m < k; m++){
  140. a[k][i] = a[k][i] - a[m][i] * a[k][m];
  141. }
  142. a[i][k] = a[k][i] / a[k][k];
  143. }
  144. }
  145. return 0;
  146. }
  147. //LDLT分解法解线性方程组,和LDLTDCMP_6一起用
  148. void LDLTBKSB_6(int n, float (*a)[6], float* b)
  149. {
  150. int k;
  151. int i;
  152. for (i = 0; i < n; i++){
  153. for (k = 0; k < i; k++){
  154. b[i] = b[i] - a[i][k] * b[k];
  155. }
  156. }
  157. for (i = n - 1; i >= 0; i--){
  158. b[i] = b[i] / a[i][i];
  159. for (k = i + 1; k < n; k++){
  160. b[i] = b[i] - a[k][i] * b[k];
  161. }
  162. }
  163. }
  164. float Acc_static_calibration_b[6] = {0.0f};
  165. float Acc_static_calibration_D[6][6] = {0.0f};
  166. float Acc_static_calibration_out[6] = {1.000718f, 1.001100f, 0.988632f, 0.012943f, 0.006423f, 0.0034f}; //V1.3板
  167. void Ellipsoidfit_six_pram_update(float X, float Y, float Z, float* b, float (*D)[6]){
  168. float coft[6] = {0.0f};
  169. int r, j;
  170. coft[0] = X * X;
  171. coft[3] = 2.0f * X;
  172. coft[1] = Y * Y;
  173. coft[4] = 2.0f * Y;
  174. coft[2] = Z * Z;
  175. coft[5] = 2.0f * Z;
  176. for (j = 0; j < 6; j++){
  177. b[j] += coft[j];
  178. }
  179. for (r = 0; r < 6; r++){
  180. for (j = 0; j <= r; j++){
  181. D[r][j] += coft[r] * coft[j];
  182. }
  183. }
  184. for (r = 0; r < 6; r++){
  185. for (j = 5; j > r; j--){
  186. D[r][j] = D[j][r];
  187. }
  188. }
  189. }
  190. void Ellipsoidfit_six_pram_Solution(float* b, float (*D)[6], float* out)
  191. {
  192. float temp = 0;
  193. //解线性方程组,求解超定方程最小二乘解
  194. LDLTDCMP_6(6, D);
  195. LDLTBKSB_6(6, D, b);
  196. temp = b[1] * b[2] * b[3] * b[3] + b[0] * b[2] * b[4] * b[4] + b[0] * b[1] * b[5] * b[5];
  197. temp = 1.0f - (temp / (temp + (b[0] * b[1] * b[2])));
  198. out[0] = sqrtf(temp * b[0]);
  199. out[1] = sqrtf(temp * b[1]);
  200. out[2] = sqrtf(temp * b[2]);
  201. out[3] = b[3] * temp / out[0];
  202. out[4] = b[4] * temp / out[1];
  203. out[5] = b[5] * temp / out[2];
  204. }
  205. static float Acc_before[3];
  206. static float accmod=0,accmodbef=0;
  207. static char cun=0;
  208. static unsigned int timer_cli=0;
  209. //需要200ms执行一次,开始信号检测,检测到返回1,否则返回0
  210. char begin_REC(float *acccli)
  211. {
  212. float gyrmod = 0;
  213. gyrmod = (acccli[0]*Acc_before[0]) + (acccli[1]*Acc_before[1]) + (acccli[2]*Acc_before[2]);
  214. accmod = sqrtf(acccli[0] * acccli[0] + acccli[1] * acccli[1] + acccli[2] * acccli[2]);
  215. gyrmod = gyrmod/(accmod * accmodbef);
  216. gyrmod = acosf(gyrmod)*3.14f;
  217. Acc_before[0]=acccli[0];Acc_before[1]=acccli[1];Acc_before[2]=acccli[2];
  218. accmodbef=accmod;
  219. Mahony_PRINT("gyrmod:%f,timer:%d cun:%d %f\n",gyrmod,TIME_GetTicks()-timer_cli,cun,accmodbef);
  220. timer_cli=TIME_GetTicks();
  221. if((gyrmod > 0.25f)&&(gyrmod < 0.75f))
  222. {
  223. cun++;
  224. }else
  225. {
  226. // if(cun>=3)cun--;else
  227. cun=0;
  228. }
  229. if(cun>=10){
  230. cun=0;
  231. return 1;
  232. }
  233. else return 0;
  234. }
  235. static uint8_t ImuCal_state = ImuCal_init;
  236. char Acc_static_calibration(float* Acc_in, float* Acc_out, const float* gyr)
  237. {
  238. static int temp = 0;
  239. static int overtime = 0;
  240. static int caiyingcun = 0;
  241. switch (ImuCal_state){
  242. case ImuCal_init:{//未校准状态
  243. if(begin_REC(Acc_in)){
  244. ImuCal_state = ImuCal_GetData;
  245. caiyingcun = 0;
  246. overtime = 0;
  247. for (int i = 0; i < 6; i++){
  248. Acc_static_calibration_b[i] = 0.0f;
  249. Acc_static_calibration_D[i][0] = 0.0f;
  250. Acc_static_calibration_D[i][1] = 0.0f;
  251. Acc_static_calibration_D[i][2] = 0.0f;
  252. Acc_static_calibration_D[i][3] = 0.0f;
  253. Acc_static_calibration_D[i][4] = 0.0f;
  254. Acc_static_calibration_D[i][5] = 0.0f;
  255. }
  256. ERROR_PIN_ON
  257. Mahony_PRINT("Acc_static_calibration -> start \r\n");
  258. temp = 0;
  259. }
  260. }
  261. break;
  262. case ImuCal_GetData:{//采集校准数据状态
  263. float gyrmod = sqrtf(gyr[0] * gyr[0] + gyr[1] * gyr[1] + gyr[2] * gyr[2]);
  264. if (gyrmod < 18.27f){ //静止检测
  265. //采样数据
  266. if(((Acc_in[0]<2.0f)||(Acc_in[0]>-2.0f))&&((Acc_in[1]<2.0f)||(Acc_in[1]>-2.0f))&&((Acc_in[2]<2.0f)||(Acc_in[2]>-2.0f))){
  267. Ellipsoidfit_six_pram_update(Acc_in[0], Acc_in[1], Acc_in[2], Acc_static_calibration_b, Acc_static_calibration_D);
  268. caiyingcun++;
  269. send_ANO_SENSER(gyr[0] * 100, gyr[1] * 100, gyr[2] * 100, Acc_in[0] * 100, Acc_in[1] * 100, Acc_in[2] * 100, 0, 0, 0);
  270. }
  271. }
  272. //检测结束条件
  273. if ((gyrmod > 25.0f) && (gyrmod < 60.0f) && (caiyingcun > 20)){
  274. temp = temp + 1;
  275. }
  276. else{
  277. if(temp>50)temp--;else temp = 0;
  278. }
  279. if (temp * invSampleFreq > 2.0f){
  280. Ellipsoidfit_six_pram_Solution(Acc_static_calibration_b, Acc_static_calibration_D, Acc_static_calibration_out);
  281. ImuCal_state = ImuCal_Analyze;
  282. Mahony_PRINT("ImuCal_GetData");
  283. }
  284. else
  285. {
  286. // Mahony_PRINT("ImuCal_GetData temp %d",temp);
  287. }
  288. overtime++;
  289. if (overtime * invSampleFreq > 300.0f){
  290. ImuCal_state = ImuCal_error;
  291. Mahony_PRINT("ImuCal overtime ImuCal_error");
  292. }
  293. }
  294. break;
  295. case ImuCal_Analyze:{//校准数据处理状态
  296. float gyrmod = sqrtf(gyr[0] * gyr[0] + gyr[1] * gyr[1] + gyr[2] * gyr[2]);
  297. if (gyrmod < 18.27f){ //静止检测
  298. if (((Acc_static_calibration_out[0] > 0.85f) && (Acc_static_calibration_out[0] < 1.15f)) &&
  299. ((Acc_static_calibration_out[1] > 0.85f) && (Acc_static_calibration_out[1] < 1.15f)) &&
  300. ((Acc_static_calibration_out[2] > 0.85f) && (Acc_static_calibration_out[2] < 1.15f))){
  301. // Mahony_PRINT("out[%f,%f,%f,%f,%f,%f] \r\n", Acc_static_calibration_out[0], Acc_static_calibration_out[1], Acc_static_calibration_out[2], Acc_static_calibration_out[3], Acc_static_calibration_out[4], Acc_static_calibration_out[5]);
  302. Mahony_PRINT("ImuCal_state ImuCal_finish");
  303. temp = 0;
  304. ImuCal_state = ImuCal_finish;
  305. for(int a = 0; a < 6; a++){
  306. mBackup.cal[a] = Acc_static_calibration_out[a];
  307. }
  308. }
  309. else{
  310. ImuCal_state = ImuCal_quiet;
  311. }
  312. overtime = 0;
  313. }
  314. }
  315. break;
  316. // case ImuCal_finish:{//已经校准完状态
  317. // Mahony_PRINT("ImuCal_state ImuCal_init");
  318. // ImuCal_state = 0;
  319. // }
  320. // break;
  321. // case ImuCal_error:{//校准过程中断退出状态
  322. // Mahony_PRINT("ImuCal_state ImuCal_init");
  323. // ImuCal_state = 0;
  324. // Mahony_PRINT("ImuCal_state ImuCal_init");
  325. // }
  326. // break;
  327. // case ImuCal_quiet:{//校准过程中断退出状态
  328. // ImuCal_state = 0;
  329. // }
  330. // break;
  331. }
  332. return ImuCal_state;
  333. }
  334. void Mahony_imu_lbs(short* Acc_in, short* Gyr_in, short* Mag_in, float* Acc, float* Gyr, float* Mag)
  335. {
  336. float ACC_LBS = 32768.0f / 16.0f;
  337. float GYR_LBS = 32768.0f / 2000.0f;
  338. Acc[0] = Acc_in[0] / ACC_LBS;
  339. Acc[1] = Acc_in[1] / ACC_LBS;
  340. Acc[2] = Acc_in[2] / ACC_LBS;
  341. Gyr[0] = Gyr_in[0] / GYR_LBS;
  342. Gyr[1] = Gyr_in[1] / GYR_LBS;
  343. Gyr[2] = Gyr_in[2] / GYR_LBS;
  344. Mag[0] = Mag_in[0] / 1.0f;
  345. Mag[1] = Mag_in[1] / 1.0f;
  346. Mag[2] = Mag_in[2] / 1.0f;
  347. }
  348. static float acc[3], gyr[3], mag[3];
  349. static void ImuCalibration_pcs(short* Acc, short* Gyr, short* Mag)
  350. {
  351. Mahony_imu_lbs(Acc, Gyr, Mag, acc, gyr, mag); //转换IMU数据量程,加速度单位转换为G,陀螺仪单位转换为度每秒,磁力计不需要转换单位,后面会做归一化处理
  352. Acc_static_calibration(acc, NULL, gyr);
  353. }
  354. /***********************************
  355. *未校准:灭灯
  356. *校准中:指示灯常亮
  357. *校准完成:指示灯慢闪,亮10ms,灭灯2S
  358. *校准错误:指示灯快闪,亮10ms,灭灯100ms
  359. ************************************/
  360. static void app_ImuCalLed_process(void){
  361. nrf_gpio_pin_write(PIN_LED_RUN,0);
  362. nrf_delay_ms(10);
  363. nrf_gpio_pin_write(PIN_LED_RUN,1);
  364. char printdata[200]={0};
  365. 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]);
  366. SEGGER_RTT_printf(0,"%s\n",printdata);
  367. 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]);
  368. }
  369. uint8_t app_imucal_getstate(void){
  370. if(ImuCal_GetData == ImuCal_state || ImuCal_Analyze == ImuCal_state)return 1;
  371. else return 0;
  372. }
  373. static void app_cal_process(void){
  374. static uint8_t state =0;
  375. static uint8_t temp =0;
  376. int16_t Acc[3]={0};
  377. int16_t Gry[3]={0};
  378. int16_t Mag[3]={0};
  379. uint8_t isGameMode =0;
  380. switch(state){
  381. case 0:
  382. IMU_GetFrontMagAndLowPowerAcc();
  383. IMU_GetAcc(Acc);
  384. IMU_GetGry(Gry);
  385. ImuCalibration_pcs((short *)Acc,(short *)Gry,(short *)Mag);
  386. if(ImuCal_state == ImuCal_GetData){
  387. Process_UpdatePeroid(app_cal_process,10);
  388. // Process_SetHoldOn(app_cal_process,1);
  389. nrf_gpio_pin_write(PIN_LED_RUN,0);
  390. state =1;
  391. isGameMode = 1;
  392. IMU_SetGameMode(isGameMode);
  393. temp=0;
  394. }
  395. break;
  396. case 1:
  397. if(IMU_GetGameMode()){
  398. state =2;
  399. }else{
  400. temp++;
  401. if(temp >=20)state =0;
  402. }
  403. break;
  404. case 2:
  405. IMU_GetAcc(Acc);
  406. IMU_GetGry(Gry);
  407. ImuCalibration_pcs((short *)Acc,(short *)Gry,(short *)Mag);
  408. // SEGGER_RTT_printf(0,">>>>>>>22222\n");
  409. if(ImuCal_state == ImuCal_finish){
  410. Flash_SaveBackup();
  411. Mahony_PRINT("ImuCal_state ImuCal_finish");
  412. Process_Start(2000,"app_cal_process",app_ImuCalLed_process);
  413. Process_UpdatePeroid(app_ImuCalLed_process,2000);
  414. Process_UpdatePeroid(app_cal_process,200);
  415. // Process_SetHoldOn(app_cal_process,0);
  416. state =3;
  417. }
  418. else if(ImuCal_state == ImuCal_error || ImuCal_state == ImuCal_quiet){
  419. Mahony_PRINT("ImuCal_state ImuCal_error");
  420. Process_Start(100,"app_cal_process",app_ImuCalLed_process);
  421. Process_UpdatePeroid(app_ImuCalLed_process,100);
  422. Process_UpdatePeroid(app_cal_process,200);
  423. // Process_SetHoldOn(app_cal_process,0);
  424. isGameMode = 0;
  425. IMU_SetGameMode(isGameMode);
  426. state =3;
  427. }
  428. break;
  429. case 3:
  430. if(!IMU_GetGameMode()){
  431. state = 4;
  432. }
  433. else if(++temp>20){ temp = 0;
  434. state = 4;
  435. }
  436. break;
  437. case 4:
  438. if(app_charge_Getstate() == BLE_Client_T_CHARGE_INSERT || app_charge_Getstate() == BLE_Client_T_CHARGE_DONE){
  439. state =0;
  440. ImuCal_state = ImuCal_init;
  441. Process_Stop(app_ImuCalLed_process);
  442. }
  443. break;
  444. }
  445. }
  446. void app_ImuCalibration_init(void){
  447. Process_Start(200,"app_cal_process",app_cal_process);
  448. Process_SetHoldOn(app_cal_process,1);
  449. }
  450. #endif