Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
lis3mdl.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
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/lis3mdl.h"
28 
29 #define LIS3MDL_ENABLE_AUTO_INC (0x1<<7)
30 
31 #define LIS3MDL_STATUS_ZYXOR 0b10000000
32 #define LIS3MDL_STATUS_ZOR 0b01000000
33 #define LIS3MDL_STATUS_YOR 0b00100000
34 #define LIS3MDL_STATUS_XOR 0b00010000
35 #define LIS3MDL_STATUS_ZYXDA 0b00001000
36 #define LIS3MDL_STATUS_ZDA 0b00000100
37 #define LIS3MDL_STATUS_YDA 0b00000010
38 #define LIS3MDL_STATUS_XDA 0b00000001
39 
40 #define LIS3MDL_DEVICE_ID 0b00111101
41 
42 #define LIS3MDL_REG_WHO_AM_I 0x0F
43 #define LIS3MDL_REG_CTL_1 0x20
44 #define LIS3MDL_REG_CTL_2 0x21
45 #define LIS3MDL_REG_CTL_3 0x22
46 #define LIS3MDL_REG_CTL_4 0x23
47 #define LIS3MDL_REG_STATUS 0x27
48 #define LIS3MDL_REG_OUT_X_L 0x28
49 #define LIS3MDL_REG_OUT_X_H 0x29
50 #define LIS3MDL_REG_OUT_Y_L 0x2A
51 #define LIS3MDL_REG_OUT_Y_H 0x2B
52 #define LIS3MDL_REG_OUT_Z_L 0x2C
53 #define LIS3MDL_REG_OUT_Z_H 0x2D
54 #define LIS3MDL_REG_OUT_TEMP_L 0x2E
55 #define LIS3MDL_REG_OUT_TEMP_H 0x2F
56 
57 #define LIS3MDL_REG_CTL_1_TEMP_EN 0b10000000
58 #define LIS3MDL_REG_CTL_2_RESET 0b00000100
59 
60 void lis3mdl_init(struct Lis3mdl *mag, struct i2c_periph *i2c_p, uint8_t addr, uint8_t data_rate, uint8_t scale, uint8_t mode, uint8_t perf)
61 {
62  /* set i2c_peripheral */
63  mag->i2c_p = i2c_p;
64  /* set i2c address */
65  mag->i2c_trans.slave_addr = addr;
67 
68  /* prepare config request */
70  mag->i2c_trans.buf[1] = (data_rate << 2) | (perf << 5);
71  mag->i2c_trans.buf[2] = (scale << 5);
72  mag->i2c_trans.buf[3] = mode;
73  mag->i2c_trans.buf[4] = (perf << 2);
74  mag->i2c_trans.buf[5] = 0;
75 
76  mag->initialized = false;
78  mag->data_available = false;
79 }
80 
81 // Configure
82 void lis3mdl_configure(struct Lis3mdl *mag)
83 {
84  // Only configure when not busy
86  && mag->i2c_trans.status != I2CTransDone) {
87  return;
88  }
89 
90  // Only when succesfull continue with next
91  if (mag->i2c_trans.status == I2CTransSuccess) {
92  mag->status++;
93  }
94 
96  switch (mag->status) {
97 
99  // send config request
100  i2c_transmit(mag->i2c_p, &(mag->i2c_trans), mag->i2c_trans.slave_addr, 6);
101  break;
102 
103  case LIS3MDL_CONF_REG:
105  mag->initialized = true;
106  break;
107 
108  default:
109  break;
110  }
111 }
112 
113 void lis3mdl_read(struct Lis3mdl *mag)
114 {
115  if (mag->status != LIS3MDL_STATUS_IDLE) {
116  return;
117  }
118 
120  i2c_transceive(mag->i2c_p, &(mag->i2c_trans), mag->i2c_trans.slave_addr, 1, 7);
122 }
123 
124 // Get raw value
125 #define Int16FromBuf(_buf,_idx) ((int16_t)(_buf[_idx] | (_buf[_idx+1] << 8)))
126 
127 void lis3mdl_event(struct Lis3mdl *mag)
128 {
129  if (!mag->initialized) {
130  return;
131  }
132 
133  switch (mag->status) {
134 
135  case LIS3MDL_STATUS_MEAS:
136  if (mag->i2c_trans.status == I2CTransSuccess) {
137  // Read status and error bytes
138  const bool dr = mag->i2c_trans.buf[0] & LIS3MDL_STATUS_ZYXDA; // data ready
139  if (dr > 0) {
140  // Copy the data
141  mag->data.vect.x = Int16FromBuf(mag->i2c_trans.buf, 1);
142  mag->data.vect.y = Int16FromBuf(mag->i2c_trans.buf, 3);
143  mag->data.vect.z = Int16FromBuf(mag->i2c_trans.buf, 5);
144  mag->data_available = true;
145  }
146  // End reading, back to idle
148  }
149  break;
150 
151  default:
153  // Goto idle
156  }
157  break;
158  }
159 }
160 
i2c_transaction::buf
volatile uint8_t buf[I2C_BUF_LEN]
Transaction buffer With I2C_BUF_LEN number of bytes.
Definition: i2c.h:122
LIS3MDL_CONF_REG
@ LIS3MDL_CONF_REG
Definition: lis3mdl.h:63
Lis3mdl::initialized
bool initialized
config done flag
Definition: lis3mdl.h:72
LIS3MDL_CONF_UNINIT
@ LIS3MDL_CONF_UNINIT
Definition: lis3mdl.h:62
scale
static const float scale[]
Definition: dw1000_arduino.c:200
lis3mdl_read
void lis3mdl_read(struct Lis3mdl *mag)
Definition: lis3mdl.c:113
I2CTransFailed
@ I2CTransFailed
transaction failed
Definition: i2c.h:58
LIS3MDL_ENABLE_AUTO_INC
#define LIS3MDL_ENABLE_AUTO_INC
Definition: lis3mdl.c:29
Lis3mdl::i2c_p
struct i2c_periph * i2c_p
peripheral used for communcation
Definition: lis3mdl.h:70
I2CTransSuccess
@ I2CTransSuccess
transaction successfully finished by I2C driver
Definition: i2c.h:57
i2c_transmit
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
i2c_transceive
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
Lis3mdl::data
union Lis3mdl::@323 data
uint8_t
unsigned char uint8_t
Definition: types.h:14
i2c_transaction::status
enum I2CTransactionStatus status
Transaction status.
Definition: i2c.h:126
i2c_transaction::slave_addr
uint8_t slave_addr
Slave address.
Definition: i2c.h:104
Lis3mdl
Definition: lis3mdl.h:69
LIS3MDL_STATUS_IDLE
@ LIS3MDL_STATUS_IDLE
Definition: lis3mdl.h:64
lis3mdl_configure
void lis3mdl_configure(struct Lis3mdl *mag)
Definition: lis3mdl.c:82
LIS3MDL_REG_STATUS
#define LIS3MDL_REG_STATUS
Definition: lis3mdl.c:47
Lis3mdl::data_available
volatile bool data_available
data ready flag
Definition: lis3mdl.h:74
LIS3MDL_STATUS_MEAS
@ LIS3MDL_STATUS_MEAS
Definition: lis3mdl.h:65
Lis3mdl::status
enum Lis3mdlStatus status
init status
Definition: lis3mdl.h:73
I2CTransDone
@ I2CTransDone
transaction set to done by user level
Definition: i2c.h:59
LIS3MDL_STATUS_ZYXDA
#define LIS3MDL_STATUS_ZYXDA
Definition: lis3mdl.c:35
lis3mdl.h
lis3mdl_init
void lis3mdl_init(struct Lis3mdl *mag, struct i2c_periph *i2c_p, uint8_t addr, uint8_t data_rate, uint8_t scale, uint8_t mode, uint8_t perf)
Definition: lis3mdl.c:60
mode
static uint8_t mode
mode holds the current sonar mode mode = 0 used at high altitude, uses 16 wave patterns mode = 1 used...
Definition: sonar_bebop.c:69
lis3mdl_event
void lis3mdl_event(struct Lis3mdl *mag)
Definition: lis3mdl.c:127
Lis3mdl::i2c_trans
struct i2c_transaction i2c_trans
i2c transaction
Definition: lis3mdl.h:71
LIS3MDL_REG_CTL_1
#define LIS3MDL_REG_CTL_1
Definition: lis3mdl.c:43
i2c_periph
Definition: i2c.h:144
Int16FromBuf
#define Int16FromBuf(_buf, _idx)
Definition: lis3mdl.c:125