123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340 |
-
- #include "drv_qmc6310.h"
- #include <math.h>
- #include "lsm6ds3tr_c.h"
- #include "nrf_delay.h"
- static uint8_t chipid = 0;
- static QMC6310_map c_map;
- static int qmc6310_read_block(uint8_t addr, uint8_t *data, uint8_t len)
- {
- if(lsm6ds3tr_c_switch_read_sensor_hub_cx(LSM_DEV_CENTER, 0x1C, addr, data,len) == 0)
- {
- return 1;
- }
- return 0;
- }
- static int qmc6310_write_reg(uint8_t addr, uint8_t data)
- {
- if(lsm6ds3_c_switch_write_sensor_hub_cx(LSM_DEV_CENTER, 0x1C, addr, &data) == 0)
- {
- return true;
- }
- return false;
- }
- static void qmc6310_set_layout(int layout)
- {
- if(layout == 0)
- {
- c_map.sign[AXIS_X] = 1;
- c_map.sign[AXIS_Y] = 1;
- c_map.sign[AXIS_Z] = 1;
- c_map.map[AXIS_X] = AXIS_X;
- c_map.map[AXIS_Y] = AXIS_Y;
- c_map.map[AXIS_Z] = AXIS_Z;
- }
- else if(layout == 1)
- {
- c_map.sign[AXIS_X] = -1;
- c_map.sign[AXIS_Y] = 1;
- c_map.sign[AXIS_Z] = 1;
- c_map.map[AXIS_X] = AXIS_Y;
- c_map.map[AXIS_Y] = AXIS_X;
- c_map.map[AXIS_Z] = AXIS_Z;
- }
- else if(layout == 2)
- {
- c_map.sign[AXIS_X] = -1;
- c_map.sign[AXIS_Y] = -1;
- c_map.sign[AXIS_Z] = 1;
- c_map.map[AXIS_X] = AXIS_X;
- c_map.map[AXIS_Y] = AXIS_Y;
- c_map.map[AXIS_Z] = AXIS_Z;
- }
- else if(layout == 3)
- {
- c_map.sign[AXIS_X] = 1;
- c_map.sign[AXIS_Y] = -1;
- c_map.sign[AXIS_Z] = 1;
- c_map.map[AXIS_X] = AXIS_Y;
- c_map.map[AXIS_Y] = AXIS_X;
- c_map.map[AXIS_Z] = AXIS_Z;
- }
- else if(layout == 4)
- {
- c_map.sign[AXIS_X] = -1;
- c_map.sign[AXIS_Y] = 1;
- c_map.sign[AXIS_Z] = -1;
- c_map.map[AXIS_X] = AXIS_X;
- c_map.map[AXIS_Y] = AXIS_Y;
- c_map.map[AXIS_Z] = AXIS_Z;
- }
- else if(layout == 5)
- {
- c_map.sign[AXIS_X] = 1;
- c_map.sign[AXIS_Y] = 1;
- c_map.sign[AXIS_Z] = -1;
- c_map.map[AXIS_X] = AXIS_Y;
- c_map.map[AXIS_Y] = AXIS_X;
- c_map.map[AXIS_Z] = AXIS_Z;
- }
- else if(layout == 6)
- {
- c_map.sign[AXIS_X] = 1;
- c_map.sign[AXIS_Y] = -1;
- c_map.sign[AXIS_Z] = -1;
- c_map.map[AXIS_X] = AXIS_X;
- c_map.map[AXIS_Y] = AXIS_Y;
- c_map.map[AXIS_Z] = AXIS_Z;
- }
- else if(layout == 7)
- {
- c_map.sign[AXIS_X] = -1;
- c_map.sign[AXIS_Y] = -1;
- c_map.sign[AXIS_Z] = -1;
- c_map.map[AXIS_X] = AXIS_Y;
- c_map.map[AXIS_Y] = AXIS_X;
- c_map.map[AXIS_Z] = AXIS_Z;
- }
- else
- {
- c_map.sign[AXIS_X] = 1;
- c_map.sign[AXIS_Y] = 1;
- c_map.sign[AXIS_Z] = 1;
- c_map.map[AXIS_X] = AXIS_X;
- c_map.map[AXIS_Y] = AXIS_Y;
- c_map.map[AXIS_Z] = AXIS_Z;
- }
- }
- static int qmc6310_get_chipid(void)
- {
- int ret = 0;
- ret = qmc6310_read_block(QMC6310_CHIP_ID_REG, &chipid , 1);
- if(ret == 0){
- DEBUG_LOG("change_iic_addr = 0x3c\n");
- }
- else
- {
- DEBUG_LOG("QMC6308_get_chipid chipid = 0x%x,i2c_addr = 0x2c\n", chipid);
- return 1;
- }
-
- ret = qmc6310_read_block(QMC6310_CHIP_ID_REG, &chipid , 1);
- if(ret == 0){
- DEBUG_LOG("change_iic_addr = 0x1c\n");
- }
- else
- {
- DEBUG_LOG("QMC6310_get_chipid chipid = 0x%x,i2c_addr = 0x3c\n", chipid);
- return 1;
- }
-
- ret = qmc6310_read_block(QMC6310_CHIP_ID_REG, &chipid , 1);
- if(ret == 0){
- DEBUG_LOG("Get_Chip_ID_Failed!\n");
- return 0;
- }
- else
- {
- DEBUG_LOG("QMC6310_get_chipid chipid = 0x%x,i2c_addr = 0x1c\n", chipid);
- return 1;
- }
- }
- uint8_t qmc6310_center_read_mag_xyz(int16_t *data)
- {
- int res;
- unsigned char mag_data[6];
- int16_t hw_d[3] = {0};
- int16_t raw_c[3];
- int t1 = 0;
- unsigned char rdy = 0;
-
- while(!(rdy & 0x01) && (t1 < 5))
- {
- rdy = QMC6310_STATUS_REG;
- res = qmc6310_read_block(QMC6310_STATUS_REG, &rdy, 1);
- t1++;
- }
- mag_data[0] = QMC6310_DATA_OUT_X_LSB_REG;
- res = qmc6310_read_block(QMC6310_DATA_OUT_X_LSB_REG, mag_data, 6);
- if(res == 0)
- {
- return 0;
- }
- hw_d[0] = (int16_t)(((mag_data[1]) << 8) | mag_data[0]);
- hw_d[1] = (int16_t)(((mag_data[3]) << 8) | mag_data[2]);
- hw_d[2] = (int16_t)(((mag_data[5]) << 8) | mag_data[4]);
-
-
- raw_c[AXIS_X] = (int16_t)(c_map.sign[AXIS_X]*hw_d[c_map.map[AXIS_X]]);
- raw_c[AXIS_Y] = (int16_t)(c_map.sign[AXIS_Y]*hw_d[c_map.map[AXIS_Y]]);
- raw_c[AXIS_Z] = (int16_t)(c_map.sign[AXIS_Z]*hw_d[c_map.map[AXIS_Z]]);
-
- #if DEBUG_IMU
- DEBUG_LOG("center raw_c[0]=%d,raw_c[1]=%d,raw_c[2]=%d\n",raw_c[0],raw_c[1],raw_c[2]);
- #endif
-
- data[0] = raw_c[0];
- data[1] = raw_c[1];
- data[2] = raw_c[2];
- return res;
- }
- static int qmc6310_reset(void)
- {
- uint16_t timeout = 200;
- unsigned char ctrl_value = 0;
- do{
- qmc6310_write_reg(0x0b, 0x80);
- nrf_delay_ms(100);
- qmc6310_read_block(0x0b, &ctrl_value, 1);
- timeout--;
- DEBUG_LOG("---->center qmc6310_reset timeout:%d ctrl_value:0x%x\r\n",timeout,ctrl_value);
- }while(timeout !=0 && ctrl_value != 0x80);
- if(timeout !=0 && ctrl_value != 0x80)DEBUG_LOG("---->center qmc6310_reset timeout:%d\r\n",timeout);
- qmc6310_write_reg(0x0b, 0x00);
- return 1;
- }
- static int qmc6310_center_CheckIsConfigure(uint8_t hz)
- {
- unsigned char ctrl_value;
-
- qmc6310_read_block(0x0d, &ctrl_value, 1);
- nrf_delay_ms(3);
- if(ctrl_value != 0x40) return 0;
-
- qmc6310_read_block(QMC6310_CTL_REG_TWO, &ctrl_value, 1);
- nrf_delay_ms(3);
- if(ctrl_value != 0x00) return 0;
-
- qmc6310_read_block(QMC6310_CTL_REG_ONE, &ctrl_value, 1);
- nrf_delay_ms(3);
- if(ctrl_value != hz) return 0;
-
- return 1;
- }
- int qmc6310_center_ReConfigure(uint8_t hz)
- {
- if(!qmc6310_write_reg(0x29, 0x06))return 0;
- if(!qmc6310_write_reg(0x0d, 0x40))return 0;
- if(!qmc6310_write_reg(0x0b, 0x00))return 0;
- if(!qmc6310_write_reg(0x0a, hz))return 0;
-
- return 1;
- }
- int qmc6310_center_Suspend(void)
- {
- return qmc6310_reset();
- }
- QMC6310_map qmc6310_center_GetMap(void)
- {
- return c_map;
- }
- int qmc6310_center_Init(uint8_t hz)
- {
- qmc6310_reset();
-
- int ret = 0;
- ret = qmc6310_get_chipid();
- if(ret==0)
- {
- return 0;
- }
- qmc6310_set_layout(0);
-
- qmc6310_reset();
-
- qmc6310_center_ReConfigure(hz);
-
- return qmc6310_center_CheckIsConfigure(hz);
- }
|