drv_qmc6310_back.c 9.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400
  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 "drv_iic_back.h"
  20. #include "nrf_delay.h"
  21. #include "tool.h"
  22. static uint8_t chipid = 0;
  23. static QMC6310_map c_map;
  24. //bool MPU9250_Write_Byte(uint8_t Device_Address,uint8_t REG_Address,uint8_t REG_data)
  25. //{
  26. // return IIC_WriteBytes(Device_Address,REG_Address,&REG_data,1);
  27. //}
  28. //bool MPU9250_Read_nBytes(uint8_t Device_Address,uint8_t REG_Address,uint8_t *readDataBuf,uint8_t readDataLen)
  29. //{
  30. // return IIC_ReadBytes(Device_Address,REG_Address,readDataBuf,readDataLen);
  31. //}
  32. //bool MPU9250_register_write_len(uint8_t Device_Address,uint8_t register_address, uint8_t len,uint8_t *buf)
  33. //{
  34. // return IIC_WriteBytes(Device_Address,register_address,buf,len);
  35. //}
  36. //bool MPU9250_register_read_len(uint8_t Device_Address,uint8_t register_address, uint8_t number_of_bytes,uint8_t * destination )
  37. //{
  38. // return IIC_ReadBytes(Device_Address,register_address,destination,number_of_bytes);
  39. //}
  40. static int qmc6310_read_block(uint8_t addr, uint8_t *data, uint8_t len)
  41. {
  42. return IIC_BACK_ReadBytes(QMC6310U_IIC_ADDR,addr,data,len);
  43. // uint16_t error;
  44. // error = I2C_ReadData(0x1c, addr, data, len);
  45. // if(error != TWI_OP_SUCCESS){
  46. // SEGGER_RTT_printf(0,"I2C_ReadData_Failed:%d!\n",error);
  47. // return 0;
  48. // }
  49. // return 1;
  50. }
  51. static int qmc6310_write_reg(uint8_t addr, uint8_t data)
  52. {
  53. return IIC_BACK_WriteBytes(QMC6310U_IIC_ADDR,addr,&data,1);
  54. // uint16_t error;
  55. // error = I2C_WriteData(0x1c, addr, &data, 1);
  56. // if(error != TWI_OP_SUCCESS){
  57. // SEGGER_RTT_printf(0,"I2C_WriteData_Failed:%d!\n",error);
  58. // return 0;
  59. // }
  60. // return 1;
  61. }
  62. static void qmc6310_set_layout(int layout)
  63. {
  64. if(layout == 0)
  65. {
  66. c_map.sign[AXIS_X] = 1;
  67. c_map.sign[AXIS_Y] = 1;
  68. c_map.sign[AXIS_Z] = 1;
  69. c_map.map[AXIS_X] = AXIS_X;
  70. c_map.map[AXIS_Y] = AXIS_Y;
  71. c_map.map[AXIS_Z] = AXIS_Z;
  72. }
  73. else if(layout == 1)
  74. {
  75. c_map.sign[AXIS_X] = -1;
  76. c_map.sign[AXIS_Y] = 1;
  77. c_map.sign[AXIS_Z] = 1;
  78. c_map.map[AXIS_X] = AXIS_Y;
  79. c_map.map[AXIS_Y] = AXIS_X;
  80. c_map.map[AXIS_Z] = AXIS_Z;
  81. }
  82. else if(layout == 2)
  83. {
  84. c_map.sign[AXIS_X] = -1;
  85. c_map.sign[AXIS_Y] = -1;
  86. c_map.sign[AXIS_Z] = 1;
  87. c_map.map[AXIS_X] = AXIS_X;
  88. c_map.map[AXIS_Y] = AXIS_Y;
  89. c_map.map[AXIS_Z] = AXIS_Z;
  90. }
  91. else if(layout == 3)
  92. {
  93. c_map.sign[AXIS_X] = 1;
  94. c_map.sign[AXIS_Y] = -1;
  95. c_map.sign[AXIS_Z] = 1;
  96. c_map.map[AXIS_X] = AXIS_Y;
  97. c_map.map[AXIS_Y] = AXIS_X;
  98. c_map.map[AXIS_Z] = AXIS_Z;
  99. }
  100. else if(layout == 4)
  101. {
  102. c_map.sign[AXIS_X] = -1;
  103. c_map.sign[AXIS_Y] = 1;
  104. c_map.sign[AXIS_Z] = -1;
  105. c_map.map[AXIS_X] = AXIS_X;
  106. c_map.map[AXIS_Y] = AXIS_Y;
  107. c_map.map[AXIS_Z] = AXIS_Z;
  108. }
  109. else if(layout == 5)
  110. {
  111. c_map.sign[AXIS_X] = 1;
  112. c_map.sign[AXIS_Y] = 1;
  113. c_map.sign[AXIS_Z] = -1;
  114. c_map.map[AXIS_X] = AXIS_Y;
  115. c_map.map[AXIS_Y] = AXIS_X;
  116. c_map.map[AXIS_Z] = AXIS_Z;
  117. }
  118. else if(layout == 6)
  119. {
  120. c_map.sign[AXIS_X] = 1;
  121. c_map.sign[AXIS_Y] = -1;
  122. c_map.sign[AXIS_Z] = -1;
  123. c_map.map[AXIS_X] = AXIS_X;
  124. c_map.map[AXIS_Y] = AXIS_Y;
  125. c_map.map[AXIS_Z] = AXIS_Z;
  126. }
  127. else if(layout == 7)
  128. {
  129. c_map.sign[AXIS_X] = -1;
  130. c_map.sign[AXIS_Y] = -1;
  131. c_map.sign[AXIS_Z] = -1;
  132. c_map.map[AXIS_X] = AXIS_Y;
  133. c_map.map[AXIS_Y] = AXIS_X;
  134. c_map.map[AXIS_Z] = AXIS_Z;
  135. }
  136. else
  137. {
  138. c_map.sign[AXIS_X] = 1;
  139. c_map.sign[AXIS_Y] = 1;
  140. c_map.sign[AXIS_Z] = 1;
  141. c_map.map[AXIS_X] = AXIS_X;
  142. c_map.map[AXIS_Y] = AXIS_Y;
  143. c_map.map[AXIS_Z] = AXIS_Z;
  144. }
  145. }
  146. static int qmc6310_get_chipid(void)
  147. {
  148. int ret = 0;
  149. ret = qmc6310_read_block(QMC6310_CHIP_ID_REG, &chipid , 1);
  150. if(ret == 0){
  151. SEGGER_RTT_printf(0,"change_iic_addr = 0x3c\n");
  152. }
  153. else
  154. {
  155. SEGGER_RTT_printf(0,"QMC6308_get_chipid chipid = 0x%x,i2c_addr = 0x2c\n", chipid);
  156. return 1;
  157. }
  158. ret = qmc6310_read_block(QMC6310_CHIP_ID_REG, &chipid , 1);
  159. if(ret == 0){
  160. SEGGER_RTT_printf(0,"change_iic_addr = 0x1c\n");
  161. }
  162. else
  163. {
  164. SEGGER_RTT_printf(0,"QMC6310_get_chipid chipid = 0x%x,i2c_addr = 0x3c\n", chipid);
  165. return 1;
  166. }
  167. ret = qmc6310_read_block(QMC6310_CHIP_ID_REG, &chipid , 1);
  168. if(ret == 0){
  169. SEGGER_RTT_printf(0,"Get_Chip_ID_Failed!\n");
  170. return 0;
  171. }
  172. else
  173. {
  174. SEGGER_RTT_printf(0,"QMC6310_get_chipid chipid = 0x%x,i2c_addr = 0x1c\n", chipid);
  175. return 1;
  176. }
  177. }
  178. uint8_t qmc6310_back_read_mag_xyz(int16_t *data)
  179. {
  180. int res;
  181. unsigned char mag_data[6];
  182. int16_t hw_d[3] = {0};
  183. int16_t raw_c[3];
  184. // int t1 = 0;
  185. // unsigned char rdy = 0;
  186. /* Check status register for data availability */
  187. // while(!(rdy & 0x01) && (t1 < 5))
  188. // {
  189. // rdy = QMC6310_STATUS_REG;
  190. // res = qmc6310_read_block(QMC6310_STATUS_REG, &rdy, 1);
  191. // t1++;
  192. // }
  193. mag_data[0] = QMC6310_DATA_OUT_X_LSB_REG;
  194. res = qmc6310_read_block(QMC6310_DATA_OUT_X_LSB_REG, mag_data, 6);
  195. if(res == 0)
  196. {
  197. return 0;
  198. }
  199. hw_d[0] = (int16_t)(((mag_data[1]) << 8) | mag_data[0]);
  200. hw_d[1] = (int16_t)(((mag_data[3]) << 8) | mag_data[2]);
  201. hw_d[2] = (int16_t)(((mag_data[5]) << 8) | mag_data[4]);
  202. //Unit:mG 1G = 100uT = 1000mG
  203. //SEGGER_RTT_printf(0,"Hx=%d, Hy=%d, Hz=%d\n",hw_d[0],hw_d[1],hw_d[2]);
  204. raw_c[AXIS_X] = (int16_t)(c_map.sign[AXIS_X]*hw_d[c_map.map[AXIS_X]]);
  205. raw_c[AXIS_Y] = (int16_t)(c_map.sign[AXIS_Y]*hw_d[c_map.map[AXIS_Y]]);
  206. raw_c[AXIS_Z] = (int16_t)(c_map.sign[AXIS_Z]*hw_d[c_map.map[AXIS_Z]]);
  207. // #if DEBUG_IMU
  208. // SEGGER_RTT_printf(0,"back raw_c[0]=%d,raw_c[1]=%d,raw_c[2]=%d\n",raw_c[0],raw_c[1],raw_c[2]);
  209. // #endif
  210. data[0] = raw_c[0];
  211. data[1] = raw_c[1];
  212. data[2] = raw_c[2];
  213. return res;
  214. }
  215. ///* Set the sensor mode */
  216. //static int qmc6310_set_mode(unsigned char mode)
  217. //{
  218. // int err = 0;
  219. // unsigned char ctrl1_value = 0;
  220. //
  221. // err = qmc6310_read_block(QMC6310_CTL_REG_ONE, &ctrl1_value, 1);
  222. // ctrl1_value = (ctrl1_value&(~0x03))|mode;
  223. // SEGGER_RTT_printf(0,"QMC6310_set_mode, 0x0A = [%02x]->[%02x] \r\n", QMC6310_CTL_REG_ONE,ctrl1_value);
  224. // err = qmc6310_write_reg(QMC6310_CTL_REG_ONE, ctrl1_value);
  225. // return err;
  226. //}
  227. //static int qmc6310_set_output_data_rate(unsigned char rate){
  228. //
  229. // int err = 0;
  230. // unsigned char ctrl1_value = 0;
  231. //
  232. // err = qmc6310_read_block(QMC6310_CTL_REG_ONE, &ctrl1_value, 1);
  233. // ctrl1_value = (ctrl1_value& (~0xE8)) | (rate << 5);
  234. // SEGGER_RTT_printf(0,"QMC6310_set_output_data_rate, 0x0A = [%02x]->[%02x] \r\n", QMC6310_CTL_REG_ONE,ctrl1_value);
  235. // err = qmc6310_write_reg(QMC6310_CTL_REG_ONE, ctrl1_value);
  236. // return err;
  237. //}
  238. static int qmc6310_reset(void)
  239. {
  240. uint16_t timeout = 20;
  241. unsigned char ctrl_value = 0xFF;
  242. do{
  243. qmc6310_write_reg(0x0b, 0x80); //reset
  244. qmc6310_read_block(0x0b, &ctrl_value, 1);
  245. timeout--;
  246. // SEGGER_RTT_printf(0,"---->back qmc6310_reset timeout:%d 0x80==0x%x\r\n",timeout,ctrl_value);
  247. }while(timeout !=0 && ctrl_value != 0x80);
  248. if(timeout == 0 || ctrl_value != 0x80) return 0;
  249. timeout = 20;
  250. ctrl_value = 0xFF;
  251. do{
  252. qmc6310_write_reg(0x0b, 0x00); //取消reset
  253. qmc6310_read_block(0x0b, &ctrl_value, 1);
  254. timeout--;
  255. // SEGGER_RTT_printf(0,"---->back qmc6310_unreset timeout:%d 0x00==0x%x\r\n",timeout,ctrl_value);
  256. }while(timeout !=0 && ctrl_value != 0x00);
  257. if(timeout == 0 || ctrl_value != 0x00) return 0;
  258. return 1;
  259. }
  260. static int qmc6310_back_CheckIsConfigure(uint8_t hz)
  261. {
  262. unsigned char ctrl_value = 0xFF;
  263. uint16_t timeout = 20;
  264. // do{
  265. // qmc6310_write_reg(0x0d, 0x40);
  266. // qmc6310_read_block(0x0d, &ctrl_value, 1);
  267. // timeout--;
  268. //// SEGGER_RTT_printf(0,"---->back timeout=%d 0x40==0x%x \r\n", timeout, ctrl_value);
  269. // }while(timeout != 0 && ctrl_value != 0x40);
  270. // if(timeout == 0 || ctrl_value != 0x40) return 0;
  271. timeout = 20;
  272. ctrl_value = 0xFF;
  273. do{
  274. qmc6310_write_reg(0x29, 0x06);
  275. qmc6310_read_block(0x29, &ctrl_value, 1);
  276. timeout--;
  277. // SEGGER_RTT_printf(0,"---->back timeout=%d 0x06==0x%x \r\n", timeout, ctrl_value);
  278. }while(timeout != 0 && ctrl_value != 0x06);
  279. if(timeout == 0 || ctrl_value != 0x06) return 0;
  280. timeout = 20;
  281. ctrl_value = 0xFF;
  282. do{
  283. qmc6310_write_reg(0x0b, 0x00);
  284. qmc6310_read_block(0x0b, &ctrl_value, 1);
  285. timeout--;
  286. // SEGGER_RTT_printf(0,"---->back timeout=%d 0x00==0x%x \r\n", timeout, ctrl_value);
  287. }while(timeout != 0 && ctrl_value != 0x00);
  288. if(timeout == 0 || ctrl_value != 0x00) return 0;
  289. timeout = 20;
  290. ctrl_value = 0xFF;
  291. do{
  292. qmc6310_write_reg(0x0a, hz);
  293. qmc6310_read_block(0x0a, &ctrl_value, 1);
  294. timeout--;
  295. // SEGGER_RTT_printf(0,"---->back timeout=%d 0x%x==0x%x \r\n", timeout, hz,ctrl_value);
  296. }while(timeout != 0 && ctrl_value != hz);
  297. if(timeout == 0 || ctrl_value != hz) return 0;
  298. return 1;
  299. }
  300. //int qmc6310_back_ReConfigure(uint8_t hz)
  301. //{
  302. // if(!qmc6310_write_reg(0x29, 0x06))return 0;//(Define the sign for X Y and Z axis)
  303. // if(!qmc6310_write_reg(0x0d, 0x40))return 0;
  304. // if(!qmc6310_write_reg(0x0b, 0x00))return 0; //30 GS
  305. // if(!qmc6310_write_reg(0x0a, hz))return 0;
  306. //
  307. // return 1;
  308. //}
  309. int qmc6310_back_Suspend(void)
  310. {
  311. return qmc6310_reset();
  312. }
  313. int qmc6310_back_Init(uint8_t hz)
  314. {
  315. nrf_gpio_cfg(
  316. PIN_BACK_SENSE_POWER,
  317. NRF_GPIO_PIN_DIR_OUTPUT,
  318. NRF_GPIO_PIN_INPUT_DISCONNECT,
  319. NRF_GPIO_PIN_NOPULL,
  320. NRF_GPIO_PIN_H0H1,
  321. NRF_GPIO_PIN_NOSENSE);
  322. //供电
  323. nrf_gpio_pin_write(PIN_BACK_SENSE_POWER,0);
  324. nrf_delay_ms(20);
  325. nrf_gpio_pin_write(PIN_BACK_SENSE_POWER,1);
  326. nrf_delay_ms(20);
  327. IIC_BACK_Init();
  328. int ret = 0;
  329. ret = qmc6310_get_chipid();
  330. if(ret==0)
  331. {
  332. return 0;
  333. }
  334. ret = qmc6310_reset();
  335. if(ret==0)
  336. {
  337. return 0;
  338. }
  339. qmc6310_set_layout(0);
  340. return qmc6310_back_CheckIsConfigure(hz);
  341. }