Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
mpu9250_i2c.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 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 
29 
30 bool imu_mpu9250_configure_mag_slave(Mpu9250ConfigSet mpu_set __attribute__((unused)),
31  void *mpu __attribute__((unused)));
32 
33 void mpu9250_i2c_init(struct Mpu9250_I2c *mpu, struct i2c_periph *i2c_p, uint8_t addr)
34 {
35  /* set i2c_peripheral */
36  mpu->i2c_p = i2c_p;
37 
38  /* slave address */
39  mpu->i2c_trans.slave_addr = addr;
40  /* set inital status: Success or Done */
42 
43  /* set default MPU9250 config options */
45 
46  mpu->data_available = false;
47  mpu->config.initialized = false;
49 
50 #if IMU_MPU9250_READ_MAG
51  /* "internal" ak8963 magnetometer */
52  ak8963_init(&mpu->akm, i2c_p, MPU9250_MAG_ADDR);
53 
54  /* mag is declared as slave to call the configure function,
55  * regardless if it is an actual MPU slave or passthrough
56  */
57  mpu->config.nb_slaves = 1;
58  /* set callback function to configure mag */
60  /* read the mag directly for now */
61  mpu->config.i2c_bypass = true;
62 
64 #endif
65 }
66 
67 
68 static void mpu9250_i2c_write_to_reg(void *mpu, uint8_t _reg, uint8_t _val)
69 {
70  struct Mpu9250_I2c *mpu_i2c = (struct Mpu9250_I2c *)(mpu);
71  mpu_i2c->i2c_trans.buf[0] = _reg;
72  mpu_i2c->i2c_trans.buf[1] = _val;
73  i2c_transmit(mpu_i2c->i2c_p, &(mpu_i2c->i2c_trans), mpu_i2c->i2c_trans.slave_addr, 2);
74 }
75 
76 // Configuration function called once before normal use
78 {
80  mpu->config.init_status++;
82  mpu9250_send_config(mpu9250_i2c_write_to_reg, (void *)mpu, &(mpu->config));
83  }
84  }
85 }
86 
87 void mpu9250_i2c_read(struct Mpu9250_I2c *mpu)
88 {
89  if (mpu->config.initialized && mpu->i2c_trans.status == I2CTransDone) {
90  /* set read bit and multiple byte bit, then address */
92  i2c_transceive(mpu->i2c_p, &(mpu->i2c_trans), mpu->i2c_trans.slave_addr, 1, mpu->config.nb_bytes);
93  /* read mag */
94 #if IMU_MPU9250_READ_MAG
95 #ifdef MPU9250_MAG_PRESCALER
96  RunOnceEvery(MPU9250_MAG_PRESCALER, ak8963_read(&mpu->akm));
97 #else
98  ak8963_read(&mpu->akm);
99 #endif
100 #endif
101  }
102 }
103 
104 #define Int16FromBuf(_buf,_idx) ((int16_t)((_buf[_idx]<<8) | _buf[_idx+1]))
105 
107 {
108  if (mpu->config.initialized) {
109  if (mpu->i2c_trans.status == I2CTransFailed) {
111  } else if (mpu->i2c_trans.status == I2CTransSuccess) {
112  // Successfull reading
113  if (bit_is_set(mpu->i2c_trans.buf[0], 0)) {
114  // new data
115  mpu->data_accel.vect.x = Int16FromBuf(mpu->i2c_trans.buf, 1);
116  mpu->data_accel.vect.y = Int16FromBuf(mpu->i2c_trans.buf, 3);
117  mpu->data_accel.vect.z = Int16FromBuf(mpu->i2c_trans.buf, 5);
118  mpu->data_rates.rates.p = Int16FromBuf(mpu->i2c_trans.buf, 9);
119  mpu->data_rates.rates.q = Int16FromBuf(mpu->i2c_trans.buf, 11);
120  mpu->data_rates.rates.r = Int16FromBuf(mpu->i2c_trans.buf, 13);
121 
122  // if we are reading slaves through the mpu, copy the ext_sens_data
123  if ((mpu->config.i2c_bypass == FALSE) && (mpu->config.nb_slaves > 0)) {
124  /* the buffer is volatile, since filled from ISR
125  * but we know it's ok to use it here so we silence the warning
126  */
127 #pragma GCC diagnostic push
128 #pragma GCC diagnostic ignored "-Wcast-qual"
129  memcpy(mpu->data_ext, (uint8_t *) & (mpu->i2c_trans.buf[15]), mpu->config.nb_bytes - 15);
130 #pragma GCC diagnostic pop
131  }
132 
133  mpu->data_available = true;
134  }
136  }
137  } else if (mpu->config.init_status != MPU9250_CONF_UNINIT) { // Configuring but not yet initialized
138  switch (mpu->i2c_trans.status) {
139  case I2CTransFailed:
140  mpu->config.init_status--; // Retry config (TODO max retry)
141  /* Falls through. */
142  case I2CTransSuccess:
143  case I2CTransDone:
144  mpu9250_send_config(mpu9250_i2c_write_to_reg, (void *)mpu, &(mpu->config));
145  if (mpu->config.initialized) {
146  mpu->i2c_trans.status = I2CTransDone;
147  }
148  break;
149  default:
150  break;
151  }
152  }
153 #if IMU_MPU9250_READ_MAG
154  // Ak8963 event function
155  ak8963_event(&mpu->akm);
156 #endif
157 }
158 
162 bool imu_mpu9250_configure_mag_slave(Mpu9250ConfigSet mpu_set __attribute__((unused)), void *mpu)
163 {
164  struct Mpu9250_I2c *mpu_i2c = (struct Mpu9250_I2c *)(mpu);
165 
166  ak8963_configure(&mpu_i2c->akm);
167  if (mpu_i2c->akm.initialized) {
168  return true;
169  } else {
170  return false;
171  }
172 }
173 
176 {
177  struct Mpu9250_I2c *mpu_i2c = (struct Mpu9250_I2c *)(mpu);
178 
179  if (mpu_i2c->slave_init_status == MPU9250_I2C_CONF_UNINIT) {
180  mpu_i2c->slave_init_status++;
181  }
182 
183  switch (mpu_i2c->slave_init_status) {
185  mpu_set(mpu, MPU9250_REG_USER_CTRL, 0);
186  mpu_i2c->slave_init_status++;
187  break;
189  /* switch to I2C passthrough */
190  mpu_set(mpu, MPU9250_REG_INT_PIN_CFG, (1 << 1));
191  mpu_i2c->slave_init_status++;
192  break;
194  /* configure each slave until all nb_slaves are done */
195  if (mpu_i2c->config.nb_slave_init < mpu_i2c->config.nb_slaves && mpu_i2c->config.nb_slave_init < MPU9250_I2C_NB_SLAVES) {
196  // proceed to next slave if configure for current one returns true
197  if (mpu_i2c->config.slaves[mpu_i2c->config.nb_slave_init].configure(mpu_set, mpu)) {
198  mpu_i2c->config.nb_slave_init++;
199  }
200  } else {
201  /* all slave devies configured, continue MPU side configuration of I2C slave stuff */
202  mpu_i2c->slave_init_status++;
203  }
204  break;
206  if (mpu_i2c->config.i2c_bypass) {
207  /* if bypassing I2C skip MPU I2C master setup */
209  } else {
210  /* disable I2C passthrough again */
211  mpu_set(mpu, MPU9250_REG_INT_PIN_CFG, (0 << 1));
212  mpu_i2c->slave_init_status++;
213  }
214  break;
216  /* configure MPU I2C master clock and stop/start between slave reads */
217  mpu_set(mpu, MPU9250_REG_I2C_MST_CTRL,
218  ((1 << 4) | mpu_i2c->config.i2c_mst_clk));
219  mpu_i2c->slave_init_status++;
220  break;
222  /* Set I2C slaves delayed sample rate */
223  mpu_set(mpu, MPU9250_REG_I2C_MST_DELAY, mpu_i2c->config.i2c_mst_delay);
224  mpu_i2c->slave_init_status++;
225  break;
227  /* I2C slave0 sample rate/2 = 100/2 = 50Hz */
228  mpu_set(mpu, MPU9250_REG_I2C_SLV4_CTRL, 0);
229  mpu_i2c->slave_init_status++;
230  break;
232  /* enable internal I2C master */
233  mpu_set(mpu, MPU9250_REG_USER_CTRL, (1 << MPU9250_I2C_MST_EN));
234  mpu_i2c->slave_init_status++;
235  break;
237  return true;
238  default:
239  break;
240  }
241  return false;
242 }
void ak8963_event(struct Ak8963 *ak)
Definition: ak8963.c:95
void ak8963_configure(struct Ak8963 *ak)
Definition: ak8963.c:46
void ak8963_init(struct Ak8963 *ak, struct i2c_periph *i2c_p, uint8_t addr)
Initialize AK8963 struct.
Definition: ak8963.c:34
void ak8963_read(struct Ak8963 *ak)
Definition: ak8963.c:83
bool initialized
config done flag
Definition: ak8963.h:56
volatile uint8_t buf[I2C_BUF_LEN]
Transaction buffer With I2C_BUF_LEN number of bytes.
Definition: i2c.h:122
enum I2CTransactionStatus status
Transaction status.
Definition: i2c.h:126
uint8_t slave_addr
Slave address.
Definition: i2c.h:104
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
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
@ I2CTransSuccess
transaction successfully finished by I2C driver
Definition: i2c.h:57
@ I2CTransFailed
transaction failed
Definition: i2c.h:58
@ I2CTransDone
transaction set to done by user level
Definition: i2c.h:59
void mpu9250_send_config(Mpu9250ConfigSet mpu_set, void *mpu, struct Mpu9250Config *config)
Configuration sequence called once before normal use.
Definition: mpu9250.c:89
void mpu9250_set_default_config(struct Mpu9250Config *c)
Definition: mpu9250.c:68
Mpu9250I2cSlaveConfigure configure
Definition: mpu9250.h:124
void(* Mpu9250ConfigSet)(void *mpu, uint8_t _reg, uint8_t _val)
Configuration function prototype.
Definition: mpu9250.h:118
uint8_t nb_slave_init
number of already configured/initialized slaves
Definition: mpu9250.h:145
uint8_t i2c_mst_delay
MPU I2C slaves delayed sample rate.
Definition: mpu9250.h:148
enum Mpu9250MstClk i2c_mst_clk
MPU I2C master clock speed.
Definition: mpu9250.h:147
uint8_t nb_slaves
number of used I2C slaves
Definition: mpu9250.h:144
uint8_t nb_bytes
number of bytes to read starting with MPU9250_REG_INT_STATUS
Definition: mpu9250.h:135
#define MPU9250_I2C_NB_SLAVES
Definition: mpu9250.h:53
bool i2c_bypass
Bypass MPU I2C.
Definition: mpu9250.h:142
enum Mpu9250ConfStatus init_status
init status
Definition: mpu9250.h:136
struct Mpu9250I2cSlave slaves[MPU9250_I2C_NB_SLAVES]
I2C slaves.
Definition: mpu9250.h:146
@ MPU9250_CONF_UNINIT
Definition: mpu9250.h:103
bool initialized
config done flag
Definition: mpu9250.h:137
void mpu9250_i2c_event(struct Mpu9250_I2c *mpu)
Definition: mpu9250_i2c.c:106
bool imu_mpu9250_configure_mag_slave(Mpu9250ConfigSet mpu_set, void *mpu)
callback function to configure ak8963 mag
Definition: mpu9250_i2c.c:162
static void mpu9250_i2c_write_to_reg(void *mpu, uint8_t _reg, uint8_t _val)
Definition: mpu9250_i2c.c:68
void mpu9250_i2c_read(struct Mpu9250_I2c *mpu)
Definition: mpu9250_i2c.c:87
void mpu9250_i2c_init(struct Mpu9250_I2c *mpu, struct i2c_periph *i2c_p, uint8_t addr)
Definition: mpu9250_i2c.c:33
void mpu9250_i2c_start_configure(struct Mpu9250_I2c *mpu)
Definition: mpu9250_i2c.c:77
bool mpu9250_configure_i2c_slaves(Mpu9250ConfigSet mpu_set, void *mpu)
configure the registered I2C slaves
Definition: mpu9250_i2c.c:175
#define Int16FromBuf(_buf, _idx)
Definition: mpu9250_i2c.c:104
Driver for the MPU-9250 using I2C.
struct Mpu9250Config config
Definition: mpu9250_i2c.h:72
uint8_t data_ext[MPU9250_BUFFER_EXT_LEN]
Definition: mpu9250_i2c.h:71
struct i2c_transaction i2c_trans
Definition: mpu9250_i2c.h:61
struct Ak8963 akm
"internal" magnetometer
Definition: mpu9250_i2c.h:75
union Mpu9250_I2c::@334 data_rates
union Mpu9250_I2c::@333 data_accel
enum Mpu9250I2cSlaveInitStatus slave_init_status
Definition: mpu9250_i2c.h:73
struct i2c_periph * i2c_p
Definition: mpu9250_i2c.h:60
@ MPU9250_I2C_CONF_DONE
Definition: mpu9250_i2c.h:56
@ MPU9250_I2C_CONF_I2C_SMPLRT
Definition: mpu9250_i2c.h:54
@ MPU9250_I2C_CONF_I2C_BYPASS_EN
Definition: mpu9250_i2c.h:49
@ MPU9250_I2C_CONF_UNINIT
Definition: mpu9250_i2c.h:47
@ MPU9250_I2C_CONF_I2C_MST_DELAY
Definition: mpu9250_i2c.h:53
@ MPU9250_I2C_CONF_I2C_MST_EN
Definition: mpu9250_i2c.h:55
@ MPU9250_I2C_CONF_I2C_MST_DIS
Definition: mpu9250_i2c.h:48
@ MPU9250_I2C_CONF_I2C_BYPASS_DIS
Definition: mpu9250_i2c.h:51
@ MPU9250_I2C_CONF_I2C_MST_CLK
Definition: mpu9250_i2c.h:52
@ MPU9250_I2C_CONF_SLAVES_CONFIGURE
Definition: mpu9250_i2c.h:50
volatile bool data_available
data ready flag
Definition: mpu9250_i2c.h:62
#define MPU9250_REG_I2C_MST_DELAY
Definition: mpu9250_regs.h:62
#define MPU9250_REG_I2C_MST_CTRL
Definition: mpu9250_regs.h:60
#define MPU9250_I2C_MST_EN
Definition: mpu9250_regs.h:131
#define MPU9250_REG_INT_PIN_CFG
Definition: mpu9250_regs.h:91
#define MPU9250_REG_USER_CTRL
Definition: mpu9250_regs.h:41
#define MPU9250_MAG_ADDR
Definition: mpu9250_regs.h:35
#define MPU9250_REG_I2C_SLV4_CTRL
Definition: mpu9250_regs.h:87
#define MPU9250_REG_INT_STATUS
Definition: mpu9250_regs.h:93
#define FALSE
Definition: std.h:5
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98