Motion.c 12 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462
  1. /***************************************************************************************************
  2. Motion.c
  3. ****************************************************************************************************/
  4. #define THIS_FILE_ID 19
  5. #include "includes.h"
  6. #define LSM303D_WHO_AM_I_VAL 0x49
  7. #define LSM6DS3_WHO_AM_I_VAL 0x69
  8. MOTION_SENSOR_TYPE MotionSensorType;
  9. SUT_MOTION sutMotion;
  10. /**********************************************************************
  11. DelayNs
  12. ***********************************************************************/
  13. void DelayNs(unsigned short ns)
  14. {
  15. while(ns--);
  16. }
  17. /**********************************************************************
  18. DelayUs
  19. SystemCoreClock=72 Mhz t=70
  20. SystemCoreClock=60 Mhz t=60
  21. ***********************************************************************/
  22. //void DelayUs(unsigned short us)
  23. //{
  24. // int t;
  25. // while(us--){
  26. // t=60;
  27. // while(t--);
  28. // }
  29. //}
  30. //void DelayMs(unsigned short ms)
  31. //{
  32. // while(ms--){
  33. // DelayUs(1000);
  34. // IWDG_ReloadCounter();
  35. // }
  36. //}
  37. static int32_t platform_write(void *handle, uint8_t Reg, uint8_t *Bufp,
  38. uint16_t len)
  39. {
  40. unsigned char data;
  41. unsigned short i,j;
  42. data=Reg;
  43. if(handle==(void *)LSM303AGR_I2C_ADD_XL){
  44. MOTION_CS_XL_LOW;
  45. }else MOTION_CS_MAG_LOW;
  46. DELAY_NOP;
  47. for(i=0;i<8;i++)
  48. {
  49. MOTION_CLK_LOW;
  50. DELAY_NOP;
  51. DELAY_NOP;
  52. if((data<<i)&0x80) MOTION_SDA_HIGH;
  53. else MOTION_SDA_LOW;
  54. DELAY_NOP;
  55. MOTION_CLK_HIGH;
  56. DELAY_NOP;
  57. }
  58. for(j=0;j<len;j++){
  59. data=Bufp[j];
  60. for(i=0;i<8;i++)
  61. {
  62. MOTION_CLK_LOW;
  63. DELAY_NOP;
  64. if((data<<i)&0x80) MOTION_SDA_HIGH;
  65. else MOTION_SDA_LOW;
  66. DELAY_NOP;
  67. MOTION_CLK_HIGH;
  68. DELAY_NOP;
  69. }
  70. }
  71. if(handle==(void *)LSM303AGR_I2C_ADD_XL)MOTION_CS_XL_HIGH;
  72. else MOTION_CS_MAG_HIGH;
  73. DELAY_NOP;
  74. return 0;
  75. }
  76. static int32_t platform_read(void *handle, uint8_t Reg, uint8_t *Bufp,
  77. uint16_t len)
  78. {
  79. unsigned char data;
  80. unsigned short dat=0,i,j;
  81. data=0x80|Reg;
  82. if(handle==(void *)LSM303AGR_I2C_ADD_XL){
  83. MOTION_CS_XL_LOW;
  84. if(len>1)data|=0x40;//地址自增
  85. }else MOTION_CS_MAG_LOW;
  86. DELAY_NOP;
  87. //发地址
  88. for(i=0;i<8;i++)
  89. {
  90. DELAY_NOP;
  91. MOTION_CLK_LOW;
  92. DELAY_NOP;
  93. if((data<<i)&0x80) MOTION_SDA_HIGH;
  94. else MOTION_SDA_LOW;
  95. DELAY_NOP;
  96. MOTION_CLK_HIGH;
  97. DELAY_NOP;
  98. }
  99. //读数据
  100. for(j=0;j<len;j++){
  101. for(i=0;i<8;i++)
  102. {
  103. data<<=1;
  104. DELAY_NOP;
  105. MOTION_CLK_LOW;
  106. DELAY_NOP;
  107. MOTION_CLK_HIGH;
  108. DELAY_NOP;
  109. if(MOTION_SDA_READ) data|=0x01;
  110. }
  111. Bufp[j]=data;
  112. }
  113. if(handle==(void *)LSM303AGR_I2C_ADD_XL)MOTION_CS_XL_HIGH;
  114. else MOTION_CS_MAG_HIGH;
  115. DELAY_NOP;
  116. return 0;
  117. }
  118. /**********************************************************************************************
  119. *
  120. ***********************************************************************************************/
  121. void SPI_LSM_Init()
  122. {
  123. }
  124. void Motion_PortInit()
  125. {
  126. //SPI 初始化
  127. GPIO_InitTypeDef GPIO_InitStructure;
  128. RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOA,ENABLE);
  129. //IP模拟SPI三线式 SCK--PB13 SDA(MISO/MOSI)--PB15
  130. GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13 ;
  131. GPIO_InitStructure.GPIO_Speed =GPIO_Speed_10MHz;
  132. GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
  133. GPIO_Init(GPIOB, &GPIO_InitStructure);
  134. GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15 ;
  135. GPIO_InitStructure.GPIO_Speed =GPIO_Speed_10MHz;
  136. GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
  137. GPIO_Init(GPIOB, &GPIO_InitStructure);
  138. //片选IO设置
  139. RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2,ENABLE);
  140. RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOA,ENABLE);
  141. GPIO_InitStructure.GPIO_Pin = MOTION_CS_XL_PIN|MOTION_CS_MAG_PIN;
  142. GPIO_InitStructure.GPIO_Speed =GPIO_Speed_50MHz;
  143. GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
  144. GPIO_Init(MOTION_CS_XL_PORT, &GPIO_InitStructure);
  145. MOTION_CS_XL_HIGH;
  146. MOTION_CS_MAG_HIGH;
  147. }
  148. //lsm303agr_ctx_t dev_ctx_xl;
  149. //lsm303agr_ctx_t dev_ctx_mg;
  150. static uint8_t whoamI, rst;
  151. int MotionInit(void)
  152. {
  153. unsigned char whoAmI = 0,r,a;
  154. status_t response; // mems error variable
  155. Motion_PortInit();
  156. os_dly_wait(10);
  157. //配置为SPI模式
  158. a=1;
  159. LSM303C_ACC_WriteReg(0,0x23,&a,1);
  160. a=4;
  161. LSM303C_MAG_WriteReg(0,0x22,&a,1);
  162. whoAmI=0;
  163. //读WHO AM I
  164. LSM303C_MAG_R_WHO_AM_I_(0, &whoAmI);
  165. if(whoAmI !=MAG_WHO_AM_I_VALUE){
  166. SlwTrace(DEBUG,"Lsm303C Init Failed.\r\n");
  167. return 0;
  168. }
  169. whoAmI=0;
  170. LSM303C_ACC_ReadReg(0, LSM303C_ACC_WHO_AM_I_REG, &whoAmI, 1);
  171. if(whoAmI !=ACC_WHO_AM_I_VALUE){
  172. SlwTrace(DEBUG,"Lsm303C Init Failed.\r\n");
  173. return 0;
  174. }
  175. //Initialize Magnetometer
  176. //set ODR (turn ON device)
  177. response = LSM303C_MAG_W_OutputDataRate(0, LSM303C_MAG_DO_10Hz);
  178. if(response==MEMS_ERROR) while(1); //manage here comunication error
  179. //set Fullscale
  180. response = LSM303C_MAG_W_FullScale(0, LSM303C_MAG_FS_16Ga);
  181. if(response==MEMS_ERROR) while(1); //manage here comunication error
  182. //set Block Data Update
  183. response = LSM303C_MAG_W_BlockDataUpdate(0, LSM303C_MAG_BDU_ENABLE);
  184. if(response==MEMS_ERROR) while(1); //manage here comunication error
  185. //set XY Axis Operation Mode
  186. response = LSM303C_MAG_W_OperatingModeXY(0, LSM303C_MAG_OM_HIGH);
  187. if(response==MEMS_ERROR) while(1) //manage here comunication error
  188. //set Z Axis Operation Mode
  189. response = LSM303C_MAG_W_OperatingModeZ(0, LSM303C_MAG_OMZ_HIGH);
  190. if(response==MEMS_ERROR) while(1); //manage here comunication error
  191. //set Continuous Mode
  192. response = LSM303C_MAG_W_SystemOperatingMode(0, LSM303C_MAG_MD_CONTINUOUS);
  193. if(response==MEMS_ERROR) while(1); //manage here comunication error
  194. //this flag is used for reading data only when there are new data
  195. //Initialize Accelerometer
  196. //set Fullscale
  197. response = LSM303C_ACC_SetFullScale(0, LSM303C_ACC_FS_2g);
  198. if(response==MEMS_ERROR) while(1); //manage here comunication error
  199. //set Block Data Update
  200. response = LSM303C_ACC_BlockDataUpdate(0, LSM303C_ACC_BDU_ENABLE);
  201. if(response==MEMS_ERROR) while(1); //manage here comunication error
  202. //Enable Axis
  203. response = LSM303C_ACC_EnableAxis(0, LSM303C_ACC_X_ENABLE|LSM303C_ACC_Y_ENABLE|LSM303C_ACC_Z_ENABLE);
  204. if(response==MEMS_ERROR) while(1); //manage here comunication error
  205. //set ODR (turn ON device)
  206. response = LSM303C_ACC_SetODR(0, LSM303C_ACC_ODR_10_Hz);
  207. if(response==MEMS_ERROR) while(1); //manage here comunication error
  208. SlwTrace(DEBUG,"Lsm303C Init OK.\r\n");
  209. return 1;
  210. }
  211. int MotionReadAccel(short *pX,short *pY,short *pZ)
  212. {
  213. u8_t flag_LSM303C_ACC_STATUS_FLAGS;
  214. Type3Axis16bit_U data; // accelerometer row data
  215. status_t response; // mems error variable
  216. response = LSM303C_ACC_Status_Flags(0, &flag_LSM303C_ACC_STATUS_FLAGS);
  217. if(response==MEMS_ERROR)return 0;
  218. else if (flag_LSM303C_ACC_STATUS_FLAGS&LSM303C_ACC_ZYX_NEW_DATA_AVAILABLE){
  219. //read only if new data are available from accelerometer
  220. response = LSM303C_ACC_GetAccRaw(0, data.u8bit);
  221. *pX=data.i16bit[0];
  222. *pY=data.i16bit[1];
  223. *pZ=data.i16bit[2];
  224. }
  225. return 1;
  226. }
  227. int MotionReadMagne(short *pX,short *pY,short *pZ)
  228. {
  229. LSM303C_MAG_ZYXDA_t flag_LSM303C_MAG_XYZDA;
  230. Type3Axis16bit_U data; // accelerometer row data
  231. status_t response; // mems error variable
  232. response = LSM303C_MAG_R_NewXYZData(0, (LSM303C_MAG_ZYXDA_t*) &flag_LSM303C_MAG_XYZDA);
  233. if(response==MEMS_ERROR)return 0;
  234. else if ((LSM303C_MAG_ZYXDA_t) flag_LSM303C_MAG_XYZDA & LSM303C_MAG_ZYXDA_AVAILABLE){
  235. //read only if new data are available from accelerometer
  236. response = LSM303C_MAG_Get_Magnetic(0, data.u8bit);
  237. *pX=data.i16bit[0];
  238. *pY=data.i16bit[1];
  239. *pZ=data.i16bit[2];
  240. }
  241. return 1;
  242. }
  243. /*
  244. 循环采样,1秒一个点
  245. */
  246. void MotionLoop(void)
  247. {
  248. int i,i1,i2;
  249. char flag;
  250. short Ax,Ay,Az,Mx,My,Mz;
  251. int Axp,Ayp,Axm,Aym;
  252. static unsigned long lastTick=50;
  253. unsigned long curTick;
  254. static short Axx[10],Ayy[10],Azz[10],Mxx[10],Myy[10],Mzz[10];
  255. static unsigned char sucCt=0;
  256. //控制执行频率
  257. curTick=os_time_get();
  258. if(curTick>lastTick)lastTick=curTick+50;
  259. else return;
  260. flag=0;
  261. if(MotionReadAccel(&Ax,&Ay,&Az)){
  262. flag=1;
  263. }
  264. if(MotionReadMagne(&Mx,&My,&Mz)){
  265. flag=1;
  266. }
  267. if(flag){
  268. if(++sucCt>9)sucCt=0;
  269. //先存起来
  270. Axx[sucCt]=Ax;
  271. Ayy[sucCt]=Ay;
  272. Azz[sucCt]=Az;
  273. Mxx[sucCt]=Mx;
  274. Myy[sucCt]=My;
  275. Mzz[sucCt]=Mz;
  276. //再处理
  277. if(sucCt==9){//计算处理
  278. //求平均
  279. Axp=0;Ayp=0;
  280. for(i=0;i<10;i++){
  281. Axp+=Axx[i];
  282. Ayp+=Ayy[i];
  283. }
  284. Axp/=10;
  285. Ayp/=10;
  286. Axm=0;Aym=0;
  287. //找到最大偏差点
  288. for(i=0;i<10;i++){
  289. Ax=Axx[i]-Axp;
  290. Ay=Ayy[i]-Ayp;
  291. if(Ax<0)Ax*=-1;
  292. if(Ay<0)Ay*=-1;
  293. if(Ax>Axm){
  294. Axm=Ax;
  295. i1=i;
  296. }
  297. if(Ay>Aym){
  298. Aym=Ay;
  299. i2=i;
  300. }
  301. }
  302. tsk_lock();
  303. sutMotion.Flag=0;
  304. //判断Ax还是Ay偏差最大
  305. if(Axm>Aym){
  306. //Ax偏差最大,取Ax
  307. sutMotion.Ax=Axx[i1];
  308. sutMotion.Ay=Ayy[i1];
  309. sutMotion.Az=Azz[i1];
  310. sutMotion.Mx=Mxx[i1];
  311. sutMotion.My=Myy[i1];
  312. sutMotion.Mz=Mzz[i1];
  313. if(Axm>(Axp+ACC_THRESHOLD))sutMotion.Flag=1;
  314. }else{
  315. //Ay偏差最大,取Ay
  316. sutMotion.Ax=Axx[i2];
  317. sutMotion.Ay=Ayy[i2];
  318. sutMotion.Az=Azz[i2];
  319. sutMotion.Mx=Mxx[i2];
  320. sutMotion.My=Myy[i2];
  321. sutMotion.Mz=Mzz[i2];
  322. if(Aym>(Ayp+ACC_THRESHOLD))sutMotion.Flag=2;
  323. }
  324. tsk_unlock();
  325. //打印
  326. #if 0
  327. float fAx,fAy,fAz,fMx,fMy,fMz;
  328. fAx=sutMotion.Ax*SENSITIVITY_ACC;
  329. fAy=sutMotion.Ay*SENSITIVITY_ACC;
  330. fAz=sutMotion.Az*SENSITIVITY_ACC;
  331. fMx=sutMotion.Mx*SENSITIVITY_MAG;
  332. fMy=sutMotion.My*SENSITIVITY_MAG;
  333. fMz=sutMotion.Mz*SENSITIVITY_MAG;
  334. printf("\r\n");
  335. printf("Ax=%10.3f\r\n",fAx);
  336. printf("Ay=%10.3f\r\n",fAy);
  337. printf("Az=%10.3f\r\n",fAz);
  338. printf("Mx=%10.3f\r\n",fMx);
  339. printf("My=%10.3f\r\n",fMy);
  340. printf("Mz=%10.3f\r\n",fMz);
  341. printf("----Flag=%d---\r\n",sutMotion.Flag);
  342. #endif
  343. }
  344. }else{
  345. SlwTrace(DEBUG,"Motion Test Error!\r\n");
  346. sucCt=0;
  347. tsk_lock();
  348. sutMotion.Ax=0;
  349. sutMotion.Ay=0;
  350. sutMotion.Az=0;
  351. sutMotion.Mx=0;
  352. sutMotion.My=0;
  353. sutMotion.Mz=0;
  354. sutMotion.Flag=0;
  355. tsk_unlock();
  356. }
  357. }
  358. /***************************************************
  359. *每20ms调用一次
  360. ****************************************************/
  361. void MotionTest(void)
  362. {
  363. static unsigned char ct=0;
  364. int Ax,Ay,Az,Mx,My,Mz;
  365. float mgData_X, mgData_Y,mgData_Z; //data in mg
  366. float gaussData_X, gaussData_Y, gaussData_Z; //data in Gauss
  367. u8_t flag_LSM303C_ACC_STATUS_FLAGS;
  368. LSM303C_MAG_ZYXDA_t flag_LSM303C_MAG_XYZDA;
  369. Type3Axis16bit_U data; // accelerometer row data
  370. status_t response; // mems error variable
  371. if(++ct>100)ct=0;
  372. else return;
  373. //get Acceleration Raw data
  374. response = LSM303C_MAG_R_NewXYZData(0, (LSM303C_MAG_ZYXDA_t*) &flag_LSM303C_MAG_XYZDA);
  375. if(response==MEMS_ERROR) SlwTrace(DEBUG,"ERROR111"); //manage here comunication error
  376. //read only if new data are available from accelerometer
  377. else if (flag_LSM303C_ACC_STATUS_FLAGS&LSM303C_ACC_ZYX_NEW_DATA_AVAILABLE)
  378. {
  379. response = LSM303C_ACC_GetAccRaw(0, data.u8bit);
  380. //convert from LSB to mg
  381. mgData_X=data.i16bit[0]*SENSITIVITY_ACC;
  382. mgData_Y=data.i16bit[1]*SENSITIVITY_ACC;
  383. mgData_Z=data.i16bit[2]*SENSITIVITY_ACC;
  384. Ax=data.i16bit[0];
  385. Ax=data.i16bit[1];
  386. Ax=data.i16bit[2];
  387. }
  388. //get Magnetic Raw data
  389. response = LSM303C_MAG_R_NewXYZData(0, (LSM303C_MAG_ZYXDA_t*) &flag_LSM303C_MAG_XYZDA);
  390. if(response==MEMS_ERROR) SlwTrace(DEBUG,"ERROR222"); //manage here comunication error
  391. //read only if new data are available
  392. else if ((LSM303C_MAG_ZYXDA_t) flag_LSM303C_MAG_XYZDA & LSM303C_MAG_ZYXDA_AVAILABLE)
  393. {
  394. response = LSM303C_MAG_Get_Magnetic(0, data.u8bit);
  395. //convert from LSB to Gauss
  396. gaussData_X=data.i16bit[0]*SENSITIVITY_MAG;
  397. gaussData_Y=data.i16bit[1]*SENSITIVITY_MAG;
  398. gaussData_Z=data.i16bit[2]*SENSITIVITY_MAG;
  399. Mx=data.i16bit[0];
  400. My=data.i16bit[1];
  401. Mz=data.i16bit[2];
  402. }
  403. printf("\r\n");
  404. printf("Ax=%10.3f\r\n",mgData_X);
  405. printf("Ay=%10.3f\r\n",mgData_Y);
  406. printf("Az=%10.3f\r\n",mgData_Z);
  407. printf("Mx=%10.3f\r\n",gaussData_X);
  408. printf("My=%10.3f\r\n",gaussData_Y);
  409. printf("Mz=%10.3f\r\n",gaussData_Z);
  410. printf("----\r\n");
  411. }