Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
qmc5883l.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2022 Paparazzi Team
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
27 #include "peripherals/qmc5883l.h"
28 
29 /* Registers Axis X,Y,Z */
30 #define QMC5883L_REG_DATXL 0x00
31 #define QMC5883L_REG_DATXM 0x01
32 #define QMC5883L_REG_DATYL 0x02
33 #define QMC5883L_REG_DATYM 0x03
34 #define QMC5883L_REG_DATZL 0x04
35 #define QMC5883L_REG_DATZM 0x05
36 
37 /* Register I2C bus transaction Status */
38 #define QMC5883L_REG_STATUS 0x06
39 
40 /* Registers Temperature, relative thus not so useful ATM, therefore not implemented in reading */
41 #define QMC5883L_REG_TEMPM 0x07
42 #define QMC5883L_REG_TEMPL 0x08
43 
44 /* Registers Config */
45 #define QMC5883L_REG_CONTROL_1 0x09 /* settings for MODE */
46 #define QMC5883L_REG_CONTROL_2 0x0A /* settings for INT_ENB */
47 #define QMC5883L_REG_RESET_PERIOD 0x0B
48 
49 #define QMC5883L_REG_IDC 0x0C /* OEM reserved */
50 #define QMC5883L_REG_IDD 0x0D /* OEM reserved */
51 
52 /* Options for CONTROL_1 */
53 #define QMC5883L_MODE_STBY 0x00
54 #define QMC5883L_MODE_CONT 0x01
55 
56 /* Options for scale RaNGe(RNG) Gauss */
57 #define QMC5883L_RNG_2G 0x00
58 #define QMC5883L_RNG_8G 0x10
59 
60 /* options for Over-Sample Ratio (OSR) */
61 #define QMC5883L_OSR_512 0x00 /* Use 512 if powerusage of chip is not an issue */
62 #define QMC5883L_OSR_256 0x40
63 #define QMC5883L_OSR_128 0x80
64 #define QMC5883L_OSR_64 0xC0
65 
66 void qmc5883l_init(struct Qmc5883l *mag, struct i2c_periph *i2c_p, uint8_t addr, uint8_t data_rate)
67 {
68  /* set i2c_peripheral */
69  mag->i2c_p = i2c_p;
70  /* set i2c address */
71  mag->i2c_trans.slave_addr = addr;
73  /* store data rate */
74  mag->data_rate = data_rate;
75  mag->initialized = false;
77  mag->data_available = false;
78 }
79 
80 void qmc5883l_configure(struct Qmc5883l *mag)
81 {
82  // Only configure when not busy
84  && mag->i2c_trans.status != I2CTransDone) {
85  return;
86  }
87 
88  // Only when successful continue with next
89  if (mag->i2c_trans.status == I2CTransSuccess) {
90  mag->status++; //Here the Enum Counter goes to the next one
91  }
92 
94  switch (mag->status) {
95 
97  /* prepare config request */
99  mag->i2c_trans.buf[0] = 0x01;
100  /* send config request, ask for i2c frame for set/reset period */
101  i2c_transmit(mag->i2c_p, &(mag->i2c_trans), mag->i2c_trans.slave_addr, 2);
102  break;
103 
106  mag->i2c_trans.buf[1] = (QMC5883L_MODE_CONT|QMC5883L_ODR_200|QMC5883L_RNG_8G|QMC5883L_OSR_512);//todo datarate from settings mag->data_rate;
107  i2c_transmit(mag->i2c_p, &(mag->i2c_trans), mag->i2c_trans.slave_addr, 2);
108  break;
109 
112  mag->i2c_trans.buf[1] = (QMC5883L_MODE_CONT|QMC5883L_ODR_200|QMC5883L_RNG_8G|QMC5883L_OSR_512);//todo datarate from settings //mag->data_rate;
113  i2c_transmit(mag->i2c_p, &(mag->i2c_trans), mag->i2c_trans.slave_addr, 2);
114  break;
115 
118  mag->initialized = true;
119  break;
120 
121  default:
122  break;
123  }
124 }
125 
126 void qmc5883l_read(struct Qmc5883l *mag)
127 {
128  if (mag->status != QMC5883L_STATUS_IDLE) {
129  return;
130  }
131 
132  /* get 3 x 2 bytes data = 6 Bytes and one status byte = 7 */
133  mag->i2c_trans.buf[0] = QMC5883L_REG_DATXL;
134  mag->i2c_trans.buf[1] = QMC5883L_REG_DATXM;
135  mag->i2c_trans.buf[2] = QMC5883L_REG_DATYL;
136  mag->i2c_trans.buf[3] = QMC5883L_REG_DATYM;
137  mag->i2c_trans.buf[4] = QMC5883L_REG_DATZL;
138  mag->i2c_trans.buf[5] = QMC5883L_REG_DATZM;
139 
140  /* Chip transaction status, not used ATM in case of driver mishap once can considder using it */
142  // Add some code if you experience reading issue
143  // DRDY = ((mag->i2c_trans.buf[6]) >> 0) & 1;
144  // OVL = ((mag->i2c_trans.buf[6]) >> 1) & 1;
145  // DOR = ((uint8_t)(mag->i2c_trans.buf[6]) >> 2) & 1;
146 
147  i2c_transceive(mag->i2c_p, &(mag->i2c_trans), mag->i2c_trans.slave_addr, 1, 7);
149 }
150 /* Convert and align raw values */
151 #define Int16FromBuf(_buf,_idx) ((int16_t)(_buf[_idx] | (_buf[_idx+1] << 8)))
152 
153 
154 void qmc5883l_event(struct Qmc5883l *mag)
155 {
156  if (!mag->initialized) {
157  return;
158  }
159 
160  switch (mag->status) {
161 
163  if (mag->i2c_trans.status == I2CTransSuccess) {
164  mag->data.vect.x = Int16FromBuf(mag->i2c_trans.buf, 0);
165  mag->data.vect.y = Int16FromBuf(mag->i2c_trans.buf, 2);
166  mag->data.vect.z = Int16FromBuf(mag->i2c_trans.buf, 4);
167 
168  /* only set available if measurements valid: -4096 if ADC under/overflow in sensor
169  The sensor sends out 12 bit wrapped in 16 bit, that sometimes gets corrupded */
170  if (mag->data.vect.x >= 4096 || mag->data.vect.y >= 4096 || mag->data.vect.z >= 4096) {
171  //mag->data.adc_overflow_cnt++;
172  mag->data_available = false;
173  }
174  else {
175  mag->data_available = true;
176  }
177  /* End of measure reading, go back to idle */
179  }
180  break;
181 
182  default:
184  /* Per default set to idle */
187  }
188  break;
189  }
190 }
191 
volatile uint8_t buf[I2C_BUF_LEN]
Transaction buffer With I2C_BUF_LEN number of bytes.
Definition: i2c.h:122
enum I2CTransactionStatus status
Transaction status.
Definition: i2c.h:126
uint8_t slave_addr
Slave address.
Definition: i2c.h:104
bool i2c_transmit(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len)
Submit a write only transaction.
Definition: i2c.c:324
bool i2c_transceive(struct i2c_periph *p, struct i2c_transaction *t, uint8_t s_addr, uint8_t len_w, uint16_t len_r)
Submit a write/read transaction.
Definition: i2c.c:344
@ I2CTransSuccess
transaction successfully finished by I2C driver
Definition: i2c.h:57
@ I2CTransFailed
transaction failed
Definition: i2c.h:58
@ I2CTransDone
transaction set to done by user level
Definition: i2c.h:59
void qmc5883l_event(struct Qmc5883l *mag)
Definition: qmc5883l.c:154
#define QMC5883L_REG_DATZM
Definition: qmc5883l.c:35
void qmc5883l_read(struct Qmc5883l *mag)
Definition: qmc5883l.c:126
#define QMC5883L_RNG_8G
Definition: qmc5883l.c:58
#define QMC5883L_MODE_CONT
Definition: qmc5883l.c:54
#define QMC5883L_REG_CONTROL_1
Definition: qmc5883l.c:45
#define QMC5883L_REG_STATUS
Definition: qmc5883l.c:38
#define QMC5883L_REG_DATZL
Definition: qmc5883l.c:34
#define QMC5883L_REG_DATXM
Definition: qmc5883l.c:31
#define QMC5883L_REG_DATYM
Definition: qmc5883l.c:33
#define QMC5883L_OSR_512
Definition: qmc5883l.c:61
#define QMC5883L_REG_DATXL
Definition: qmc5883l.c:30
#define QMC5883L_REG_DATYL
Definition: qmc5883l.c:32
void qmc5883l_configure(struct Qmc5883l *mag)
Definition: qmc5883l.c:80
#define QMC5883L_REG_RESET_PERIOD
Definition: qmc5883l.c:47
void qmc5883l_init(struct Qmc5883l *mag, struct i2c_periph *i2c_p, uint8_t addr, uint8_t data_rate)
Definition: qmc5883l.c:66
#define Int16FromBuf(_buf, _idx)
Definition: qmc5883l.c:151
QST QMC5883L 3-axis magnetometer driver interface (I2C).
volatile bool data_available
data ready flag
Definition: qmc5883l.h:66
@ QMC5883L_CONF_CCR_DONE
Definition: qmc5883l.h:53
@ QMC5883L_CONF_TMRC_DONE
Definition: qmc5883l.h:54
@ QMC5883L_STATUS_MEAS
Definition: qmc5883l.h:57
@ QMC5883L_STATUS_IDLE
Definition: qmc5883l.h:56
@ QMC5883L_CONF_UNINIT
Definition: qmc5883l.h:52
@ QMC5883L_CONF_CCM_DONE
Definition: qmc5883l.h:55
struct i2c_periph * i2c_p
peripheral used for communcation
Definition: qmc5883l.h:61
enum Qmc5883lStatus status
init status
Definition: qmc5883l.h:65
union Qmc5883l::@338 data
bool initialized
config done flag
Definition: qmc5883l.h:64
#define QMC5883L_ODR_200
Definition: qmc5883l.h:45
struct i2c_transaction i2c_trans
i2c transaction
Definition: qmc5883l.h:62
uint8_t data_rate
sensor data rate
Definition: qmc5883l.h:63
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98