123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379 |
-
- #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_FRONT, 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_FRONT, 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_front_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("front 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 = 20;
- unsigned char ctrl_value = 0xFF;
- do{
- qmc6310_write_reg(0x0b, 0x80);
- qmc6310_read_block(0x0b, &ctrl_value, 1);
- timeout--;
- }while(timeout !=0 && ctrl_value != 0x80);
- if(timeout == 0 || ctrl_value != 0x80) return 0;
-
- timeout = 20;
- ctrl_value = 0xFF;
- do{
- qmc6310_write_reg(0x0b, 0x00);
- qmc6310_read_block(0x0b, &ctrl_value, 1);
- timeout--;
- }while(timeout !=0 && ctrl_value != 0x00);
- if(timeout == 0 || ctrl_value != 0x00) return 0;
-
-
- return 1;
- }
- static int qmc6310_front_CheckIsConfigure(uint8_t hz)
- {
- unsigned char ctrl_value = 0xFF;
- uint16_t timeout = 20;
-
-
- timeout = 20;
- ctrl_value = 0xFF;
-
- do{
- qmc6310_write_reg(0x29, 0x06);
- qmc6310_read_block(0x29, &ctrl_value, 1);
- timeout--;
- }while(timeout != 0 && ctrl_value != 0x06);
- if(timeout == 0 || ctrl_value != 0x06) return 0;
-
-
- timeout = 20;
- ctrl_value = 0xFF;
-
- do{
- qmc6310_write_reg(0x0b, 0x00);
- qmc6310_read_block(0x0b, &ctrl_value, 1);
- timeout--;
- }while(timeout != 0 && ctrl_value != 0x00);
- if(timeout == 0 || ctrl_value != 0x00) return 0;
-
- timeout = 20;
- ctrl_value = 0xFF;
-
- do{
- qmc6310_write_reg(0x0a, hz);
- qmc6310_read_block(0x0a, &ctrl_value, 1);
- timeout--;
- }while(timeout != 0 && ctrl_value != hz);
- if(timeout == 0 || ctrl_value != hz) return 0;
-
-
- return 1;
- }
- int qmc6310_front_Suspend(void)
- {
- return qmc6310_reset();
- }
- QMC6310_map qmc6310_front_GetMap(void)
- {
- return c_map;
- }
- int qmc6310_front_Init(uint8_t hz)
- {
- int ret = 0;
- ret = qmc6310_get_chipid();
- if(ret==0)
- {
- return 0;
- }
-
- ret = qmc6310_reset();
- if(ret==0)
- {
- return 0;
- }
-
- qmc6310_set_layout(0);
-
- return qmc6310_front_CheckIsConfigure(hz);
- }
|