Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
lsm303dlhc.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2011 Gautier Hattenberger <gautier.hattenberger@enac.fr>
3  * 2013 Felix Ruess <felix.ruess@gmail.com>
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
29 #include "peripherals/lsm303dlhc.h"
30 #include "std.h"
31 
32 /* LSM303DLHC default conf */
33 #ifndef LSM303DLHC_DEFAULT_ODR
34 #define LSM303DLHC_DEFAULT_ODR 0x90 //90 //normal 1.344khz, low power 5.376khz
35 #endif
36 
37 #ifndef LSM303DLHC_DEFAULT_LP
38 #define LSM303DLHC_DEFAULT_LP 0x00 //low power disabled
39 #endif
40 
41 #ifndef LSM303DLHC_DEFAULT_FS
42 #define LSM303DLHC_DEFAULT_FS 0x00 // +- 2G
43 #endif
44 
45 #ifndef LSM303DLHC_DEFAULT_HR
46 #define LSM303DLHC_DEFAULT_HR 0x04 // high res enabled
47 #endif
48 
49 #ifndef LSM303DLHC_DEFAULT_DO
50 #define LSM303DLHC_DEFAULT_DO (0x6 << 2) // Data Output Rate (75Hz)
51 #endif
52 
53 #ifndef LSM303DLHC_DEFAULT_GN
54 #define LSM303DLHC_DEFAULT_GN (0x1 << 5) // Gain configuration (1 -> +- 1.3 Gauss)
55 #endif
56 
57 #ifndef LSM303DLHC_DEFAULT_MD
58 #define LSM303DLHC_DEFAULT_MD 0x00 // Continious conversion mode
59 #endif
60 
62 {
67 }
68 
70 {
74 }
75 
82 void lsm303dlhc_init(struct Lsm303dlhc *lsm, struct i2c_periph *i2c_p, uint8_t addr)
83 {
84  /* set i2c_peripheral */
85  lsm->i2c_p = i2c_p;
86  /* set i2c address */
87  lsm->i2c_trans.slave_addr = addr;
89  /* set default config options */
90  if (addr == LSM303DLHC_ACC_ADDR) {
93  } else {
96  }
97  lsm->initialized = FALSE;
98 }
99 
100 static void lsm303dlhc_i2c_tx_reg(struct Lsm303dlhc *lsm, uint8_t reg, uint8_t val)
101 {
102  lsm->i2c_trans.type = I2CTransTx;
103  lsm->i2c_trans.buf[0] = reg;
104  lsm->i2c_trans.buf[1] = val;
105  lsm->i2c_trans.len_r = 0;
106  lsm->i2c_trans.len_w = 2;
107  i2c_submit(lsm->i2c_p, &(lsm->i2c_trans));
108 }
109 
111 static void lsm303dlhc_send_config(struct Lsm303dlhc *lsm)
112 {
114  switch (lsm->init_status.acc) {
117  (lsm->config.acc.scale & LSM303DLHC_FS_MASK) |
118  (lsm->config.acc.hres & LSM303DLHC_DEFAULT_HR));
119  lsm->init_status.acc++;
120  break;
123  (lsm->config.acc.rate & LSM303DLHC_ODR_MASK) |
124  (lsm->config.acc.lp_mode & LSM303DLHC_LPen) |
126  lsm->init_status.acc++;
127  break;
130  lsm->init_status.acc++;
131  break;
132  case LSM_CONF_ACC_DONE:
133  lsm->initialized = TRUE;
135  lsm303dlhc_read(lsm);
136  break;
137  default:
138  break;
139  }
140  } else {
141  switch (lsm->init_status.mag) {
144  lsm->init_status.mag++;
145  break;
148  lsm->init_status.mag++;
149  break;
152  lsm->init_status.mag++;
153  break;
154  case LSM_CONF_MAG_DONE:
155  lsm->initialized = TRUE;
157  break;
158  default:
159  break;
160  }
161  }
162 }
163 
164 // Configure
166 {
168  if (lsm->init_status.acc == LSM_CONF_ACC_UNINIT) {
169  lsm->init_status.acc++;
170  if (lsm->i2c_trans.status == I2CTransSuccess || lsm->i2c_trans.status == I2CTransDone) {
172  }
173  }
174  } else {
175  if (lsm->init_status.mag == LSM_CONF_MAG_UNINIT) {
176  lsm->init_status.mag++;
177  if (lsm->i2c_trans.status == I2CTransSuccess || lsm->i2c_trans.status == I2CTransDone) {
179  }
180  }
181  }
182 }
183 
184 // Normal reading
185 void lsm303dlhc_read(struct Lsm303dlhc *lsm)
186 {
188  //if ((lsm->init_status.acc == LSM_CONF_ACC_CLR_INT_READ) && (lsm->i2c_trans.status == I2CTransDone)){
189  if (!(lsm->initialized) || (lsm->initialized && lsm->i2c_trans.status == I2CTransDone)){
190  lsm->i2c_trans.buf[0] = LSM303DLHC_REG_OUT_X_L_A | 0x80;
191  lsm->i2c_trans.type = I2CTransTxRx;
192  lsm->i2c_trans.len_r = 6;
193  lsm->i2c_trans.len_w = 1;
194  i2c_submit(lsm->i2c_p, &(lsm->i2c_trans));
195  }
196  }
197  else {
198  if (lsm->initialized && lsm->i2c_trans.status == I2CTransDone){
200  lsm->i2c_trans.type = I2CTransTxRx;
201  lsm->i2c_trans.len_r = 6;
202  lsm->i2c_trans.len_w = 1;
203  i2c_submit(lsm->i2c_p, &(lsm->i2c_trans));
204  }
205  }
206 }
207 
208 #define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx+1]<<8) | _buf[_idx]))
209 
210 void lsm303dlhc_event(struct Lsm303dlhc *lsm)
211 {
212  if (lsm->initialized) {
213  if (lsm->i2c_trans.status == I2CTransFailed) {
215  }
216  else if (lsm->i2c_trans.status == I2CTransSuccess) {
217  lsm->data.vect.x = Int16FromBuf(lsm->i2c_trans.buf,0);
218  lsm->data.vect.y = Int16FromBuf(lsm->i2c_trans.buf,2);
219  lsm->data.vect.z = Int16FromBuf(lsm->i2c_trans.buf,4);
220  lsm->data_available = TRUE;
222  }
223  else {
224  }
225  }
226  else
227  {
229  if (lsm->init_status.acc != LSM_CONF_ACC_UNINIT) { // Configuring but not yet initialized
230  if (lsm->i2c_trans.status == I2CTransSuccess || lsm->i2c_trans.status == I2CTransDone) {
233  }
234  if (lsm->i2c_trans.status == I2CTransFailed) {
235  lsm->init_status.acc--;
237  lsm303dlhc_send_config(lsm); // Retry config (TODO max retry)
238  }
239  }
240  }
241  else {
242  if (lsm->init_status.mag != LSM_CONF_MAG_UNINIT) { // Configuring but not yet initialized
243  if (lsm->i2c_trans.status == I2CTransSuccess || lsm->i2c_trans.status == I2CTransDone) {
246  }
247  if (lsm->i2c_trans.status == I2CTransFailed) {
248  lsm->init_status.mag--;
250  lsm303dlhc_send_config(lsm); // Retry config (TODO max retry)
251  }
252  }
253  }
254  }
255 }
bool_t initialized
config done flag
Definition: lsm303dlhc.h:73
static void lsm303dlhc_acc_set_default_config(struct Lsm303dlhcAccConfig *c)
Definition: lsm303dlhc.c:61
#define LSM303DLHC_REG_OUT_X_H_M
uint8_t rate
Data Output Rate Bits(6 -> 50Hz with HMC5843, 75Hz with HMC5883)
Definition: lsm303dlhc.h:40
void lsm303dlhc_init(struct Lsm303dlhc *lsm, struct i2c_periph *i2c_p, uint8_t addr)
Initialize Lsm303dlhc struct and set default config options.
Definition: lsm303dlhc.c:82
#define LSM303DLHC_FS_MASK
#define LSM303DLHC_LPen
#define LSM303DLHC_I1_DRDY1
uint8_t scale
full scale selection
Definition: lsm303dlhc.h:42
#define Int16FromBuf(_buf, _idx)
Definition: lsm303dlhc.c:208
#define LSM303DLHC_Yen
bool_t i2c_submit(struct i2c_periph *p, struct i2c_transaction *t)
Submit a I2C transaction.
Definition: i2c_arch.c:335
void lsm303dlhc_read(struct Lsm303dlhc *lsm)
Definition: lsm303dlhc.c:185
#define LSM303DLHC_REG_MR_REG_M
static void lsm303dlhc_i2c_tx_reg(struct Lsm303dlhc *lsm, uint8_t reg, uint8_t val)
Definition: lsm303dlhc.c:100
#define LSM303DLHC_ACC_ADDR
transaction successfully finished by I2C driver
Definition: i2c.h:57
#define LSM303DLHC_DEFAULT_ODR
Definition: lsm303dlhc.c:34
transmit and receive transaction
Definition: i2c.h:49
#define LSM303DLHC_MD_MASK
struct i2c_transaction i2c_trans
Definition: lsm303dlhc.h:72
#define LSM303DLHC_GN_MASK
uint8_t mode
Measurement mode.
Definition: lsm303dlhc.h:49
void lsm303dlhc_event(struct Lsm303dlhc *lsm)
Definition: lsm303dlhc.c:210
uint8_t slave_addr
Slave address.
Definition: i2c.h:104
#define LSM303DLHC_DEFAULT_MD
Definition: lsm303dlhc.c:58
#define FALSE
Definition: imu_chimu.h:141
#define LSM303DLHC_DEFAULT_FS
Definition: lsm303dlhc.c:42
#define LSM303DLHC_REG_CTRL_REG1_A
uint8_t hres
high resolution output mode
Definition: lsm303dlhc.h:43
#define LSM303DLHC_DEFAULT_LP
Definition: lsm303dlhc.c:38
#define LSM303DLHC_REG_CRB_REG_M
transaction set to done by user level
Definition: i2c.h:59
#define LSM303DLHC_DEFAULT_HR
Definition: lsm303dlhc.c:46
uint16_t val[TCOUPLE_NB]
void lsm303dlhc_start_configure(struct Lsm303dlhc *lsm)
Definition: lsm303dlhc.c:165
uint8_t gain
Gain configuration (1 -> +- 1 Gauss)
Definition: lsm303dlhc.h:48
#define LSM303DLHC_DO0_MASK
transaction failed
Definition: i2c.h:58
enum I2CTransactionStatus status
Transaction status.
Definition: i2c.h:126
uint8_t rate
Data Output Rate Bits(6 -> 50Hz with HMC5843, 75Hz with HMC5883)
Definition: lsm303dlhc.h:47
#define LSM303DLHC_REG_CTRL_REG4_A
struct i2c_periph * i2c_p
Definition: lsm303dlhc.h:71
#define TRUE
Definition: imu_chimu.h:144
uint8_t lp_mode
Low power mode.
Definition: lsm303dlhc.h:41
volatile uint8_t buf[I2C_BUF_LEN]
Transaction buffer With I2C_BUF_LEN number of bytes.
Definition: i2c.h:122
uint8_t len_w
Number of bytes to write/transmit.
Definition: i2c.h:116
uint16_t len_r
Number of bytes to read/receive.
Definition: i2c.h:110
#define LSM303DLHC_REG_OUT_X_L_A
unsigned char uint8_t
Definition: types.h:14
#define LSM303DLHC_Zen
static void lsm303dlhc_send_config(struct Lsm303dlhc *lsm)
Configuration function called once before normal use.
Definition: lsm303dlhc.c:111
volatile bool_t data_available
data ready flag
Definition: lsm303dlhc.h:78
#define LSM303DLHC_DEFAULT_DO
Definition: lsm303dlhc.c:50
union Lsm303dlhc::@34 init_status
union Lsm303dlhc::@36 config
I2C peripheral structure.
Definition: i2c.h:138
transmit only transaction
Definition: i2c.h:47
enum I2CTransactionType type
Transaction type.
Definition: i2c.h:98
static void lsm303dlhc_mag_set_default_config(struct Lsm303dlhcMagConfig *c)
Definition: lsm303dlhc.c:69
#define LSM303DLHC_REG_CTRL_REG3_A
#define LSM303DLHC_ODR_MASK
#define LSM303DLHC_REG_CRA_REG_M
#define LSM303DLHC_DEFAULT_GN
Definition: lsm303dlhc.c:54
union Lsm303dlhc::@35 data
enum Lsm303dlhcMagConfStatus mag
init status
Definition: lsm303dlhc.h:76
enum Lsm303dlhcAccConfStatus acc
init status
Definition: lsm303dlhc.h:75
struct Int16Vect3 vect
data vector in acc coordinate system
Definition: lsm303dlhc.h:80
#define LSM303DLHC_Xen
Driver for ST LSM303DLHC 3D accelerometer and magnetometer.