drv_mpu6050.c
Go to the documentation of this file.
1 /*
2  drv_mpu6050.c : driver for Invensense MPU6050
3 
4  Adapted from https://github.com/multiwii/baseflight/blob/master/src/drv_mpu.c
5 
6  This file is part of BreezySTM32.
7 
8  BreezySTM32 is free software: you can redistribute it and/or modify
9  it under the terms of the GNU General Public License as published by
10  the Free Software Foundation, either version 3 of the License, or
11  (at your option) any later version.
12 
13  BreezySTM32 is distributed in the hope that it will be useful,
14  but WITHOUT ANY WARRANTY; without even the implied warranty of
15  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  GNU General Public License for more details.
17 
18  You should have received a copy of the GNU General Public License
19  along with BreezySTM32. If not, see <http://www.gnu.org/licenses/>.
20  */
21 
22 
23 #include <breezystm32.h>
24 
25 #include <math.h>
26 
27 #ifndef M_PI
28 #define M_PI 3.14159
29 #endif
30 
31 /* Generic driver for invensense gyro/acc devices.
32  *
33  * Supported hardware:
34  * MPU6050 (gyro + acc)
35  *
36  * AUX_I2C is enabled on devices which have bypass, to allow forwarding to compass in MPU9150-style devices
37  */
38 
39 // This is generally where all Invensense devices are at, for default (AD0 down) I2C address
40 #define MPU_ADDRESS (0x68)
41 
42 #define GYRO_INT_PIN (Pin_13)
43 
44 #define MPU_RA_WHO_AM_I (0x75)
45 #define MPU_RA_GYRO_XOUT_H (0x43)
46 #define MPU_RA_ACCEL_XOUT_H (0x3B)
47 #define MPU_RA_TEMP_OUT_A (0x41)
48 // For debugging/identification purposes
49 #define MPU_RA_XA_OFFS_H (0x06) //[15:0] XA_OFFS
50 #define MPU_RA_PRODUCT_ID (0x0C) // Product ID Register
51 
52 // WHO_AM_I register contents for 6050
53 #define MPUx0x0_WHO_AM_I_CONST (0x68)
54 
55 enum lpf_e {
65 };
66 
67 enum gyro_fsr_e {
73 };
74 
79 };
80 
87 };
88 
89 
90 // Lowpass
92 
93 // Timestamp
94 static uint64_t imu_time_us = 0;
95 static bool new_imu_data = false;
96 
97 // MPU6xxx registers
98 #define MPU_RA_SMPLRT_DIV 0x19
99 #define MPU_RA_CONFIG 0x1A
100 #define MPU_RA_GYRO_CONFIG 0x1B
101 #define MPU_RA_ACCEL_CONFIG 0x1C
102 #define MPU_RA_FIFO_EN 0x23
103 #define MPU_RA_I2C_MST_CTRL 0x24
104 #define MPU_RA_INT_PIN_CFG 0x37
105 #define MPU_RA_INT_ENABLE 0x38
106 #define MPU_RA_SIGNAL_PATH_RST 0x68
107 #define MPU_RA_USER_CTRL 0x6A
108 #define MPU_RA_PWR_MGMT_1 0x6B
109 #define MPU_RA_PWR_MGMT_2 0x6C
110 #define MPU_RA_FIFO_COUNT_H 0x72
111 #define MPU_RA_FIFO_R_W 0x74
112 
113 // MPU6050 bits
114 #define MPU6050_INV_CLK_GYROZ 0x03
115 #define MPU6050_BIT_FIFO_RST 0x04
116 #define MPU6050_BIT_DMP_RST 0x08
117 #define MPU6050_BIT_FIFO_EN 0x40
118 
119 static bool mpuReadRegisterI2C(uint8_t reg, uint8_t *data, int length)
120 {
121  return i2cRead(MPU_ADDRESS, reg, length, data);
122 }
123 
124 static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data)
125 {
126  return i2cWrite(MPU_ADDRESS, reg, data);
127 }
128 
129 void mpu6050_exti_init(int boardVersion)
130 {
131  // enable AFIO for EXTI support
133 
134  // Configure EXTI
136  EXTI_InitTypeDef EXTI_InitStructure;
137  // GPIO Structure Used To initialize external interrupt pin
138  // This assumes that the interrupt pin is attached to pin 26 (PB13)
139  // Which is not be the case for all boards. The naze32 rev5+ has it's
140  // interrupt on PC13, while rev4- and the flip32 devices use PB13.
141  // see src/main/sensors/initializiation.c:85 in the cleanflight source code
142  // for their version handling.
143  if (boardVersion > 4) {
145  } else {
147  }
148 
149  // Configure EXTI Line13
150  EXTI_InitStructure.EXTI_Line = EXTI_Line13;
151  EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
152  EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising;
153  EXTI_InitStructure.EXTI_LineCmd = ENABLE;
154  EXTI_Init(&EXTI_InitStructure);
155 
156  // Disable AFIO Clock - we don't need it anymore
158 
159  // Configure NVIC (Nested Vector Interrupt Controller)
160  NVIC_InitTypeDef NVIC_InitStructure;
161  // Select NVIC Channel to configure
162  NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn;
163  // Set priority to lowest
164  NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 7;
165  // Set subpriority to lowest
166  NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
167  NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
168  // Update NVIC registers
169  NVIC_Init(&NVIC_InitStructure);
170  // NVIC_SetPriority(EXTI15_10_IRQn, 1);
171 }
172 
173 
174 // ======================================================================
175 void mpu6050_init(bool enableInterrupt, uint16_t * acc1G, float * gyroScale, int boardVersion)
176 {
177  gpio_config_t gpio;
178 
179  // Set acc1G. Modified once by mpu6050CheckRevision for old (hopefully nonexistent outside of clones) parts
180  *acc1G = 512 * 8;
181 
182  // 16.4 dps/lsb scalefactor for all Invensense devices
183  *gyroScale = (1.0f / 16.4f) * (M_PI / 180.0f);
184 
185  // MPU_INT output on rev5+ hardware (PC13)
186  if (enableInterrupt) {
187  gpio.pin = Pin_13;
188  gpio.speed = Speed_2MHz;
189  gpio.mode = Mode_IN_FLOATING;
190  if (boardVersion > 4){
191  gpioInit(GPIOC, &gpio);
192  } else {
193  gpioInit(GPIOB, &gpio);
194  }
195  mpu6050_exti_init(boardVersion);
196  }
197 
198  // Device reset
199  mpuWriteRegisterI2C(MPU_RA_PWR_MGMT_1, 0x80); // Device reset
200  delay(30); // wait for reboot
201 
202  // Gyro config
203  mpuWriteRegisterI2C(MPU_RA_SMPLRT_DIV, 0x00); // Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
204  mpuWriteRegisterI2C(MPU_RA_PWR_MGMT_1, MPU6050_INV_CLK_GYROZ); // Clock source = 3 (PLL with Z Gyro reference)
206  mpuWriteRegisterI2C(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); // full-scale 2kdps gyro range
207 
208  // Accel scale 8g (4096 LSB/g)
210 
211  // Data ready interrupt configuration: INT_RD_CLEAR_DIS, I2C_BYPASS_EN
212  mpuWriteRegisterI2C(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0);
213  mpuWriteRegisterI2C(MPU_RA_INT_ENABLE, 0x01); // DATA_RDY_EN interrupt enable
214 }
215 
216 void mpu6050_read_all(int16_t *accData, int16_t *gyroData, int16_t* tempData, uint64_t *time_us)
217 {
218  uint8_t buf[14];
219 
221 
222  accData[0] = (int16_t)((buf[0] << 8) | buf[1]);
223  accData[1] = (int16_t)((buf[2] << 8) | buf[3]);
224  accData[2] = (int16_t)((buf[4] << 8) | buf[5]);
225 
226  (*tempData) = (int16_t)((buf[6] << 8) | buf[7]);
227 
228  gyroData[0] = (int16_t)((buf[8] << 8) | buf[9]);
229  gyroData[1] = (int16_t)((buf[10] << 8) | buf[11]);
230  gyroData[2] = (int16_t)((buf[12] << 8) | buf[13]);
231 
232  (*time_us) = imu_time_us;
233 }
234 
235 
236 void mpu6050_read_accel(int16_t *accData)
237 {
238  uint8_t buf[6];
239 
241  {
242 
243  accData[0] = (int16_t)((buf[0] << 8) | buf[1]);
244  accData[1] = (int16_t)((buf[2] << 8) | buf[3]);
245  accData[2] = (int16_t)((buf[4] << 8) | buf[5]);
246  }
247 }
248 
249 
250 void mpu6050_read_gyro(int16_t *gyroData)
251 {
252  uint8_t buf[6];
253 
255 
256  gyroData[0] = (int16_t)((buf[0] << 8) | buf[1]);
257  gyroData[1] = (int16_t)((buf[2] << 8) | buf[3]);
258  gyroData[2] = (int16_t)((buf[4] << 8) | buf[5]);
259 }
260 
261 void mpu6050_read_temperature(int16_t *tempData)
262 {
263  uint8_t buf[2];
264 
266 
267  *tempData = (int16_t)((buf[0] << 8) | buf[1]) / 4;
268 }
269 
270 
271 /*=======================================================
272  * Asynchronous I2C Read Functions:
273  * These methods use the asynchronous I2C
274  * read capability on the naze32.
275  */
276 static volatile int16_t accel[3];
277 static volatile int16_t gyro[3];
278 static volatile int16_t temp;
279 static volatile bool need_to_queue_new_i2c_job = false;
280 uint8_t all_buffer[14];
281 uint64_t measurement_time = 0;
282 
283 void read_all_CB(uint8_t result)
284 {
285  (void) result;
286  accel[0] = (int16_t)((all_buffer[0] << 8) | all_buffer[1]);
287  accel[1] = (int16_t)((all_buffer[2] << 8) | all_buffer[3]);
288  accel[2] = (int16_t)((all_buffer[4] << 8) | all_buffer[5]);
289 
290  temp = (int16_t)((all_buffer[6] << 8) | all_buffer[7]);
291 
292  gyro[0] = (int16_t)((all_buffer[8] << 8) | all_buffer[9]);
293  gyro[1] = (int16_t)((all_buffer[10] << 8) | all_buffer[11]);
294  gyro[2] = (int16_t)((all_buffer[12] << 8) | all_buffer[13]);
295 
296  new_imu_data = true;
298 }
299 
301 {
302  // Adds a new i2c job to the I2C job queue.
303  // Current status of the job can be read by polling the
304  // status variable, and the callback will be called when the function
305  // is finished
307  MPU_ADDRESS,
309  all_buffer,
310  14,
311  NULL,
312  &read_all_CB);
313 }
314 
315 
316 void mpu6050_async_read_all(volatile int16_t *accData, volatile int16_t *tempData, volatile int16_t *gyroData, volatile uint64_t* timeData)
317 {
318  accData[0] = accel[0];
319  accData[1] = accel[1];
320  accData[2] = accel[2];
321  (*tempData) = temp;
322  gyroData[0] = gyro[0];
323  gyroData[1] = gyro[1];
324  gyroData[2] = gyro[2];
325  (*timeData) = measurement_time;
326 }
327 
329 {
331  {
334  }
335 
336  if (new_imu_data)
337  {
338  new_imu_data = false;
339  return true;
340  }
341  return false;
342 }
343 
344 //=====================================================================================
345 
347 {
349  {
350  imu_time_us = micros();
352  }
354 }
355 
356 
357 
FunctionalState EXTI_LineCmd
uint8_t all_buffer[14]
Definition: drv_mpu6050.c:280
EXTITrigger_TypeDef EXTI_Trigger
uint8_t * data
Definition: drv_i2c.h:47
#define MPU_RA_SMPLRT_DIV
Definition: drv_mpu6050.c:98
void EXTI15_10_IRQHandler(void)
Definition: drv_mpu6050.c:346
static volatile int16_t gyro[3]
Definition: drv_mpu6050.c:277
void NVIC_Init(NVIC_InitTypeDef *NVIC_InitStruct)
Initializes the NVIC peripheral according to the specified parameters in the NVIC_InitStruct.
bool i2cRead(uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t *buf)
Definition: drv_i2c.c:157
GPIO_Mode mode
Definition: drv_gpio.h:63
EXTI Init Structure definition.
GPIO_Speed speed
Definition: drv_gpio.h:64
uint64_t measurement_time
Definition: drv_mpu6050.c:281
void mpu6050_request_async_update_all()
Definition: drv_mpu6050.c:300
#define GPIOB
Definition: stm32f4xx.h:2111
static uint64_t imu_time_us
Definition: drv_mpu6050.c:94
static bool new_imu_data
Definition: drv_mpu6050.c:95
#define MPU_RA_GYRO_CONFIG
Definition: drv_mpu6050.c:100
#define MPU_RA_INT_PIN_CFG
Definition: drv_mpu6050.c:104
void RCC_APB2PeriphClockCmd(uint32_t RCC_APB2Periph, FunctionalState NewState)
Enables or disables the High Speed APB (APB2) peripheral clock.
void mpu6050_async_read_all(volatile int16_t *accData, volatile int16_t *tempData, volatile int16_t *gyroData, volatile uint64_t *timeData)
Definition: drv_mpu6050.c:316
#define GPIOC
Definition: stm32f4xx.h:2112
#define MPU_RA_GYRO_XOUT_H
Definition: drv_mpu6050.c:45
clock_sel_e
Definition: drv_mpu6050.c:75
void gpioInit(GPIO_TypeDef *gpio, gpio_config_t *config)
Definition: drv_gpio.c:56
volatile uint64_t micros(void)
Definition: system.c:44
void mpu6050_init(bool enableInterrupt, uint16_t *acc1G, float *gyroScale, int boardVersion)
Definition: drv_mpu6050.c:175
ITStatus EXTI_GetITStatus(uint32_t EXTI_Line)
Checks whether the specified EXTI line is asserted or not.
void mpu6050_read_all(int16_t *accData, int16_t *gyroData, int16_t *tempData, uint64_t *time_us)
Definition: drv_mpu6050.c:216
accel_fsr_e
Definition: drv_mpu6050.c:81
lpf_e
Definition: drv_mpu6050.c:55
void EXTI_Init(EXTI_InitTypeDef *EXTI_InitStruct)
Initializes the EXTI peripheral according to the specified parameters in the EXTI_InitStruct. EXTI_Line specifies the EXTI line (EXTI0....EXTI35). EXTI_Mode specifies which EXTI line is used as interrupt or an event. EXTI_Trigger selects the trigger. When the trigger occurs, interrupt pending bit will be set. EXTI_LineCmd controls (Enable/Disable) the EXTI line.
uint16_t pin
Definition: drv_gpio.h:62
bool i2cWrite(uint8_t addr_, uint8_t reg_, uint8_t data)
Definition: drv_i2c.c:152
gyro_fsr_e
Definition: drv_mpu6050.c:67
EXTIMode_TypeDef EXTI_Mode
static volatile int16_t accel[3]
Definition: drv_mpu6050.c:276
void mpu6050_read_gyro(int16_t *gyroData)
Definition: drv_mpu6050.c:250
bool mpu6050_new_data()
Definition: drv_mpu6050.c:328
void mpu6050_read_temperature(int16_t *tempData)
Definition: drv_mpu6050.c:261
#define GPIO_PortSourceGPIOB
static volatile int16_t temp
Definition: drv_mpu6050.c:278
#define RCC_APB2Periph_AFIO
static volatile uint8_t reg
Definition: drv_i2c.c:96
#define EXTI_Line13
#define MPU6050_INV_CLK_GYROZ
Definition: drv_mpu6050.c:114
static uint8_t mpuLowPassFilter
Definition: drv_mpu6050.c:91
void mpu6050_exti_init(int boardVersion)
Definition: drv_mpu6050.c:129
uint8_t length
Definition: drv_i2c.h:48
void i2c_queue_job(i2cJobType_t type, uint8_t addr_, uint8_t reg_, uint8_t *data, uint8_t length, volatile uint8_t *status_, void(*CB)(uint8_t))
Definition: drv_i2c.c:579
#define MPU_RA_TEMP_OUT_A
Definition: drv_mpu6050.c:47
void gpioExtiLineConfig(uint8_t portsrc, uint8_t pinsrc)
Definition: drv_gpio.c:88
#define MPU_RA_ACCEL_XOUT_H
Definition: drv_mpu6050.c:46
static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data)
Definition: drv_mpu6050.c:124
#define MPU_ADDRESS
Definition: drv_mpu6050.c:40
#define MPU_RA_ACCEL_CONFIG
Definition: drv_mpu6050.c:101
static bool mpuReadRegisterI2C(uint8_t reg, uint8_t *data, int length)
Definition: drv_mpu6050.c:119
#define MPU_RA_PWR_MGMT_1
Definition: drv_mpu6050.c:108
void EXTI_ClearITPendingBit(uint32_t EXTI_Line)
Clears the EXTI&#39;s line pending bits.
Definition: drv_i2c.h:31
#define NULL
Definition: usbd_def.h:50
#define M_PI
Definition: drv_mpu6050.c:28
#define GPIO_PortSourceGPIOC
void read_all_CB(uint8_t result)
Definition: drv_mpu6050.c:283
static volatile bool need_to_queue_new_i2c_job
Definition: drv_mpu6050.c:279
#define MPU_RA_INT_ENABLE
Definition: drv_mpu6050.c:105
void mpu6050_read_accel(int16_t *accData)
Definition: drv_mpu6050.c:236
void delay(uint32_t ms)
Definition: system.c:98
#define MPU_RA_CONFIG
Definition: drv_mpu6050.c:99


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Thu Oct 24 2019 03:17:18