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 
74 
75  for (int i = 0; i < 100; i++)
76  // Read the IMU
77  raw[0] = MPU_RA_TEMP_OUT_H | 0x80;
78  spi->transfer(raw, 3, raw, &cs_);
79  delay(20);
80 // while (spi->is_busy()) {}
81 
82  // Set up the EXTI pin
84 
86 
87  EXTI_InitTypeDef EXTI_InitStruct;
88  EXTI_InitStruct.EXTI_Line = EXTI_Line4;
89  EXTI_InitStruct.EXTI_LineCmd = ENABLE;
90  EXTI_InitStruct.EXTI_Mode = EXTI_Mode_Interrupt;
91  EXTI_InitStruct.EXTI_Trigger = EXTI_Trigger_Rising;
92  EXTI_Init(&EXTI_InitStruct);
93 
94  NVIC_InitTypeDef NVIC_InitStruct;
95  NVIC_InitStruct.NVIC_IRQChannel = EXTI4_IRQn;
96  NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0x03;
97  NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0x03;
98  NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;
99  NVIC_Init(&NVIC_InitStruct);
100 
101 
102  // set the accel and gyro scale parameters
103  accel_scale_ = (4.0 * 9.80665f) / (static_cast<float>(0x7FFF));
104  gyro_scale_= (2000.0 * 3.14159f/180.0f) / (static_cast<float>(0x7FFF));
105 }
106 
108 {
109  new_data_ = true;
110  acc_[0] = static_cast<float>(static_cast<int16_t>((raw[1] << 8) | raw[2])) * accel_scale_;
111  acc_[1] = static_cast<float>(static_cast<int16_t>((raw[3] << 8) | raw[4])) * accel_scale_;
112  acc_[2] = static_cast<float>(static_cast<int16_t>((raw[5] << 8) | raw[6])) * accel_scale_;
113 
114  temp_ = static_cast<float>(static_cast<int16_t>((raw[7] << 8) | raw[8])) / 340.0f + 36.53f;
115 
116  gyro_[0] = static_cast<float>(static_cast<int16_t>((raw[9] << 8) | raw[10])) * gyro_scale_;
117  gyro_[1] = static_cast<float>(static_cast<int16_t>((raw[11] << 8) | raw[12])) * gyro_scale_;
118  gyro_[2] = static_cast<float>(static_cast<int16_t>((raw[13] << 8) | raw[14])) * gyro_scale_;
119 }
120 
121 void MPU6000::read(float* accel_data, float* gyro_data, float* temp_data, uint64_t* time_us)
122 {
123  accel_data[0] = acc_[0];
124  accel_data[1] = acc_[1];
125  accel_data[2] = acc_[2];
126  gyro_data[0] = gyro_[0];
127  gyro_data[1] = gyro_[1];
128  gyro_data[2] = gyro_[2];
129  *temp_data = temp_;
130  *time_us = imu_timestamp_;
131 }
132 
134 {
136  raw[0] = MPU_RA_ACCEL_XOUT_H | 0x80;
137  spi->transfer(raw, 15, raw, &cs_, &data_transfer_cb);
138 }
139 
141  {
142  if (new_data_)
143  {
144  new_data_ = false;
145  return true;
146  }
147  else
148  return false;
149  }
150 
151 extern "C"
152 {
153 
155 {
157  IMU_Ptr->exti_cb();
158 }
159 
160 }
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:111
#define MPU_RA_PWR_MGMT_2
Definition: mpu6000.h:149
Definition: spi.h:38
void delayMicroseconds(uint32_t us)
Definition: system.c:91
void exti_cb()
Definition: mpu6000.cpp:133
GPIO exti_
Definition: mpu6000.h:174
void enable(GPIO &cs)
Definition: spi.cpp:148
bool transfer(uint8_t *out_data, uint32_t num_bytes, uint8_t *in_data, GPIO *cs=NULL, void(*cb)(void)=NULL)
Definition: spi.cpp:203
bool new_data_
Definition: mpu6000.h:171
GPIO cs_
Definition: mpu6000.h:175
#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:176
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:177
uint64_t imu_timestamp_
Definition: mpu6000.h:172
SPI * spi
Definition: mpu6000.h:173
void data_transfer_callback()
Definition: mpu6000.cpp:107
#define EXTI_PinSource4
void EXTI4_IRQHandler(void)
Definition: mpu6000.cpp:154
uint8_t transfer_byte(uint8_t data, GPIO *cs=NULL)
Definition: spi.cpp:156
#define MPU_BIT_I2C_IF_DIS
Definition: mpu6000.h:65
float temp_
Definition: mpu6000.h:180
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:112
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:179
#define EXTI_PortSourceGPIOC
#define MPU_RA_TEMP_OUT_H
Definition: mpu6000.h:130
void disable(GPIO &cs)
Definition: spi.cpp:152
void read(float *accel_data, float *gyro_data, float *temp_data, uint64_t *time_us)
Definition: mpu6000.cpp:121
float acc_[3]
Definition: mpu6000.h:178
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:140
#define MPU_RA_ACCEL_CONFIG
Definition: mpu6000.h:93
void delay(uint32_t ms)
Definition: system.c:98
#define MPU_RA_CONFIG
Definition: mpu6000.h:91


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