Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
mpu60x0_spi.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2013 Gautier Hattenberger
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, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
30 
31 void mpu60x0_spi_init(struct Mpu60x0_Spi *mpu, struct spi_periph *spi_p, uint8_t slave_idx)
32 {
33  /* set spi_peripheral */
34  mpu->spi_p = spi_p;
35 
36  /* configure spi transaction */
39  mpu->spi_trans.dss = SPIDss8bit;
41  mpu->spi_trans.cdiv = SPIDiv64;
42 
44  mpu->spi_trans.slave_idx = slave_idx;
45  mpu->spi_trans.output_length = 2;
47  mpu->spi_trans.before_cb = NULL;
48  mpu->spi_trans.after_cb = NULL;
49  mpu->spi_trans.input_buf = &(mpu->rx_buf[0]);
50  mpu->spi_trans.output_buf = &(mpu->tx_buf[0]);
51 
52  /* set inital status: Success or Done */
54 
55  /* set default MPU60X0 config options */
57 
58  mpu->data_available = false;
59  mpu->config.initialized = false;
61 
63 }
64 
65 
66 static void mpu60x0_spi_write_to_reg(void *mpu, uint8_t _reg, uint8_t _val)
67 {
68  struct Mpu60x0_Spi *mpu_spi = (struct Mpu60x0_Spi *)(mpu);
69  mpu_spi->spi_trans.output_length = 2;
70  mpu_spi->spi_trans.input_length = 0;
71  mpu_spi->tx_buf[0] = _reg;
72  mpu_spi->tx_buf[1] = _val;
73  spi_submit(mpu_spi->spi_p, &(mpu_spi->spi_trans));
74 }
75 
76 // Configuration function called once before normal use
78 {
80 
81  // First check if we found the chip (succesfull WHO_AM_I response)
82  if (mpu->spi_trans.status == SPITransSuccess &&
83  (mpu->rx_buf[1] == MPU60X0_WHOAMI_REPLY ||
84  mpu->rx_buf[1] == ICM20600_WHOAMI_REPLY ||
85  mpu->rx_buf[1] == ICM20608_WHOAMI_REPLY ||
86  mpu->rx_buf[1] == ICM20602_WHOAMI_REPLY ||
87  mpu->rx_buf[1] == ICM20689_WHOAMI_REPLY)) {
88 
89  if (mpu->rx_buf[1] == MPU60X0_WHOAMI_REPLY) {
90  mpu->config.type = MPU60X0;
91  } else if (mpu->rx_buf[1] == ICM20600_WHOAMI_REPLY) {
92  mpu->config.type = ICM20600;
93  } else if (mpu->rx_buf[1] == ICM20608_WHOAMI_REPLY) {
94  mpu->config.type = ICM20608;
95  } else if (mpu->rx_buf[1] == ICM20602_WHOAMI_REPLY) {
96  mpu->config.type = ICM20602;
97  } else if (mpu->rx_buf[1] == ICM20689_WHOAMI_REPLY) {
98  mpu->config.type = ICM20689;
99  }
100 
101  mpu->config.init_status++;
103  mpu60x0_send_config(mpu60x0_spi_write_to_reg, (void *)mpu, &(mpu->config));
104  }
105  // Send WHO_AM_I to check if chip is there
106  else if (mpu->spi_trans.status != SPITransRunning && mpu->spi_trans.status != SPITransPending) {
107  mpu->spi_trans.output_length = 1;
108  mpu->spi_trans.input_length = 2;
110  spi_submit(mpu->spi_p, &(mpu->spi_trans));
111  }
112  }
113 }
114 
115 void mpu60x0_spi_read(struct Mpu60x0_Spi *mpu)
116 {
117  if (mpu->config.initialized && mpu->spi_trans.status == SPITransDone) {
118  mpu->spi_trans.output_length = 1;
119  mpu->spi_trans.input_length = 1 + mpu->config.nb_bytes;
120  /* set read bit and multiple byte bit, then address */
122  spi_submit(mpu->spi_p, &(mpu->spi_trans));
123  }
124 }
125 
126 #define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx]<<8) | _buf[_idx+1]))
127 
129 {
130  if (mpu->config.initialized) {
131  if (mpu->spi_trans.status == SPITransFailed) {
133  } else if (mpu->spi_trans.status == SPITransSuccess) {
134  // Successfull reading
135  if (bit_is_set(mpu->rx_buf[1], 0)) {
136  // new data
137  mpu->data_accel.vect.x = Int16FromBuf(mpu->rx_buf, 2);
138  mpu->data_accel.vect.y = Int16FromBuf(mpu->rx_buf, 4);
139  mpu->data_accel.vect.z = Int16FromBuf(mpu->rx_buf, 6);
140  mpu->data_rates.rates.p = Int16FromBuf(mpu->rx_buf, 10);
141  mpu->data_rates.rates.q = Int16FromBuf(mpu->rx_buf, 12);
142  mpu->data_rates.rates.r = Int16FromBuf(mpu->rx_buf, 14);
143 
144  int16_t temp_raw = Int16FromBuf(mpu->rx_buf, 8);
145  if (mpu->config.type == MPU60X0) {
146  mpu->temp = (float)temp_raw / 361.0f + 35.0f;
147  } else {
148  mpu->temp = (float)temp_raw / 326.8f + 25.0f;
149  }
150 
151  // if we are reading slaves, copy the ext_sens_data
152  if (mpu->config.nb_slaves > 0) {
153  /* the buffer is volatile, since filled from ISR
154  * but we know it's ok to use it here so we silence the warning
155  */
156 #pragma GCC diagnostic push
157 #pragma GCC diagnostic ignored "-Wcast-qual"
158  memcpy(mpu->data_ext, (uint8_t *) & (mpu->rx_buf[16]), mpu->config.nb_bytes - 15);
159 #pragma GCC diagnostic pop
160  }
161 
162  mpu->data_available = true;
163  }
165  }
166  } else if (mpu->config.init_status != MPU60X0_CONF_UNINIT) { // Configuring but not yet initialized
167  switch (mpu->spi_trans.status) {
168  case SPITransFailed:
169  mpu->config.init_status--; // Retry config (TODO max retry)
170  /* Falls through. */
171  case SPITransSuccess:
172  case SPITransDone:
173  mpu60x0_send_config(mpu60x0_spi_write_to_reg, (void *)mpu, &(mpu->config));
174  if (mpu->config.initialized) {
175  mpu->spi_trans.status = SPITransDone;
176  }
177  break;
178  default:
179  break;
180  }
181  }
182 }
183 
186 {
187  struct Mpu60x0_Spi *mpu_spi = (struct Mpu60x0_Spi *)(mpu);
188 
189  if (mpu_spi->slave_init_status == MPU60X0_SPI_CONF_UNINIT) {
190  mpu_spi->slave_init_status++;
191  }
192 
193  switch (mpu_spi->slave_init_status) {
195  /* configure MPU I2C master clock and stop/start between slave reads */
196  mpu_set(mpu, MPU60X0_REG_I2C_MST_CTRL, ((1 << 4) | mpu_spi->config.i2c_mst_clk));
197  mpu_spi->slave_init_status++;
198  break;
200  /* Set I2C slaves delayed sample rate */
201  mpu_set(mpu, MPU60X0_REG_I2C_MST_DELAY, mpu_spi->config.i2c_mst_delay);
202  mpu_spi->slave_init_status++;
203  break;
205  /* enable internal I2C master and disable primary I2C interface */
206  mpu_set(mpu, MPU60X0_REG_USER_CTRL, ((1 << MPU60X0_I2C_IF_DIS) |
207  (1 << MPU60X0_I2C_MST_EN)));
208  mpu_spi->slave_init_status++;
209  break;
211  /* configure each slave until all nb_slaves are done */
212  if (mpu_spi->config.nb_slave_init < mpu_spi->config.nb_slaves && mpu_spi->config.nb_slave_init < MPU60X0_I2C_NB_SLAVES) {
213  // proceed to next slave if configure for current one returns true
214  if (mpu_spi->config.slaves[mpu_spi->config.nb_slave_init].configure(mpu_set, mpu)) {
215  mpu_spi->config.nb_slave_init++;
216  }
217  }
218  else {
219  /* all slave devies configured, continue MPU side configuration of I2C slave stuff */
220  mpu_spi->slave_init_status++;
221  }
222  break;
224  return true;
225  default:
226  break;
227  }
228  return false;
229 }
Mpu60x0Config::nb_bytes
uint8_t nb_bytes
number of bytes to read starting with MPU60X0_REG_INT_STATUS
Definition: mpu60x0.h:147
Mpu60x0_Spi::spi_trans
struct spi_transaction spi_trans
Definition: mpu60x0_spi.h:53
spi_transaction::cdiv
enum SPIClockDiv cdiv
prescaler of main clock to use as SPI clock
Definition: spi.h:159
Mpu60x0Config::nb_slave_init
uint8_t nb_slave_init
number of already configured/initialized slaves
Definition: mpu60x0.h:157
MPU60X0_CONF_UNINIT
@ MPU60X0_CONF_UNINIT
Definition: mpu60x0.h:113
spi_transaction::cpol
enum SPIClockPolarity cpol
clock polarity control
Definition: spi.h:155
MPU60X0
@ MPU60X0
Definition: mpu60x0.h:105
SPITransPending
@ SPITransPending
Definition: spi.h:97
MPU60X0_SPI_CONF_SLAVES_CONFIGURE
@ MPU60X0_SPI_CONF_SLAVES_CONFIGURE
Definition: mpu60x0_spi.h:47
ICM20689
@ ICM20689
Definition: mpu60x0.h:109
MPU60X0_REG_USER_CTRL
#define MPU60X0_REG_USER_CTRL
Definition: mpu60x0_regs.h:39
ICM20602
@ ICM20602
Definition: mpu60x0.h:108
spi_transaction::output_length
uint16_t output_length
number of data words to write
Definition: spi.h:152
ICM20600_WHOAMI_REPLY
#define ICM20600_WHOAMI_REPLY
Definition: mpu60x0_regs.h:120
MPU60X0_SPI_CONF_UNINIT
@ MPU60X0_SPI_CONF_UNINIT
Definition: mpu60x0_spi.h:43
Mpu60x0_Spi::data_ext
uint8_t data_ext[MPU60X0_BUFFER_EXT_LEN]
Definition: mpu60x0_spi.h:66
mpu60x0_configure_i2c_slaves
bool mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void *mpu)
configure the registered I2C slaves
Definition: mpu60x0_spi.c:185
MPU60X0_SPI_READ
#define MPU60X0_SPI_READ
Definition: mpu60x0_regs.h:35
MPU60X0_SPI_CONF_I2C_MST_EN
@ MPU60X0_SPI_CONF_I2C_MST_EN
Definition: mpu60x0_spi.h:46
Mpu60x0Config::i2c_mst_clk
enum Mpu60x0MstClk i2c_mst_clk
MPU I2C master clock speed.
Definition: mpu60x0.h:159
MPU60X0_WHOAMI_REPLY
#define MPU60X0_WHOAMI_REPLY
Definition: mpu60x0_regs.h:119
SPISelectUnselect
@ SPISelectUnselect
slave is selected before transaction and unselected after
Definition: spi.h:63
MPU60X0_SPI_CONF_DONE
@ MPU60X0_SPI_CONF_DONE
Definition: mpu60x0_spi.h:48
MPU60X0_REG_I2C_MST_CTRL
#define MPU60X0_REG_I2C_MST_CTRL
Definition: mpu60x0_regs.h:58
ICM20608
@ ICM20608
Definition: mpu60x0.h:107
SPITransSuccess
@ SPITransSuccess
Definition: spi.h:99
MPU60X0_REG_WHO_AM_I
#define MPU60X0_REG_WHO_AM_I
Definition: mpu60x0_regs.h:118
spi_transaction::bitorder
enum SPIBitOrder bitorder
MSB/LSB order.
Definition: spi.h:158
mpu60x0_spi.h
Mpu60x0ConfigSet
void(* Mpu60x0ConfigSet)(void *mpu, uint8_t _reg, uint8_t _val)
Configuration function prototype.
Definition: mpu60x0.h:129
spi_transaction::output_buf
volatile uint8_t * output_buf
pointer to transmit buffer for DMA
Definition: spi.h:150
spi_periph
SPI peripheral structure.
Definition: spi.h:174
spi_transaction::select
enum SPISlaveSelect select
slave selection behavior
Definition: spi.h:154
mpu60x0_spi_write_to_reg
static void mpu60x0_spi_write_to_reg(void *mpu, uint8_t _reg, uint8_t _val)
Definition: mpu60x0_spi.c:66
MPU60X0_REG_I2C_MST_DELAY
#define MPU60X0_REG_I2C_MST_DELAY
Definition: mpu60x0_regs.h:60
SPIMSBFirst
@ SPIMSBFirst
Definition: spi.h:112
Mpu60x0I2cSlave::configure
Mpu60x0I2cSlaveConfigure configure
Definition: mpu60x0.h:135
MPU60X0_BUFFER_LEN
#define MPU60X0_BUFFER_LEN
Definition: mpu60x0_spi.h:39
Mpu60x0_Spi::spi_p
struct spi_periph * spi_p
Definition: mpu60x0_spi.h:52
spi_transaction::cpha
enum SPIClockPhase cpha
clock phase control
Definition: spi.h:156
spi_transaction::after_cb
SPICallback after_cb
NULL or function called after the transaction.
Definition: spi.h:161
spi_submit
bool spi_submit(struct spi_periph *p, struct spi_transaction *t)
Submit SPI transaction.
Definition: spi_arch.c:511
SPITransFailed
@ SPITransFailed
Definition: spi.h:100
mpu60x0_send_config
void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void *mpu, struct Mpu60x0Config *config)
Configuration sequence called once before normal use.
Definition: mpu60x0.c:82
MPU60X0_I2C_IF_DIS
#define MPU60X0_I2C_IF_DIS
Definition: mpu60x0_regs.h:132
Int16FromBuf
#define Int16FromBuf(_buf, _idx)
Definition: mpu60x0_spi.c:126
int16_t
signed short int16_t
Definition: types.h:17
SPICpolIdleHigh
@ SPICpolIdleHigh
CPOL = 1.
Definition: spi.h:84
uint8_t
unsigned char uint8_t
Definition: types.h:14
Mpu60x0Config::init_status
enum Mpu60x0ConfStatus init_status
init status
Definition: mpu60x0.h:148
Mpu60x0_Spi::slave_init_status
enum Mpu60x0SpiSlaveInitStatus slave_init_status
Definition: mpu60x0_spi.h:68
ICM20689_WHOAMI_REPLY
#define ICM20689_WHOAMI_REPLY
Definition: mpu60x0_regs.h:123
mpu60x0_spi_start_configure
void mpu60x0_spi_start_configure(struct Mpu60x0_Spi *mpu)
Definition: mpu60x0_spi.c:77
SPIDss8bit
@ SPIDss8bit
Definition: spi.h:90
mpu60x0_spi_read
void mpu60x0_spi_read(struct Mpu60x0_Spi *mpu)
Definition: mpu60x0_spi.c:115
ICM20602_WHOAMI_REPLY
#define ICM20602_WHOAMI_REPLY
Definition: mpu60x0_regs.h:122
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
MPU60X0_SPI_CONF_I2C_MST_CLK
@ MPU60X0_SPI_CONF_I2C_MST_CLK
Definition: mpu60x0_spi.h:44
MPU60X0_I2C_NB_SLAVES
#define MPU60X0_I2C_NB_SLAVES
Definition: mpu60x0.h:53
SPIDiv64
@ SPIDiv64
Definition: spi.h:125
Mpu60x0Config::nb_slaves
uint8_t nb_slaves
number of used I2C slaves
Definition: mpu60x0.h:156
spi_transaction::input_length
uint16_t input_length
number of data words to read
Definition: spi.h:151
ICM20600
@ ICM20600
Definition: mpu60x0.h:106
spi_transaction::dss
enum SPIDataSizeSelect dss
data transfer word size
Definition: spi.h:157
MPU60X0_REG_INT_STATUS
#define MPU60X0_REG_INT_STATUS
Definition: mpu60x0_regs.h:91
spi_transaction::slave_idx
uint8_t slave_idx
slave id: SPI_SLAVE0 to SPI_SLAVE4
Definition: spi.h:153
SPITransDone
@ SPITransDone
Definition: spi.h:101
Mpu60x0_Spi::data_accel
union Mpu60x0_Spi::@333 data_accel
Mpu60x0Config::i2c_mst_delay
uint8_t i2c_mst_delay
MPU I2C slaves delayed sample rate.
Definition: mpu60x0.h:160
SPITransRunning
@ SPITransRunning
Definition: spi.h:98
Mpu60x0Config::slaves
struct Mpu60x0I2cSlave slaves[MPU60X0_I2C_NB_SLAVES]
I2C slaves.
Definition: mpu60x0.h:158
spi_transaction::input_buf
volatile uint8_t * input_buf
pointer to receive buffer for DMA
Definition: spi.h:149
Mpu60x0_Spi::data_available
volatile bool data_available
data ready flag
Definition: mpu60x0_spi.h:56
Mpu60x0_Spi::temp
float temp
temperature in degrees Celcius
Definition: mpu60x0_spi.h:65
SPICphaEdge2
@ SPICphaEdge2
CPHA = 1.
Definition: spi.h:75
MPU60X0_SPI_CONF_I2C_MST_DELAY
@ MPU60X0_SPI_CONF_I2C_MST_DELAY
Definition: mpu60x0_spi.h:45
Mpu60x0_Spi::tx_buf
volatile uint8_t tx_buf[2]
Definition: mpu60x0_spi.h:54
mpu60x0_set_default_config
void mpu60x0_set_default_config(struct Mpu60x0Config *c)
Definition: mpu60x0.c:60
Mpu60x0_Spi
Definition: mpu60x0_spi.h:51
spi_transaction::before_cb
SPICallback before_cb
NULL or function called before the transaction.
Definition: spi.h:160
Mpu60x0Config::type
enum Mpu60x0Type type
The type of sensor (MPU60x0, ICM20608, ...)
Definition: mpu60x0.h:139
Mpu60x0_Spi::data_rates
union Mpu60x0_Spi::@334 data_rates
Mpu60x0_Spi::config
struct Mpu60x0Config config
Definition: mpu60x0_spi.h:67
mpu60x0_spi_init
void mpu60x0_spi_init(struct Mpu60x0_Spi *mpu, struct spi_periph *spi_p, uint8_t slave_idx)
Definition: mpu60x0_spi.c:31
spi_transaction::status
enum SPITransactionStatus status
Definition: spi.h:162
ICM20608_WHOAMI_REPLY
#define ICM20608_WHOAMI_REPLY
Definition: mpu60x0_regs.h:121
MPU60X0_I2C_MST_EN
#define MPU60X0_I2C_MST_EN
Definition: mpu60x0_regs.h:133
Mpu60x0_Spi::rx_buf
volatile uint8_t rx_buf[MPU60X0_BUFFER_LEN]
Definition: mpu60x0_spi.h:55
mpu60x0_spi_event
void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu)
Definition: mpu60x0_spi.c:128
Mpu60x0Config::initialized
bool initialized
config done flag
Definition: mpu60x0.h:149