mpu6000.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017, James Jackson
3  *
4  * All rights reserved.
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions are met:
8  *
9  * * Redistributions of source code must retain the above copyright notice, this
10  * list of conditions and the following disclaimer.
11  *
12  * * Redistributions in binary form must reproduce the above copyright notice,
13  * this list of conditions and the following disclaimer in the documentation
14  * and/or other materials provided with the distribution.
15  *
16  * * Neither the name of the copyright holder nor the names of its
17  * contributors may be used to endorse or promote products derived from
18  * this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
24  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
28  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
29  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30  */
31 
32 #include "mpu6000.h"
33 
34 uint8_t raw[15] = {MPU_RA_ACCEL_XOUT_H | 0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
35 
37 
38 void data_transfer_cb(void)
39 {
40  IMU_Ptr->data_transfer_callback();
41 }
42 
43 void MPU6000::write(uint8_t reg, uint8_t data)
44 {
45  spi->enable(cs_);
46  spi->transfer_byte(reg);
47  spi->transfer_byte(data);
48  spi->disable(cs_);
50 }
51 
52 void MPU6000::init(SPI* spi_drv)
53 {
54  IMU_Ptr = this;
55  spi = spi_drv;
56 
57  // Configure Chip Select Pin
59  spi->set_divisor(2); // 21 MHz SPI clock (within 20 +/- 10%)
60 
62  delay(150);
63 
66  write(MPU_RA_PWR_MGMT_2, 0x00);
67  write(MPU_RA_SMPLRT_DIV, 0x00);
71  write(MPU_RA_INT_ENABLE, 0x01);
72 
73  for (int i = 0; i < 100; i++)
74  // Read the IMU
75  raw[0] = MPU_RA_TEMP_OUT_H | 0x80;
76  spi->transfer(raw, 3, raw, &cs_);
77  delay(20);
78  // while (spi->is_busy()) {}
79 
80  // Set up the EXTI pin
82 
84 
85  EXTI_InitTypeDef EXTI_InitStruct;
86  EXTI_InitStruct.EXTI_Line = EXTI_Line4;
87  EXTI_InitStruct.EXTI_LineCmd = ENABLE;
88  EXTI_InitStruct.EXTI_Mode = EXTI_Mode_Interrupt;
89  EXTI_InitStruct.EXTI_Trigger = EXTI_Trigger_Rising;
90  EXTI_Init(&EXTI_InitStruct);
91 
92  NVIC_InitTypeDef NVIC_InitStruct;
93  NVIC_InitStruct.NVIC_IRQChannel = EXTI4_IRQn;
94  NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0x03;
95  NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0x03;
96  NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;
97  NVIC_Init(&NVIC_InitStruct);
98 
99  // set the accel and gyro scale parameters
100  accel_scale_ = (4.0 * 9.80665f) / (static_cast<float>(0x7FFF));
101  gyro_scale_ = (2000.0 * 3.14159f / 180.0f) / (static_cast<float>(0x7FFF));
102 }
103 
105 {
106  new_data_ = true;
107  acc_[0] = static_cast<float>(static_cast<int16_t>((raw[1] << 8) | raw[2])) * accel_scale_;
108  acc_[1] = static_cast<float>(static_cast<int16_t>((raw[3] << 8) | raw[4])) * accel_scale_;
109  acc_[2] = static_cast<float>(static_cast<int16_t>((raw[5] << 8) | raw[6])) * accel_scale_;
110 
111  temp_ = static_cast<float>(static_cast<int16_t>((raw[7] << 8) | raw[8])) / 340.0f + 36.53f;
112 
113  gyro_[0] = static_cast<float>(static_cast<int16_t>((raw[9] << 8) | raw[10])) * gyro_scale_;
114  gyro_[1] = static_cast<float>(static_cast<int16_t>((raw[11] << 8) | raw[12])) * gyro_scale_;
115  gyro_[2] = static_cast<float>(static_cast<int16_t>((raw[13] << 8) | raw[14])) * gyro_scale_;
116 }
117 
118 void MPU6000::read(float* accel_data, float* gyro_data, float* temp_data, uint64_t* time_us)
119 {
120  accel_data[0] = acc_[0];
121  accel_data[1] = acc_[1];
122  accel_data[2] = acc_[2];
123  gyro_data[0] = gyro_[0];
124  gyro_data[1] = gyro_[1];
125  gyro_data[2] = gyro_[2];
126  *temp_data = temp_;
127  *time_us = imu_timestamp_;
128 }
129 
131 {
133  raw[0] = MPU_RA_ACCEL_XOUT_H | 0x80;
134  spi->transfer(raw, 15, raw, &cs_, &data_transfer_cb);
135 }
136 
138 {
139  if (new_data_)
140  {
141  new_data_ = false;
142  return true;
143  }
144  else
145  return false;
146 }
147 
148 extern "C"
149 {
150  void EXTI4_IRQHandler(void)
151  {
153  IMU_Ptr->exti_cb();
154  }
155 }
FunctionalState EXTI_LineCmd
#define MPU_BITS_FS_2000DPS
Definition: mpu6000.h:48
EXTITrigger_TypeDef EXTI_Trigger
#define MPU_CLK_SEL_PLLGYROZ
Definition: mpu6000.h:43
#define MPU_RA_SMPLRT_DIV
Definition: mpu6000.h:90
#define MPU_RA_PWR_MGMT_1
Definition: mpu6000.h:148
void NVIC_Init(NVIC_InitTypeDef *NVIC_InitStruct)
Initializes the NVIC peripheral according to the specified parameters in the NVIC_InitStruct.
EXTI Init Structure definition.
#define MPU6000_CS_GPIO
Definition: revo_f4.h:107
#define MPU_RA_PWR_MGMT_2
Definition: mpu6000.h:149
Definition: spi.h:37
void delayMicroseconds(uint32_t us)
Definition: system.c:94
void exti_cb()
Definition: mpu6000.cpp:130
GPIO exti_
Definition: mpu6000.h:176
void enable(GPIO &cs)
Definition: spi.cpp:150
bool transfer(uint8_t *out_data, uint32_t num_bytes, uint8_t *in_data, GPIO *cs=NULL, void(*cb)(void)=NULL)
Definition: spi.cpp:206
bool new_data_
Definition: mpu6000.h:173
GPIO cs_
Definition: mpu6000.h:177
#define GPIOC
Definition: stm32f4xx.h:2112
volatile uint64_t micros(void)
Definition: system.c:44
volatile int16_t temp_data
Definition: accelgyro.c:31
#define MPU_RA_GYRO_CONFIG
Definition: mpu6000.h:92
#define MPU_BIT_H_RESET
Definition: mpu6000.h:40
void init(GPIO_TypeDef *BasePort, uint16_t pin, gpio_mode_t mode)
Definition: gpio.cpp:34
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.
float accel_scale_
Definition: mpu6000.h:178
EXTIMode_TypeDef EXTI_Mode
#define EXTI_Line4
void init(SPI *spi_drv)
Definition: mpu6000.cpp:52
#define MPU_RA_INT_ENABLE
Definition: mpu6000.h:121
#define MPU_RA_ACCEL_XOUT_H
Definition: mpu6000.h:124
#define MPU_RA_USER_CTRL
Definition: mpu6000.h:147
static volatile uint8_t reg
Definition: drv_i2c.c:96
volatile int16_t accel_data[3]
Definition: accelgyro.c:29
float gyro_scale_
Definition: mpu6000.h:179
uint64_t imu_timestamp_
Definition: mpu6000.h:174
SPI * spi
Definition: mpu6000.h:175
void data_transfer_callback()
Definition: mpu6000.cpp:104
#define EXTI_PinSource4
void EXTI4_IRQHandler(void)
Definition: mpu6000.cpp:150
uint8_t transfer_byte(uint8_t data, GPIO *cs=NULL)
Definition: spi.cpp:160
#define MPU_BIT_I2C_IF_DIS
Definition: mpu6000.h:65
float temp_
Definition: mpu6000.h:182
void set_divisor(uint16_t new_divisor)
Definition: spi.cpp:109
#define MPU_BITS_FS_4G
Definition: mpu6000.h:50
#define MPU6000_CS_PIN
Definition: revo_f4.h:108
void SYSCFG_EXTILineConfig(uint8_t EXTI_PortSourceGPIOx, uint8_t EXTI_PinSourcex)
Selects the GPIO pin used as EXTI Line.
void EXTI_ClearITPendingBit(uint32_t EXTI_Line)
Clears the EXTI&#39;s line pending bits.
float gyro_[3]
Definition: mpu6000.h:181
#define EXTI_PortSourceGPIOC
#define MPU_RA_TEMP_OUT_H
Definition: mpu6000.h:130
void disable(GPIO &cs)
Definition: spi.cpp:155
void read(float *accel_data, float *gyro_data, float *temp_data, uint64_t *time_us)
Definition: mpu6000.cpp:118
float acc_[3]
Definition: mpu6000.h:180
void write(uint8_t reg, uint8_t data)
Definition: mpu6000.cpp:43
void data_transfer_cb(void)
Definition: mpu6000.cpp:38
MPU6000 * IMU_Ptr
Definition: mpu6000.cpp:36
uint8_t raw[15]
Definition: mpu6000.cpp:34
#define MPU_BITS_DLPF_CFG_98HZ
Definition: mpu6000.h:56
volatile int16_t gyro_data[3]
Definition: accelgyro.c:30
bool new_data()
Definition: mpu6000.cpp:137
#define MPU_RA_ACCEL_CONFIG
Definition: mpu6000.h:93
void delay(uint32_t ms)
Definition: system.c:101
#define MPU_RA_CONFIG
Definition: mpu6000.h:91


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Sat May 9 2020 03:16:53