Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
lsm303d_spi.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 
30 #include "std.h"
31 
32 void lsm303d_spi_init(struct Lsm303d_Spi *lsm, struct spi_periph *spi_p, uint8_t slave_idx,
33  enum Lsm303dTarget target)
34 {
35  /* set spi_peripheral */
36  lsm->spi_p = spi_p;
37 
38  /* set internal target mag/acc*/
39  lsm->target = target;
40 
41  /* configure spi transaction */
44  lsm->spi_trans.dss = SPIDss8bit;
46  lsm->spi_trans.cdiv = SPIDiv64;
47 
49  lsm->spi_trans.slave_idx = slave_idx;
50  lsm->spi_trans.output_length = 2;
51  lsm->spi_trans.input_length = 8;
52  // callback currently unused
53  lsm->spi_trans.before_cb = NULL;
54  lsm->spi_trans.after_cb = NULL;
55  lsm->spi_trans.input_buf = &(lsm->rx_buf[0]);
56  lsm->spi_trans.output_buf = &(lsm->tx_buf[0]);
57 
58  /* set inital status: Success or Done */
60 
61  /* set default LSM303D config options */
64 
65  lsm->initialized = FALSE;
68 }
69 
70 static void lsm303d_spi_tx_reg(struct Lsm303d_Spi *lsm, uint8_t reg, uint8_t val)
71 {
72  lsm->spi_trans.output_length = 2;
73  lsm->spi_trans.input_length = 0;
74  lsm->tx_buf[0] = reg;
75  lsm->tx_buf[1] = val;
76  spi_submit(lsm->spi_p, &(lsm->spi_trans));
77 }
78 
80 static void lsm303d_spi_send_config(struct Lsm303d_Spi *lsm)
81 {
82  switch (lsm->init_status) {
84  /* query device id */
85  lsm->spi_trans.output_length = 1;
86  lsm->spi_trans.input_length = 2;
87  /* set read bit then reg address */
88  lsm->tx_buf[0] = (1 << 7 | LSM303D_REG_WHO_AM_I);
89  if (spi_submit(lsm->spi_p, &(lsm->spi_trans))) {
90  if (lsm->rx_buf[1] == LSM303D_REG_WHO_I_AM) {
91  lsm->init_status++;
92  }
93  }
94  break;
99  lsm->init_status++;
100  break;
103  lsm->init_status++;
104  break;
107  lsm->init_status++;
108  break;
111  lsm->init_status++;
112  return;
113  break;
116  (lsm->conf.mag_rate & LSM303D_M_ODR_MASK));
117  lsm->init_status++;
118  return;
119  break;
122  (lsm->conf.mag_scale & LSM303D_MFS_MASK));
123  lsm->init_status++;
124  break;
127  lsm->init_status++;
128  break;
129  case LSM303D_CONF_DONE:
130  lsm->initialized = TRUE;
132  return;
133  break;
134  default:
135  break;
136  }
137 }
138 
139 // Configure
141 {
142  if (lsm->init_status == LSM303D_CONF_UNINIT) {
143  lsm->init_status++;
144  if (lsm->spi_trans.status == SPITransSuccess || lsm->spi_trans.status == SPITransDone) {
146  }
147  }
148 }
149 
150 // Normal reading
151 void lsm303d_spi_read(struct Lsm303d_Spi *lsm)
152 {
153  if (lsm->target == LSM303D_TARGET_ACC) {
154  if (!(lsm->initialized) || (lsm->initialized && lsm->spi_trans.status == SPITransDone)) {
155  lsm->spi_trans.output_length = 1;
156  lsm->spi_trans.input_length = 8;
157  /* set read bit and multiple byte bit, then address */
158  lsm->tx_buf[0] = LSM303D_REG_STATUS_A | 1 << 7 | 1 << 6;
159  spi_submit(lsm->spi_p, &(lsm->spi_trans));
160  }
161  } else {
162  if (lsm->initialized && lsm->spi_trans.status == SPITransDone) {
163  lsm->spi_trans.output_length = 1;
164  lsm->spi_trans.input_length = 8;
165  /* set read bit and multiple byte bit, then address */
166  lsm->tx_buf[0] = LSM303D_REG_STATUS_M | 1 << 7 | 1 << 6;
167  spi_submit(lsm->spi_p, &(lsm->spi_trans));
168  }
169  }
170 }
171 
172 #define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx+1]<<8) | _buf[_idx]))
173 
174 
175 
177 {
178  if (lsm->initialized) {
179  if (lsm->spi_trans.status == SPITransFailed) {
181  } else if (lsm->spi_trans.status == SPITransSuccess) {
182  if (lsm->target == LSM303D_TARGET_ACC) {
183  if (!(lsm->rx_buf[1] & LSM303D_ZYXADA)) {
185  return;
186  }
187  lsm->data_accel.vect.x = Int16FromBuf(lsm->rx_buf, 2);
188  lsm->data_accel.vect.y = Int16FromBuf(lsm->rx_buf, 4);
189  lsm->data_accel.vect.z = Int16FromBuf(lsm->rx_buf, 6);
190  lsm->data_available_acc = TRUE;
192  } else { //magneto
193  if (!(lsm->rx_buf[1] & LSM303D_ZYXMDA)) {
195  return;
196  }
197  lsm->data_mag.vect.x = Int16FromBuf(lsm->rx_buf, 2);
198  lsm->data_mag.vect.y = Int16FromBuf(lsm->rx_buf, 4);
199  lsm->data_mag.vect.z = Int16FromBuf(lsm->rx_buf, 6);
200  lsm->data_available_mag = TRUE;
202  }
203  }
204  } else {
205  if (lsm->init_status != LSM303D_CONF_UNINIT) { // Configuring but not yet initialized
206  if (lsm->spi_trans.status == SPITransSuccess || lsm->spi_trans.status == SPITransDone) {
209  }
210  if (lsm->spi_trans.status == SPITransFailed) {
211  lsm->init_status--;
213  lsm303d_spi_send_config(lsm); // Retry config (TODO max retry)
214  }
215  }
216  }
217 }
enum SPIClockPolarity cpol
clock polarity control
Definition: spi.h:149
#define LSM303D_REG_WHO_I_AM
LSM303D device identifier in LSM303D_REG_WHO_AM_I.
Definition: lsm303d_regs.h:95
enum SPIClockDiv cdiv
prescaler of main clock to use as SPI clock
Definition: spi.h:153
uint16_t output_length
number of data words to write
Definition: spi.h:146
#define LSM303D_AODR_MASK
Definition: lsm303d_regs.h:102
#define LSM303D_REG_WHO_AM_I
Definition: lsm303d_regs.h:45
#define LSM303D_INT2_DRDY_M
Definition: lsm303d_regs.h:155
bool initialized
config done flag
Definition: lsm303d_spi.h:40
CPHA = 1.
Definition: spi.h:69
#define LSM303D_REG_STATUS_M
Definition: lsm303d_regs.h:37
#define LSM303D_REG_CTRL2
Definition: lsm303d_regs.h:62
Lsm303dTarget
Definition: lsm303d.h:80
#define LSM303D_REG_CTRL6
Definition: lsm303d_regs.h:66
bool spi_submit(struct spi_periph *p, struct spi_transaction *t)
Submit SPI transaction.
Definition: spi_arch.c:458
#define LSM303D_ZYXMDA
Definition: lsm303d_regs.h:221
static void lsm303d_spi_send_config(struct Lsm303d_Spi *lsm)
Configuration function called once before normal use.
Definition: lsm303d_spi.c:80
CPOL = 1.
Definition: spi.h:78
enum SPIBitOrder bitorder
MSB/LSB order.
Definition: spi.h:152
void lsm303d_spi_read(struct Lsm303d_Spi *lsm)
Definition: lsm303d_spi.c:151
#define LSM303D_REG_CTRL7
Definition: lsm303d_regs.h:67
volatile uint8_t * output_buf
pointer to transmit buffer for DMA
Definition: spi.h:144
Driver for ST LSM303D 3D accelerometer and magnetometer.
#define LSM303D_REG_CTRL5
Definition: lsm303d_regs.h:65
enum SPISlaveSelect select
slave selection behavior
Definition: spi.h:148
#define FALSE
Definition: std.h:5
struct spi_periph * spi_p
Definition: lsm303d_spi.h:38
union Lsm303d_Spi::@308 data_accel
uint8_t acc_scale
full scale selection (m/s²)
Definition: lsm303d.h:58
#define LSM303D_M_ODR_MASK
Definition: lsm303d_regs.h:166
volatile uint8_t rx_buf[8]
Definition: lsm303d_spi.h:43
enum SPIClockPhase cpha
clock phase control
Definition: spi.h:150
uint8_t mag_scale
Full scale gain configuration (Gauss)
Definition: lsm303d.h:61
#define TRUE
Definition: std.h:4
#define LSM303D_ZYXADA
Definition: lsm303d_regs.h:211
SPI peripheral structure.
Definition: spi.h:168
Definition: spi.h:84
uint8_t mag_mode
Measurement mode.
Definition: lsm303d.h:62
static void lsm303d_set_default_config(struct Lsm303dConfig *c)
Definition: lsm303d.h:85
volatile bool data_available_mag
data ready flag magneto
Definition: lsm303d_spi.h:46
struct spi_transaction spi_trans
Definition: lsm303d_spi.h:39
uint8_t mag_rate
Data Output Rate Bits (Hz)
Definition: lsm303d.h:60
#define LSM303D_AZEN
Definition: lsm303d_regs.h:100
volatile bool data_available_acc
data ready flag accelero
Definition: lsm303d_spi.h:45
SPICallback after_cb
NULL or function called after the transaction.
Definition: spi.h:155
uint16_t val[TCOUPLE_NB]
enum Lsm303dTarget target
Definition: lsm303d_spi.h:41
#define LSM303D_AHPM_MASK
Definition: lsm303d_regs.h:197
volatile uint8_t tx_buf[2]
Definition: lsm303d_spi.h:42
void lsm303d_spi_init(struct Lsm303d_Spi *lsm, struct spi_periph *spi_p, uint8_t slave_idx, enum Lsm303dTarget target)
Definition: lsm303d_spi.c:32
#define LSM303D_REG_CTRL4
Definition: lsm303d_regs.h:64
#define LSM303D_AYEN
Definition: lsm303d_regs.h:99
uint16_t input_length
number of data words to read
Definition: spi.h:145
void lsm303d_spi_event(struct Lsm303d_Spi *lsm)
Definition: lsm303d_spi.c:176
struct Lsm303dConfig conf
Definition: lsm303d_spi.h:55
unsigned char uint8_t
Definition: types.h:14
static void lsm303d_spi_tx_reg(struct Lsm303d_Spi *lsm, uint8_t reg, uint8_t val)
Definition: lsm303d_spi.c:70
uint8_t acc_rate
Data Output Rate (Hz)
Definition: lsm303d.h:57
slave is selected before transaction and unselected after
Definition: spi.h:57
#define LSM303D_INT1_DRDY_A
Definition: lsm303d_regs.h:145
#define LSM303D_REG_CTRL3
Definition: lsm303d_regs.h:63
#define LSM303D_AXEN
Definition: lsm303d_regs.h:98
enum SPIDataSizeSelect dss
data transfer word size
Definition: spi.h:151
uint8_t slave_idx
slave id: SPI_SLAVE0 to SPI_SLAVE4
Definition: spi.h:147
Definition: spi.h:119
volatile uint8_t * input_buf
pointer to receive buffer for DMA
Definition: spi.h:143
#define LSM303D_AFS_MASK
Definition: lsm303d_regs.h:122
#define LSM303D_REG_CTRL1
Definition: lsm303d_regs.h:61
SPICallback before_cb
NULL or function called before the transaction.
Definition: spi.h:154
union Lsm303d_Spi::@309 data_mag
#define Int16FromBuf(_buf, _idx)
Definition: lsm303d_spi.c:172
void lsm303d_spi_start_configure(struct Lsm303d_Spi *lsm)
Definition: lsm303d_spi.c:140
struct FloatVect2 target
#define LSM303D_REG_STATUS_A
Definition: lsm303d_regs.h:68
enum SPITransactionStatus status
Definition: spi.h:156
#define LSM303D_MFS_MASK
Definition: lsm303d_regs.h:182
enum Lsm303dConfStatus init_status
Definition: lsm303d_spi.h:44