drv_qmc6310_center.c 8.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340
  1. /**
  2. ******************************************************************************
  3. * @file qmc6310.c
  4. * @author STMicroelectronics
  5. * @version V1.0
  6. * @date 2013-xx-xx
  7. * @brief qmc6310动
  8. ******************************************************************************
  9. * @attention
  10. *
  11. * 实验平台:秉火 指南者 开发板
  12. * 论坛 :http://www.firebbs.cn
  13. * 淘宝 :https://fire-stm32.taobao.com
  14. *
  15. ******************************************************************************
  16. */
  17. #include "drv_qmc6310.h"
  18. #include <math.h>
  19. #include "lsm6ds3tr_c.h"
  20. #include "nrf_delay.h"
  21. static uint8_t chipid = 0;
  22. static QMC6310_map c_map;
  23. //bool MPU9250_Write_Byte(uint8_t Device_Address,uint8_t REG_Address,uint8_t REG_data)
  24. //{
  25. // return IIC_WriteBytes(Device_Address,REG_Address,&REG_data,1);
  26. //}
  27. //bool MPU9250_Read_nBytes(uint8_t Device_Address,uint8_t REG_Address,uint8_t *readDataBuf,uint8_t readDataLen)
  28. //{
  29. // return IIC_ReadBytes(Device_Address,REG_Address,readDataBuf,readDataLen);
  30. //}
  31. //bool MPU9250_register_write_len(uint8_t Device_Address,uint8_t register_address, uint8_t len,uint8_t *buf)
  32. //{
  33. // return IIC_WriteBytes(Device_Address,register_address,buf,len);
  34. //}
  35. //bool MPU9250_register_read_len(uint8_t Device_Address,uint8_t register_address, uint8_t number_of_bytes,uint8_t * destination )
  36. //{
  37. // return IIC_ReadBytes(Device_Address,register_address,destination,number_of_bytes);
  38. //}
  39. static int qmc6310_read_block(uint8_t addr, uint8_t *data, uint8_t len)
  40. {
  41. if(lsm6ds3tr_c_switch_read_sensor_hub_cx(LSM_DEV_CENTER, 0x1C, addr, data,len) == 0)
  42. {
  43. return 1;
  44. }
  45. return 0;
  46. }
  47. static int qmc6310_write_reg(uint8_t addr, uint8_t data)
  48. {
  49. if(lsm6ds3_c_switch_write_sensor_hub_cx(LSM_DEV_CENTER, 0x1C, addr, &data) == 0)
  50. {
  51. return true;
  52. }
  53. return false;
  54. }
  55. static void qmc6310_set_layout(int layout)
  56. {
  57. if(layout == 0)
  58. {
  59. c_map.sign[AXIS_X] = 1;
  60. c_map.sign[AXIS_Y] = 1;
  61. c_map.sign[AXIS_Z] = 1;
  62. c_map.map[AXIS_X] = AXIS_X;
  63. c_map.map[AXIS_Y] = AXIS_Y;
  64. c_map.map[AXIS_Z] = AXIS_Z;
  65. }
  66. else if(layout == 1)
  67. {
  68. c_map.sign[AXIS_X] = -1;
  69. c_map.sign[AXIS_Y] = 1;
  70. c_map.sign[AXIS_Z] = 1;
  71. c_map.map[AXIS_X] = AXIS_Y;
  72. c_map.map[AXIS_Y] = AXIS_X;
  73. c_map.map[AXIS_Z] = AXIS_Z;
  74. }
  75. else if(layout == 2)
  76. {
  77. c_map.sign[AXIS_X] = -1;
  78. c_map.sign[AXIS_Y] = -1;
  79. c_map.sign[AXIS_Z] = 1;
  80. c_map.map[AXIS_X] = AXIS_X;
  81. c_map.map[AXIS_Y] = AXIS_Y;
  82. c_map.map[AXIS_Z] = AXIS_Z;
  83. }
  84. else if(layout == 3)
  85. {
  86. c_map.sign[AXIS_X] = 1;
  87. c_map.sign[AXIS_Y] = -1;
  88. c_map.sign[AXIS_Z] = 1;
  89. c_map.map[AXIS_X] = AXIS_Y;
  90. c_map.map[AXIS_Y] = AXIS_X;
  91. c_map.map[AXIS_Z] = AXIS_Z;
  92. }
  93. else if(layout == 4)
  94. {
  95. c_map.sign[AXIS_X] = -1;
  96. c_map.sign[AXIS_Y] = 1;
  97. c_map.sign[AXIS_Z] = -1;
  98. c_map.map[AXIS_X] = AXIS_X;
  99. c_map.map[AXIS_Y] = AXIS_Y;
  100. c_map.map[AXIS_Z] = AXIS_Z;
  101. }
  102. else if(layout == 5)
  103. {
  104. c_map.sign[AXIS_X] = 1;
  105. c_map.sign[AXIS_Y] = 1;
  106. c_map.sign[AXIS_Z] = -1;
  107. c_map.map[AXIS_X] = AXIS_Y;
  108. c_map.map[AXIS_Y] = AXIS_X;
  109. c_map.map[AXIS_Z] = AXIS_Z;
  110. }
  111. else if(layout == 6)
  112. {
  113. c_map.sign[AXIS_X] = 1;
  114. c_map.sign[AXIS_Y] = -1;
  115. c_map.sign[AXIS_Z] = -1;
  116. c_map.map[AXIS_X] = AXIS_X;
  117. c_map.map[AXIS_Y] = AXIS_Y;
  118. c_map.map[AXIS_Z] = AXIS_Z;
  119. }
  120. else if(layout == 7)
  121. {
  122. c_map.sign[AXIS_X] = -1;
  123. c_map.sign[AXIS_Y] = -1;
  124. c_map.sign[AXIS_Z] = -1;
  125. c_map.map[AXIS_X] = AXIS_Y;
  126. c_map.map[AXIS_Y] = AXIS_X;
  127. c_map.map[AXIS_Z] = AXIS_Z;
  128. }
  129. else
  130. {
  131. c_map.sign[AXIS_X] = 1;
  132. c_map.sign[AXIS_Y] = 1;
  133. c_map.sign[AXIS_Z] = 1;
  134. c_map.map[AXIS_X] = AXIS_X;
  135. c_map.map[AXIS_Y] = AXIS_Y;
  136. c_map.map[AXIS_Z] = AXIS_Z;
  137. }
  138. }
  139. static int qmc6310_get_chipid(void)
  140. {
  141. int ret = 0;
  142. ret = qmc6310_read_block(QMC6310_CHIP_ID_REG, &chipid , 1);
  143. if(ret == 0){
  144. SEGGER_RTT_printf(0,"change_iic_addr = 0x3c\n");
  145. }
  146. else
  147. {
  148. SEGGER_RTT_printf(0,"QMC6308_get_chipid chipid = 0x%x,i2c_addr = 0x2c\n", chipid);
  149. return 1;
  150. }
  151. ret = qmc6310_read_block(QMC6310_CHIP_ID_REG, &chipid , 1);
  152. if(ret == 0){
  153. SEGGER_RTT_printf(0,"change_iic_addr = 0x1c\n");
  154. }
  155. else
  156. {
  157. SEGGER_RTT_printf(0,"QMC6310_get_chipid chipid = 0x%x,i2c_addr = 0x3c\n", chipid);
  158. return 1;
  159. }
  160. ret = qmc6310_read_block(QMC6310_CHIP_ID_REG, &chipid , 1);
  161. if(ret == 0){
  162. SEGGER_RTT_printf(0,"Get_Chip_ID_Failed!\n");
  163. return 0;
  164. }
  165. else
  166. {
  167. SEGGER_RTT_printf(0,"QMC6310_get_chipid chipid = 0x%x,i2c_addr = 0x1c\n", chipid);
  168. return 1;
  169. }
  170. }
  171. uint8_t qmc6310_center_read_mag_xyz(int16_t *data)
  172. {
  173. int res;
  174. unsigned char mag_data[6];
  175. int16_t hw_d[3] = {0};
  176. int16_t raw_c[3];
  177. int t1 = 0;
  178. unsigned char rdy = 0;
  179. /* Check status register for data availability */
  180. while(!(rdy & 0x01) && (t1 < 5))
  181. {
  182. rdy = QMC6310_STATUS_REG;
  183. res = qmc6310_read_block(QMC6310_STATUS_REG, &rdy, 1);
  184. t1++;
  185. }
  186. mag_data[0] = QMC6310_DATA_OUT_X_LSB_REG;
  187. res = qmc6310_read_block(QMC6310_DATA_OUT_X_LSB_REG, mag_data, 6);
  188. if(res == 0)
  189. {
  190. return 0;
  191. }
  192. hw_d[0] = (int16_t)(((mag_data[1]) << 8) | mag_data[0]);
  193. hw_d[1] = (int16_t)(((mag_data[3]) << 8) | mag_data[2]);
  194. hw_d[2] = (int16_t)(((mag_data[5]) << 8) | mag_data[4]);
  195. //Unit:mG 1G = 100uT = 1000mG
  196. //SEGGER_RTT_printf(0,"Hx=%d, Hy=%d, Hz=%d\n",hw_d[0],hw_d[1],hw_d[2]);
  197. raw_c[AXIS_X] = (int16_t)(c_map.sign[AXIS_X]*hw_d[c_map.map[AXIS_X]]);
  198. raw_c[AXIS_Y] = (int16_t)(c_map.sign[AXIS_Y]*hw_d[c_map.map[AXIS_Y]]);
  199. raw_c[AXIS_Z] = (int16_t)(c_map.sign[AXIS_Z]*hw_d[c_map.map[AXIS_Z]]);
  200. #if DEBUG_IMU
  201. SEGGER_RTT_printf(0,"center raw_c[0]=%d,raw_c[1]=%d,raw_c[2]=%d\n",raw_c[0],raw_c[1],raw_c[2]);
  202. #endif
  203. data[0] = raw_c[0];
  204. data[1] = raw_c[1];
  205. data[2] = raw_c[2];
  206. return res;
  207. }
  208. ///* Set the sensor mode */
  209. //static int qmc6310_set_mode(unsigned char mode)
  210. //{
  211. // int err = 0;
  212. // unsigned char ctrl1_value = 0;
  213. //
  214. // err = qmc6310_read_block(QMC6310_CTL_REG_ONE, &ctrl1_value, 1);
  215. // ctrl1_value = (ctrl1_value&(~0x03))|mode;
  216. // SEGGER_RTT_printf(0,"QMC6310_set_mode, 0x0A = [%02x]->[%02x] \r\n", QMC6310_CTL_REG_ONE,ctrl1_value);
  217. // err = qmc6310_write_reg(QMC6310_CTL_REG_ONE, ctrl1_value);
  218. // return err;
  219. //}
  220. //static int qmc6310_set_output_data_rate(unsigned char rate){
  221. //
  222. // int err = 0;
  223. // unsigned char ctrl1_value = 0;
  224. //
  225. // err = qmc6310_read_block(QMC6310_CTL_REG_ONE, &ctrl1_value, 1);
  226. // ctrl1_value = (ctrl1_value& (~0xE8)) | (rate << 5);
  227. // SEGGER_RTT_printf(0,"QMC6310_set_output_data_rate, 0x0A = [%02x]->[%02x] \r\n", QMC6310_CTL_REG_ONE,ctrl1_value);
  228. // err = qmc6310_write_reg(QMC6310_CTL_REG_ONE, ctrl1_value);
  229. // return err;
  230. //}
  231. static int qmc6310_reset(void)
  232. {
  233. uint16_t timeout = 200;
  234. unsigned char ctrl_value = 0;
  235. do{
  236. qmc6310_write_reg(0x0b, 0x80); //reset
  237. nrf_delay_ms(100);
  238. qmc6310_read_block(0x0b, &ctrl_value, 1);
  239. timeout--;
  240. SEGGER_RTT_printf(0,"---->center qmc6310_reset timeout:%d ctrl_value:0x%x\r\n",timeout,ctrl_value);
  241. }while(timeout !=0 && ctrl_value != 0x80);
  242. if(timeout !=0 && ctrl_value != 0x80)SEGGER_RTT_printf(0,"---->center qmc6310_reset timeout:%d\r\n",timeout);
  243. qmc6310_write_reg(0x0b, 0x00); //取消reset
  244. return 1;
  245. }
  246. static int qmc6310_center_CheckIsConfigure(uint8_t hz)
  247. {
  248. unsigned char ctrl_value;
  249. qmc6310_read_block(0x0d, &ctrl_value, 1);
  250. nrf_delay_ms(3);
  251. // SEGGER_RTT_printf(0,"QMC6310 02 0x%x=0x%x \r\n", 0x0d, ctrl_value);
  252. if(ctrl_value != 0x40) return 0;
  253. qmc6310_read_block(QMC6310_CTL_REG_TWO, &ctrl_value, 1);
  254. nrf_delay_ms(3);
  255. // SEGGER_RTT_printf(0,"QMC6310 02 0x%x=0x%x \r\n", QMC6310_CTL_REG_TWO, ctrl_value);
  256. if(ctrl_value != 0x00) return 0;
  257. qmc6310_read_block(QMC6310_CTL_REG_ONE, &ctrl_value, 1);
  258. nrf_delay_ms(3);
  259. // SEGGER_RTT_printf(0,"QMC6310 02 0x%x=0x%x \r\n", QMC6310_CTL_REG_ONE, ctrl_value);
  260. if(ctrl_value != hz) return 0;
  261. return 1;
  262. }
  263. int qmc6310_center_ReConfigure(uint8_t hz)
  264. {
  265. if(!qmc6310_write_reg(0x29, 0x06))return 0;//(Define the sign for X Y and Z axis)
  266. if(!qmc6310_write_reg(0x0d, 0x40))return 0;
  267. if(!qmc6310_write_reg(0x0b, 0x00))return 0; //30 GS
  268. if(!qmc6310_write_reg(0x0a, hz))return 0;
  269. return 1;
  270. }
  271. int qmc6310_center_Suspend(void)
  272. {
  273. return qmc6310_reset();
  274. }
  275. QMC6310_map qmc6310_center_GetMap(void)
  276. {
  277. return c_map;
  278. }
  279. int qmc6310_center_Init(uint8_t hz)
  280. {
  281. qmc6310_reset();
  282. int ret = 0;
  283. ret = qmc6310_get_chipid();
  284. if(ret==0)
  285. {
  286. return 0;
  287. }
  288. qmc6310_set_layout(0);
  289. qmc6310_reset();
  290. qmc6310_center_ReConfigure(hz);
  291. return qmc6310_center_CheckIsConfigure(hz);
  292. }