Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
mpu60x0_i2c.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_i2c_init(struct Mpu60x0_I2c *mpu, struct i2c_periph *i2c_p, uint8_t addr)
32 {
33  /* set i2c_peripheral */
34  mpu->i2c_p = i2c_p;
35 
36  /* slave address */
37  mpu->i2c_trans.slave_addr = addr;
38  /* set inital status: Success or Done */
40 
41  /* set default MPU60X0 config options */
43 
44  mpu->data_available = false;
45  mpu->config.initialized = false;
47 
49 }
50 
51 
52 static void mpu60x0_i2c_write_to_reg(void *mpu, uint8_t _reg, uint8_t _val)
53 {
54  struct Mpu60x0_I2c *mpu_i2c = (struct Mpu60x0_I2c *)(mpu);
55  mpu_i2c->i2c_trans.buf[0] = _reg;
56  mpu_i2c->i2c_trans.buf[1] = _val;
57  i2c_transmit(mpu_i2c->i2c_p, &(mpu_i2c->i2c_trans), mpu_i2c->i2c_trans.slave_addr, 2);
58 }
59 
60 // Configuration function called once before normal use
62 {
64  mpu->config.init_status++;
66  mpu60x0_send_config(mpu60x0_i2c_write_to_reg, (void *)mpu, &(mpu->config));
67  }
68  }
69 }
70 
71 void mpu60x0_i2c_read(struct Mpu60x0_I2c *mpu)
72 {
73  if (mpu->config.initialized && mpu->i2c_trans.status == I2CTransDone) {
74  /* set read bit and multiple byte bit, then address */
76  i2c_transceive(mpu->i2c_p, &(mpu->i2c_trans), mpu->i2c_trans.slave_addr, 1, mpu->config.nb_bytes);
77  }
78 }
79 
80 #define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx]<<8) | _buf[_idx+1]))
81 
82 void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu)
83 {
84  if (mpu->config.initialized) {
85  if (mpu->i2c_trans.status == I2CTransFailed) {
87  } else if (mpu->i2c_trans.status == I2CTransSuccess) {
88  // Successfull reading
89  if (bit_is_set(mpu->i2c_trans.buf[0], 0)) {
90  // new data
91  mpu->data_accel.vect.x = Int16FromBuf(mpu->i2c_trans.buf, 1);
92  mpu->data_accel.vect.y = Int16FromBuf(mpu->i2c_trans.buf, 3);
93  mpu->data_accel.vect.z = Int16FromBuf(mpu->i2c_trans.buf, 5);
94  mpu->data_rates.rates.p = Int16FromBuf(mpu->i2c_trans.buf, 9);
95  mpu->data_rates.rates.q = Int16FromBuf(mpu->i2c_trans.buf, 11);
96  mpu->data_rates.rates.r = Int16FromBuf(mpu->i2c_trans.buf, 13);
97 
98  int16_t temp_raw = Int16FromBuf(mpu->i2c_trans.buf, 7);
99  mpu->temp = (float)temp_raw / 340.0f + 36.53f;
100 
101  // if we are reading slaves through the mpu, copy the ext_sens_data
102  if ((mpu->config.i2c_bypass == FALSE) && (mpu->config.nb_slaves > 0)) {
103  /* the buffer is volatile, since filled from ISR
104  * but we know it's ok to use it here so we silence the warning
105  */
106 #pragma GCC diagnostic push
107 #pragma GCC diagnostic ignored "-Wcast-qual"
108  memcpy(mpu->data_ext, (uint8_t *) & (mpu->i2c_trans.buf[15]), mpu->config.nb_bytes - 15);
109 #pragma GCC diagnostic pop
110  }
111 
112  mpu->data_available = true;
113  }
115  }
116  } else if (mpu->config.init_status != MPU60X0_CONF_UNINIT) { // Configuring but not yet initialized
117  switch (mpu->i2c_trans.status) {
118  case I2CTransFailed:
119  mpu->config.init_status--; // Retry config (TODO max retry)
120  /* Falls through. */
121  case I2CTransSuccess:
122  case I2CTransDone:
123  mpu60x0_send_config(mpu60x0_i2c_write_to_reg, (void *)mpu, &(mpu->config));
124  if (mpu->config.initialized) {
125  mpu->i2c_trans.status = I2CTransDone;
126  }
127  break;
128  default:
129  break;
130  }
131  }
132 }
133 
136 {
137  struct Mpu60x0_I2c *mpu_i2c = (struct Mpu60x0_I2c *)(mpu);
138 
139  if (mpu_i2c->slave_init_status == MPU60X0_I2C_CONF_UNINIT) {
140  mpu_i2c->slave_init_status++;
141  }
142 
143  switch (mpu_i2c->slave_init_status) {
145  mpu_set(mpu, MPU60X0_REG_USER_CTRL, 0);
146  mpu_i2c->slave_init_status++;
147  break;
149  /* switch to I2C passthrough */
150  mpu_set(mpu, MPU60X0_REG_INT_PIN_CFG, (1 << 1));
151  mpu_i2c->slave_init_status++;
152  break;
154  /* configure each slave until all nb_slaves are done */
155  if (mpu_i2c->config.nb_slave_init < mpu_i2c->config.nb_slaves && mpu_i2c->config.nb_slave_init < MPU60X0_I2C_NB_SLAVES) {
156  // proceed to next slave if configure for current one returns true
157  if (mpu_i2c->config.slaves[mpu_i2c->config.nb_slave_init].configure(mpu_set, mpu)) {
158  mpu_i2c->config.nb_slave_init++;
159  }
160  }
161  else {
162  /* all slave devies configured, continue MPU side configuration of I2C slave stuff */
163  mpu_i2c->slave_init_status++;
164  }
165  break;
167  if (mpu_i2c->config.i2c_bypass) {
168  /* if bypassing I2C skip MPU I2C master setup */
170  } else {
171  /* disable I2C passthrough again */
172  mpu_set(mpu, MPU60X0_REG_INT_PIN_CFG, (0 << 1));
173  mpu_i2c->slave_init_status++;
174  }
175  break;
177  /* configure MPU I2C master clock and stop/start between slave reads */
178  mpu_set(mpu, MPU60X0_REG_I2C_MST_CTRL,
179  ((1 << 4) | mpu_i2c->config.i2c_mst_clk));
180  mpu_i2c->slave_init_status++;
181  break;
183  /* Set I2C slaves delayed sample rate */
184  mpu_set(mpu, MPU60X0_REG_I2C_MST_DELAY, mpu_i2c->config.i2c_mst_delay);
185  mpu_i2c->slave_init_status++;
186  break;
188  /* I2C slave0 sample rate/2 = 100/2 = 50Hz */
189  mpu_set(mpu, MPU60X0_REG_I2C_SLV4_CTRL, 0);
190  mpu_i2c->slave_init_status++;
191  break;
193  /* enable internal I2C master */
194  mpu_set(mpu, MPU60X0_REG_USER_CTRL, (1 << MPU60X0_I2C_MST_EN));
195  mpu_i2c->slave_init_status++;
196  break;
198  return true;
199  default:
200  break;
201  }
202  return false;
203 }
Mpu60x0_I2c::temp
float temp
temperature in degrees Celcius
Definition: mpu60x0_i2c.h:66
Mpu60x0Config::nb_bytes
uint8_t nb_bytes
number of bytes to read starting with MPU60X0_REG_INT_STATUS
Definition: mpu60x0.h:147
i2c_transaction::buf
volatile uint8_t buf[I2C_BUF_LEN]
Transaction buffer With I2C_BUF_LEN number of bytes.
Definition: i2c.h:122
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
MPU60X0_REG_USER_CTRL
#define MPU60X0_REG_USER_CTRL
Definition: mpu60x0_regs.h:39
MPU60X0_I2C_CONF_I2C_MST_DELAY
@ MPU60X0_I2C_CONF_I2C_MST_DELAY
Definition: mpu60x0_i2c.h:48
MPU60X0_I2C_CONF_SLAVES_CONFIGURE
@ MPU60X0_I2C_CONF_SLAVES_CONFIGURE
Definition: mpu60x0_i2c.h:45
MPU60X0_I2C_CONF_I2C_MST_CLK
@ MPU60X0_I2C_CONF_I2C_MST_CLK
Definition: mpu60x0_i2c.h:47
MPU60X0_I2C_CONF_DONE
@ MPU60X0_I2C_CONF_DONE
Definition: mpu60x0_i2c.h:51
MPU60X0_I2C_CONF_I2C_SMPLRT
@ MPU60X0_I2C_CONF_I2C_SMPLRT
Definition: mpu60x0_i2c.h:49
MPU60X0_REG_INT_PIN_CFG
#define MPU60X0_REG_INT_PIN_CFG
Definition: mpu60x0_regs.h:89
I2CTransFailed
@ I2CTransFailed
transaction failed
Definition: i2c.h:58
mpu60x0_i2c_start_configure
void mpu60x0_i2c_start_configure(struct Mpu60x0_I2c *mpu)
Definition: mpu60x0_i2c.c:61
Mpu60x0Config::i2c_mst_clk
enum Mpu60x0MstClk i2c_mst_clk
MPU I2C master clock speed.
Definition: mpu60x0.h:159
MPU60X0_REG_I2C_MST_CTRL
#define MPU60X0_REG_I2C_MST_CTRL
Definition: mpu60x0_regs.h:58
MPU60X0_I2C_CONF_I2C_BYPASS_DIS
@ MPU60X0_I2C_CONF_I2C_BYPASS_DIS
Definition: mpu60x0_i2c.h:46
Mpu60x0_I2c::i2c_p
struct i2c_periph * i2c_p
Definition: mpu60x0_i2c.h:55
mpu60x0_configure_i2c_slaves
bool mpu60x0_configure_i2c_slaves(Mpu60x0ConfigSet mpu_set, void *mpu)
configure the registered I2C slaves
Definition: mpu60x0_i2c.c:135
Mpu60x0ConfigSet
void(* Mpu60x0ConfigSet)(void *mpu, uint8_t _reg, uint8_t _val)
Configuration function prototype.
Definition: mpu60x0.h:129
MPU60X0_I2C_CONF_I2C_MST_DIS
@ MPU60X0_I2C_CONF_I2C_MST_DIS
Definition: mpu60x0_i2c.h:43
I2CTransSuccess
@ I2CTransSuccess
transaction successfully finished by I2C driver
Definition: i2c.h:57
MPU60X0_I2C_CONF_I2C_MST_EN
@ MPU60X0_I2C_CONF_I2C_MST_EN
Definition: mpu60x0_i2c.h:50
MPU60X0_REG_I2C_MST_DELAY
#define MPU60X0_REG_I2C_MST_DELAY
Definition: mpu60x0_regs.h:60
Mpu60x0I2cSlave::configure
Mpu60x0I2cSlaveConfigure configure
Definition: mpu60x0.h:135
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
mpu60x0_i2c_init
void mpu60x0_i2c_init(struct Mpu60x0_I2c *mpu, struct i2c_periph *i2c_p, uint8_t addr)
Definition: mpu60x0_i2c.c:31
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::slave_init_status
enum Mpu60x0I2cSlaveInitStatus slave_init_status
Definition: mpu60x0_i2c.h:69
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
int16_t
signed short int16_t
Definition: types.h:17
uint8_t
unsigned char uint8_t
Definition: types.h:14
Mpu60x0_I2c::i2c_trans
struct i2c_transaction i2c_trans
Definition: mpu60x0_i2c.h:56
mpu60x0_i2c_event
void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu)
Definition: mpu60x0_i2c.c:82
Mpu60x0Config::init_status
enum Mpu60x0ConfStatus init_status
init status
Definition: mpu60x0.h:148
i2c_transaction::status
enum I2CTransactionStatus status
Transaction status.
Definition: i2c.h:126
Mpu60x0_I2c::config
struct Mpu60x0Config config
Definition: mpu60x0_i2c.h:68
mpu60x0_i2c_read
void mpu60x0_i2c_read(struct Mpu60x0_I2c *mpu)
Definition: mpu60x0_i2c.c:71
mpu60x0_i2c_write_to_reg
static void mpu60x0_i2c_write_to_reg(void *mpu, uint8_t _reg, uint8_t _val)
Definition: mpu60x0_i2c.c:52
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
i2c_transaction::slave_addr
uint8_t slave_addr
Slave address.
Definition: i2c.h:104
MPU60X0_I2C_NB_SLAVES
#define MPU60X0_I2C_NB_SLAVES
Definition: mpu60x0.h:53
Mpu60x0Config::nb_slaves
uint8_t nb_slaves
number of used I2C slaves
Definition: mpu60x0.h:156
Mpu60x0Config::i2c_bypass
bool i2c_bypass
Bypass MPU I2C.
Definition: mpu60x0.h:154
Mpu60x0_I2c::data_ext
uint8_t data_ext[MPU60X0_BUFFER_EXT_LEN]
Definition: mpu60x0_i2c.h:67
Mpu60x0_I2c::data_accel
union Mpu60x0_I2c::@331 data_accel
MPU60X0_REG_INT_STATUS
#define MPU60X0_REG_INT_STATUS
Definition: mpu60x0_regs.h:91
MPU60X0_I2C_CONF_I2C_BYPASS_EN
@ MPU60X0_I2C_CONF_I2C_BYPASS_EN
Definition: mpu60x0_i2c.h:44
Mpu60x0Config::i2c_mst_delay
uint8_t i2c_mst_delay
MPU I2C slaves delayed sample rate.
Definition: mpu60x0.h:160
Mpu60x0Config::slaves
struct Mpu60x0I2cSlave slaves[MPU60X0_I2C_NB_SLAVES]
I2C slaves.
Definition: mpu60x0.h:158
Mpu60x0_I2c::data_rates
union Mpu60x0_I2c::@332 data_rates
mpu60x0_set_default_config
void mpu60x0_set_default_config(struct Mpu60x0Config *c)
Definition: mpu60x0.c:60
I2CTransDone
@ I2CTransDone
transaction set to done by user level
Definition: i2c.h:59
MPU60X0_I2C_CONF_UNINIT
@ MPU60X0_I2C_CONF_UNINIT
Definition: mpu60x0_i2c.h:42
Mpu60x0_I2c::data_available
volatile bool data_available
data ready flag
Definition: mpu60x0_i2c.h:57
Int16FromBuf
#define Int16FromBuf(_buf, _idx)
Definition: mpu60x0_i2c.c:80
FALSE
#define FALSE
Definition: std.h:5
MPU60X0_REG_I2C_SLV4_CTRL
#define MPU60X0_REG_I2C_SLV4_CTRL
Definition: mpu60x0_regs.h:85
mpu60x0_i2c.h
i2c_periph
Definition: i2c.h:144
MPU60X0_I2C_MST_EN
#define MPU60X0_I2C_MST_EN
Definition: mpu60x0_regs.h:133
Mpu60x0Config::initialized
bool initialized
config done flag
Definition: mpu60x0.h:149
Mpu60x0_I2c
Definition: mpu60x0_i2c.h:54