PX4 Firmware
PX4 Autopilot Software http://px4.io
mpu9250_spi.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2016-2019 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file mpu9250_spi.cpp
36  *
37  * Driver for the Invensense MPU9250 connected via SPI.
38  *
39  * @author Andrew Tridgell
40  * @author Pat Hickey
41  * @author David sidrane
42  */
43 
44 #include <drivers/device/spi.h>
45 #include "mpu9250.h"
46 
47 #define DIR_READ 0x80
48 #define DIR_WRITE 0x00
49 
50 /*
51  * The MPU9250 can only handle high SPI bus speeds of 20Mhz on the sensor and
52  * interrupt status registers. All other registers have a maximum 1MHz
53  * SPI speed
54  *
55  * The Actual Value will be rounded down by the spi driver.
56  * for a 168Mhz CPU this will be 10.5 Mhz and for a 180 Mhz CPU
57  * it will be 11.250 Mhz
58  */
59 #define MPU9250_LOW_SPI_BUS_SPEED 1000*1000
60 #define MPU9250_HIGH_SPI_BUS_SPEED 20*1000*1000
61 
62 device::Device *MPU9250_SPI_interface(int bus, uint32_t cs);
63 
64 class MPU9250_SPI : public device::SPI
65 {
66 public:
67  MPU9250_SPI(int bus, uint32_t device);
68  ~MPU9250_SPI() override = default;
69 
70  int read(unsigned address, void *data, unsigned count) override;
71  int write(unsigned address, void *data, unsigned count) override;
72 
73 protected:
74  int probe() override;
75 
76 private:
77 
78  /* Helper to set the desired speed and isolate the register on return */
79  void set_bus_frequency(unsigned &reg_speed_reg_out);
80 };
81 
83 MPU9250_SPI_interface(int bus, uint32_t cs)
84 {
85  device::Device *interface = nullptr;
86 
87  interface = new MPU9250_SPI(bus, cs);
88 
89  return interface;
90 }
91 
92 MPU9250_SPI::MPU9250_SPI(int bus, uint32_t device) :
93  SPI("MPU9250", nullptr, bus, device, SPIDEV_MODE3, MPU9250_LOW_SPI_BUS_SPEED)
94 {
95  _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250;
96 }
97 
98 void
99 MPU9250_SPI::set_bus_frequency(unsigned &reg_speed)
100 {
101  /* Set the desired speed */
103 
104  /* Isoolate the register on return */
105  reg_speed = MPU9250_REG(reg_speed);
106 }
107 
108 int
109 MPU9250_SPI::write(unsigned reg_speed, void *data, unsigned count)
110 {
111  uint8_t cmd[2] {};
112 
113  if (sizeof(cmd) < (count + 1)) {
114  return -EIO;
115  }
116 
117  /* Set the desired speed and isolate the register */
118  set_bus_frequency(reg_speed);
119 
120  cmd[0] = reg_speed | DIR_WRITE;
121  cmd[1] = *(uint8_t *)data;
122 
123  return transfer(&cmd[0], &cmd[0], count + 1);
124 }
125 
126 int
127 MPU9250_SPI::read(unsigned reg_speed, void *data, unsigned count)
128 {
129  /* We want to avoid copying the data of MPUReport: So if the caller
130  * supplies a buffer not MPUReport in size, it is assume to be a reg or reg 16 read
131  * and we need to provied the buffer large enough for the callers data
132  * and our command.
133  */
134  uint8_t cmd[3] {};
135 
136  uint8_t *pbuff = count < sizeof(MPUReport) ? cmd : (uint8_t *) data ;
137 
138  if (count < sizeof(MPUReport)) {
139  /* add command */
140  count++;
141  }
142 
143  set_bus_frequency(reg_speed);
144 
145  /* Set command */
146  pbuff[0] = reg_speed | DIR_READ ;
147 
148  /* Transfer the command and get the data */
149  int ret = transfer(pbuff, pbuff, count);
150 
151  if (ret == OK && pbuff == &cmd[0]) {
152  /* Adjust the count back */
153  count--;
154 
155  /* Return the data */
156  memcpy(data, &cmd[1], count);
157  }
158 
159  return ret;
160 }
161 
162 int
164 {
165  uint8_t whoami = 0;
166 
167  int ret = read(MPUREG_WHOAMI, &whoami, 1);
168 
169  if (ret != OK) {
170  return -EIO;
171  }
172 
173  switch (whoami) {
174  case MPU_WHOAMI_9250:
175  case MPU_WHOAMI_6500:
176  ret = 0;
177  break;
178 
179  default:
180  PX4_WARN("probe failed! %u", whoami);
181  ret = -EIO;
182  }
183 
184  return ret;
185 }
void set_bus_frequency(unsigned &reg_speed_reg_out)
Definition: mpu9250_spi.cpp:99
MPU9250_SPI(int bus, uint32_t device)
Definition: mpu9250_spi.cpp:92
#define MPU_WHOAMI_9250
Definition: mpu9250.h:169
device::Device * MPU9250_SPI_interface(int bus, uint32_t cs)
Definition: mpu9250_spi.cpp:83
#define MPU9250_REG(r)
Definition: mpu9250.h:212
#define DIR_READ
Definition: mpu9250_spi.cpp:47
Namespace encapsulating all device framework classes, functions and data.
Definition: CDev.cpp:47
#define MPUREG_WHOAMI
Definition: icm20948.h:52
int probe() override
Report conversation within the mpu, including command byte and interrupt status.
Definition: icm20948.h:311
int write(unsigned address, void *data, unsigned count) override
#define DIR_WRITE
Definition: mpu9250_spi.cpp:48
uint8_t * data
Definition: dataman.cpp:149
#define MPU9250_LOW_SPI_BUS_SPEED
Definition: mpu9250_spi.cpp:59
~MPU9250_SPI() override=default
#define MPU9250_IS_HIGH_SPEED(r)
Definition: mpu9250.h:211
#define MPU_WHOAMI_6500
Definition: mpu9250.h:170
#define MPU9250_HIGH_SPI_BUS_SPEED
Definition: mpu9250_spi.cpp:60
Fundamental base class for all physical drivers (I2C, SPI).
Definition: Device.hpp:65
#define OK
Definition: uavcan_main.cpp:71
#define DRV_ACC_DEVTYPE_MPU9250
Definition: drv_sensor.h:69
int read(unsigned address, void *data, unsigned count) override