fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 06:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Initial build for Crazyflie 2.0

Browse Source 
Working crazyflie firmware build

* Console on USART3
* Could not disable building PX4IO firmware, currently commented out

Don't build PX4IO firmware if the board doesn't ask for it

Added crazyflie motor driver

Fixed wrong register

CLK_SEL is in PWR_MGMT_1

Initial I2C/SPI MPU9250 device

* Tested with I2C
* Need to add error checking
* Intermittent crash on stop call

Working ak8963 mag driver

Functional lps25h driver. Work in progress.

Works well enough to probe and allow sensors task to start.

Added serial port test module

HACK! Get sensors module working

Set crazyflie PWM range

Extend baudrate for Crazyflie's NRF radio

Added dummy tone alarm to allow for init

Added autostart script for Crazyflie
This commit is contained in:
Tim Dyer
2016-09-04 19:12:08 +02:00
committed by Lorenz Meier
parent c1e681df60
commit b65ff53b00
44 changed files with 6812 additions and 119 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/mpu9250/CMakeLists.txt
+3 -1
View File
@@ -39,10 +39,12 @@ px4_add_module(
-Os
SRCS
mpu9250.cpp
mpu9250_i2c.cpp
mpu9250_spi.cpp
main.cpp
gyro.cpp
mag.cpp
DEPENDS
platforms__common
)
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
# vim: set noet ft=cmake fenc=utf-8 ff=unix :
src/drivers/mpu9250/main.cpp
+198 -83
View File
@@ -89,61 +89,111 @@ extern "C" { __EXPORT int mpu9250_main(int argc, char *argv[]); }
namespace mpu9250
{
MPU9250 *g_dev_int; // on internal bus
MPU9250 *g_dev_ext; // on external bus
enum MPU9250_BUS {
MPU9250_BUS_ALL = 0,
MPU9250_BUS_I2C_INTERNAL,
MPU9250_BUS_I2C_EXTERNAL,
MPU9250_BUS_SPI_INTERNAL,
MPU9250_BUS_SPI_EXTERNAL
};
void start(bool, enum Rotation);
void stop(bool);
void test(bool);
void reset(bool);
void info(bool);
void regdump(bool);
void testerror(bool);
/*
list of supported bus configurations
*/
struct mpu9250_bus_option {
enum MPU9250_BUS busid;
const char *accelpath;
const char *gyropath;
const char *magpath;
MPU9250_constructor interface_constructor;
uint8_t busnum;
MPU6000 *dev;
} bus_options[] = {
#if defined (USE_I2C)
# if defined(PX4_I2C_BUS_ONBOARD)
{ MPU9250_BUS_I2C_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG, &MPU9250_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL },
# endif
# if defined(PX4_I2C_BUS_EXPANSION)
{ MPU9250_BUS_I2C_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT, &MPU9250_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL },
# endif
#endif
#ifdef PX4_SPIDEV_MPU
{ MPU9250_BUS_SPI_INTERNAL, MPU_DEVICE_PATH_ACCEL, MPU_DEVICE_PATH_GYRO, MPU_DEVICE_PATH_MAG, &MPU9250_SPI_interface, PX4_SPI_BUS_SENSORS, NULL },
#endif
#if defined(PX4_SPI_BUS_EXT)
{ MPU9250_BUS_SPI_EXTERNAL, MPU_DEVICE_PATH_ACCEL_EXT, MPU_DEVICE_PATH_GYRO_EXT, MPU_DEVICE_PATH_MAG_EXT &MPU9250_SPI_interface, PX4_SPI_BUS_EXT, NULL },
#endif
};
#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
void start(enum MPU9250_BUS busid, enum Rotation rotation);
bool start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation);
struct mpu9250_bus_option &find_bus(enum MPU9250_BUS busid);
void stop(enum MPU9250_BUS busid);
void test(enum MPU9250_BUS busid);
void reset(enum MPU9250_BUS busid);
void info(enum MPU9250_BUS busid);
void regdump(enum MPU9250_BUS busid);
void testerror(enum MPU9250_BUS busid);
void usage();
/**
* Start the driver.
*
* This function only returns if the driver is up and running
* or failed to detect the sensor.
* find a bus structure for a busid
*/
void
start(bool external_bus, enum Rotation rotation)
struct mpu9250_bus_option &find_bus(enum MPU9250_BUS busid)
{
int fd;
MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL;
const char *path_gyro = external_bus ? MPU_DEVICE_PATH_GYRO_EXT : MPU_DEVICE_PATH_GYRO;
const char *path_mag = external_bus ? MPU_DEVICE_PATH_MAG_EXT : MPU_DEVICE_PATH_MAG;
if (*g_dev_ptr != nullptr)
/* if already started, the still command succeeded */
{
errx(0, "already started");
for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) {
if ((busid == MPU9250_BUS_ALL ||
busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
return bus_options[i];
}
}
/* create the driver */
if (external_bus) {
#ifdef PX4_SPI_BUS_EXT
*g_dev_ptr = new MPU9250(PX4_SPI_BUS_EXT, path_accel, path_gyro, path_mag, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation);
#else
errx(0, "External SPI not available");
#endif
errx(1, "bus %u not started", (unsigned)busid);
}
} else {
*g_dev_ptr = new MPU9250(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, path_mag, (spi_dev_e)PX4_SPIDEV_MPU, rotation);
/**
* start driver for a specific bus option
*/
bool
start_bus(struct mpu9250_bus_option &bus, enum Rotation rotation, int range, int device_type, bool external)
{
int fd = -1;
if (bus.dev != nullptr) {
warnx("%s SPI not available", external ? "External" : "Internal");
return false;
}
if (*g_dev_ptr == nullptr) {
goto fail;
device::Device *interface = bus.interface_constructor(bus.busnum, device_type, external);
if (interface == nullptr) {
warnx("no device on bus %u", (unsigned)bus.busid);
return false;
}
if (OK != (*g_dev_ptr)->init()) {
if (interface->init() != OK) {
delete interface;
warnx("no device on bus %u", (unsigned)bus.busid);
return false;
}
bus.dev = new MPU9250(interface, bus.accelpath, bus.gyropath, bus.magpath, rotation, device_type);
if (bus.dev == nullptr) {
delete interface;
return false;
}
if (OK != bus.dev->init()) {
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = open(path_accel, O_RDONLY);
fd = open(bus.accelpath, O_RDONLY);
if (fd < 0) {
goto fail;
@@ -153,27 +203,67 @@ start(bool external_bus, enum Rotation rotation)
goto fail;
}
if (ioctl(fd, ACCELIOCSRANGE, range) < 0) {
goto fail;
}
close(fd);
exit(0);
return true;
fail:
if (*g_dev_ptr != nullptr) {
delete(*g_dev_ptr);
*g_dev_ptr = nullptr;
if (fd >= 0) {
close(fd);
}
if (bus.dev != nullptr) {
delete(bus.dev);
bus.dev = nullptr;
}
errx(1, "driver start failed");
}
/**
* Start the driver.
*
* This function only returns if the driver is up and running
* or failed to detect the sensor.
*/
void
stop(bool external_bus)
start(enum MPU9250_BUS busid, enum Rotation rotation, int range, int device_type, bool external)
{
MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
if (*g_dev_ptr != nullptr) {
delete *g_dev_ptr;
*g_dev_ptr = nullptr;
bool started = false;
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
if (busid == MPU9250_BUS_ALL && bus_options[i].dev != NULL) {
// this device is already started
continue;
}
if (busid != MPU9250_BUS_ALL && bus_options[i].busid != busid) {
// not the one that is asked for
continue;
}
started |= start_bus(bus_options[i], rotation, range, device_type, external);
}
exit(started ? 0 : 1);
}
void
stop(enum MPU9250_BUS busid)
{
struct mpu9250_bus_option &bus = find_bus(busid);
if (bus.dev != nullptr) {
delete bus.dev;
bus.dev = nullptr;
} else {
/* warn, but not an error */
@@ -189,35 +279,33 @@ stop(bool external_bus)
* and automatic modes.
*/
void
test(bool external_bus)
test(enum MPU9250_BUS busid)
{
const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL;
const char *path_gyro = external_bus ? MPU_DEVICE_PATH_GYRO_EXT : MPU_DEVICE_PATH_GYRO;
const char *path_mag = external_bus ? MPU_DEVICE_PATH_MAG_EXT : MPU_DEVICE_PATH_MAG;
struct mpu9250_bus_option &bus = find_bus(busid);
accel_report a_report;
gyro_report g_report;
mag_report m_report;
ssize_t sz;
/* get the driver */
int fd = open(path_accel, O_RDONLY);
int fd = open(bus.accelpath, O_RDONLY);
if (fd < 0) {
err(1, "%s open failed (try 'm start')", path_accel);
err(1, "%s open failed (try 'm start')", bus.accelpath);
}
/* get the driver */
int fd_gyro = open(path_gyro, O_RDONLY);
int fd_gyro = open(bus.gyropath, O_RDONLY);
if (fd_gyro < 0) {
err(1, "%s open failed", path_gyro);
err(1, "%s open failed", bus.gyropath);
}
/* get the driver */
int fd_mag = open(path_mag, O_RDONLY);
int fd_mag = open(bus.magpath, O_RDONLY);
if (fd_mag < 0) {
err(1, "%s open failed", path_mag);
err(1, "%s open failed", bus.magpath);
}
/* reset to manual polling */
@@ -292,7 +380,7 @@ test(bool external_bus)
/* XXX add poll-rate tests here too */
reset(external_bus);
reset(busid);
errx(0, "PASS");
}
@@ -300,10 +388,10 @@ test(bool external_bus)
* Reset the driver.
*/
void
reset(bool external_bus)
reset(enum MPU9250_BUS busid)
{
const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL;
int fd = open(path_accel, O_RDONLY);
struct mpu9250_bus_option &bus = find_bus(busid);
int fd = open(bus.accelpath, O_RDONLY);
if (fd < 0) {
err(1, "failed ");
@@ -326,15 +414,17 @@ reset(bool external_bus)
* Print a little info about the driver.
*/
void
info(bool external_bus)
info(enum MPU9250_BUS busid)
{
MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
struct mpu9250_bus_option &bus = find_bus(busid);
if (*g_dev_ptr == nullptr) {
if (bus.dev == nullptr) {
errx(1, "driver not running");
}
(*g_dev_ptr)->print_info();
printf("state @ %p\n", bus.dev);
bus.dev->print_info();
exit(0);
}
@@ -343,15 +433,17 @@ info(bool external_bus)
* Dump the register information
*/
void
regdump(bool external_bus)
regdump(enum MPU9250_BUS busid)
{
MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
struct mpu9250_bus_option &bus = find_bus(busid);
if (*g_dev_ptr == nullptr) {
if (bus.dev == nullptr) {
errx(1, "driver not running");
}
(*g_dev_ptr)->print_registers();
printf("regdump @ %p\n", bus.dev);
bus.dev->print_registers();
exit(0);
}
@@ -360,15 +452,16 @@ regdump(bool external_bus)
* deliberately produce an error to test recovery
*/
void
testerror(bool external_bus)
testerror(enum MPU9250_BUS busid)
{
MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
struct mpu9250_bus_option &bus = find_bus(busid);
if (*g_dev_ptr == nullptr) {
if (bus.dev == nullptr) {
errx(1, "driver not running");
}
(*g_dev_ptr)->test_error();
bus.dev->test_error();
exit(0);
}
@@ -380,6 +473,7 @@ usage()
warnx("options:");
warnx(" -X (external bus)");
warnx(" -R rotation");
warnx(" -a accel range (in g)");
}
} // namespace
@@ -387,70 +481,91 @@ usage()
int
mpu9250_main(int argc, char *argv[])
{
bool external_bus = false;
enum MPU9250_BUS busid = MPU9250_BUS_ALL;
int ch;
bool external = false;
enum Rotation rotation = ROTATION_NONE;
int accel_range = 8;
/* jump over start/off/etc and look at options first */
while ((ch = getopt(argc, argv, "XR:")) != EOF) {
while ((ch = getopt(argc, argv, "XISsR:a:")) != EOF) {
switch (ch) {
case 'X':
external_bus = true;
busid = MPU9260_BUS_I2C_EXTERNAL;
break;
case 'I':
busid = MPU9250_BUS_I2C_INTERNAL;
break;
case 'S':
busid = MPU9250_BUS_SPI_EXTERNAL;
break;
case 's':
busid = MPU9260_BUS_SPI_INTERNAL;
break;
case 'R':
rotation = (enum Rotation)atoi(optarg);
break;
case 'a':
accel_range = atoi(optarg);
break;
default:
mpu9250::usage();
exit(0);
}
}
external = (busid == MPU9250_BUS_I2C_EXTERNAL || busid == MPU9250_BUS_SPI_EXTERNAL);
const char *verb = argv[optind];
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
mpu9250::start(external_bus, rotation);
mpu9250::start(busid, rotation, accel_range, device_type, external);
}
if (!strcmp(verb, "stop")) {
mpu9250::stop(external_bus);
mpu9250::stop(busid);
}
/*
* Test the driver/device.
*/
if (!strcmp(verb, "test")) {
mpu9250::test(external_bus);
mpu9250::test(busid);
}
/*
* Reset the driver.
*/
if (!strcmp(verb, "reset")) {
mpu9250::reset(external_bus);
mpu9250::reset(busid);
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info")) {
mpu9250::info(external_bus);
mpu9250::info(busid);
}
/*
* Print register information.
*/
if (!strcmp(verb, "regdump")) {
mpu9250::regdump(external_bus);
mpu9250::regdump(busid);
}
if (!strcmp(verb, "testerror")) {
mpu9250::testerror(external_bus);
mpu9250::testerror(busid);
}
mpu9250::usage();
src/drivers/mpu9250/mpu9250.cpp
+54 -13
View File
@@ -34,7 +34,7 @@
/**
* @file mpu9250.cpp
*
* Driver for the Invensense MPU9250 connected via SPI.
* Driver for the Invensense MPU9250 connected via I2C or SPI.
*
* @author Andrew Tridgell
*
@@ -215,9 +215,12 @@ const uint8_t MPU9250::_checked_registers[MPU9250_NUM_CHECKED_REGISTERS] = { MPU
};
MPU9250::MPU9250(int bus, const char *path_accel, const char *path_gyro, const char *path_mag, spi_dev_e device,
MPU9250::MPU9250(device::Device *interface, const char *path_accel, const char *path_gyro, const char *path_mag,
enum Rotation rotation) :
SPI("MPU9250", path_accel, bus, device, SPIDEV_MODE3, MPU9250_LOW_BUS_SPEED),
CDev("MPU9250", path_accel),
_interface(interface),
_interface_bus(interface_bus),
//SPI("MPU9250", path_accel, bus, device, SPIDEV_MODE3, MPU9250_LOW_BUS_SPEED),
_gyro(new MPU9250_gyro(this, path_gyro)),
_mag(new MPU9250_mag(this, path_mag)),
_whoami(0),
@@ -333,15 +336,28 @@ MPU9250::init()
{
int ret;
/* do SPI init (and probe) first */
ret = SPI::init();
/* if probe/setup failed, bail now */
/* do init */
ret = CDev::init();
/* if init failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("SPI setup failed");
DEVICE_DEBUG("CDev init failed");
return ret;
}
// /* do SPI init (and probe) first */
// ret = SPI::init();
// /* if probe/setup failed, bail now */
// if (ret != OK) {
// DEVICE_DEBUG("SPI setup failed");
// return ret;
// }
ret = probe();
if (ret != OK) {
@@ -742,7 +758,8 @@ MPU9250::test_error()
// development as a handy way to test the reset logic
uint8_t data[16];
memset(data, 0, sizeof(data));
transfer(data, data, sizeof(data));
_interface->read(MPU6000_SET_SPEED(MPUREG_INT_STATUS, MPU6000_LOW_BUS_SPEED), data, sizeof(data));
//transfer(data, data, sizeof(data));
::printf("error triggered\n");
print_registers();
}
@@ -1047,13 +1064,22 @@ MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
return CDev::ioctl(filp, cmd, arg);
}
}
uint8_t
MPU9250::read_reg(unsigned reg, uint32_t speed)
{
// From MPU6000 implementation
uint8_t buf;
_interface->read(MPU6000_SET_SPEED(reg, speed), &buf, 1);
return buf;
uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0};
// general register transfer at low clock speed
@@ -1067,6 +1093,16 @@ MPU9250::read_reg(unsigned reg, uint32_t speed)
uint16_t
MPU9250::read_reg16(unsigned reg)
{
uint8_t buf[2];
// general register transfer at low clock speed
_interface->read(MPU9250_LOW_SPEED_OP(reg), &buf, arraySize(buf));
return (uint16_t)(buf[0] << 8) | buf[1];
uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 };
// general register transfer at low clock speed
@@ -1080,6 +1116,12 @@ MPU9250::read_reg16(unsigned reg)
void
MPU9250::write_reg(unsigned reg, uint8_t value)
{
// general register transfer at low clock speed
return _interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1);
uint8_t cmd[2];
cmd[0] = reg | DIR_WRITE;
@@ -1313,10 +1355,9 @@ MPU9250::measure()
*/
mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;
// sensor transfer at high clock speed
set_frequency(MPU9250_HIGH_BUS_SPEED);
if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) {
if (sizeof(mpu_report) != _interface->read(MPU9250_SET_SPEED(MPUREG_INT_STATUS, MPU9250_HIGH_BUS_SPEED),
(uint8_t *)&mpu_report,
sizeof(mpu_report))) {
return;
}
src/drivers/mpu9250/mpu9250.h
+29 -15
View File
@@ -1,26 +1,13 @@
#ifdef PX4_SPI_BUS_EXT
#define EXTERNAL_BUS PX4_SPI_BUS_EXT
#else
#define EXTERNAL_BUS 0
#endif
/*
the MPU9250 can only handle high SPI bus speeds on the sensor and
interrupt status registers. All other registers have a maximum 1MHz
SPI speed
*/
#define MPU9250_LOW_BUS_SPEED 1000*1000
#define MPU9250_HIGH_BUS_SPEED 11*1000*1000
#define MPU9250_ONE_G 9.80665f
class MPU9250_mag;
class MPU9250_gyro;
class MPU9250 : public device::SPI
class MPU9250 : public device::CDev
{
public:
MPU9250(int bus, const char *path_accel, const char *path_gyro, const char *path_mag, spi_dev_e device,
MPU9250(device::Device *interface, const char *path_accel, const char *path_gyro, const char *path_mag,
enum Rotation rotation);
virtual ~MPU9250();
@@ -40,6 +27,8 @@ public:
void test_error();
protected:
Device *_interface;
virtual int probe();
friend class MPU9250_mag;
@@ -265,3 +254,28 @@ private:
};
#pragma pack(pop)
};
/*
The MPU9250 can only handle high bus speeds on the sensor and
interrupt status registers. All other registers have a maximum 1MHz
Communication with all registers of the device is performed using either
I2C at 400kHz or SPI at 1MHz. For applications requiring faster communications,
the sensor and interrupt registers may be read using SPI at 20MHz
*/
#define MPU9250_LOW_BUS_SPEED 0
#define MPU9250_HIGH_BUS_SPEED 0x8000
# define MPU9250_IS_HIGH_SPEED(r) ((r) & MPU9250_HIGH_BUS_SPEED)
# define MPU9250_REG(r) ((r) &~MPU9250_HIGH_BUS_SPEED)
# define MPU9250_SET_SPEED(r, s) ((r)|(s))
# define MPU9250_HIGH_SPEED_OP(r) MPU9250_SET_SPEED((r), MPU9250_HIGH_BUS_SPEED)
# define MPU9250_LOW_SPEED_OP(r) MPU9250_REG((r))
/* interface factories */
extern device::Device *MPU9250_SPI_interface(int bus, int device_type, bool external_bus);
extern device::Device *MPU9250_I2C_interface(int bus, int device_type, bool external_bus);
extern int MPU9250_probe(device::Device *dev, int device_type);
typedef device::Device *(*MPU9250_constructor)(int, int, bool);
src/drivers/mpu9250/mpu9250_i2c.cpp
+172
View File
@@ -0,0 +1,172 @@
/****************************************************************************
*
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mpu9250_i2c.cpp
*
* I2C interface for MPU9250
*/
/* XXX trim includes */
#include <px4_config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <assert.h>
#include <debug.h>
#include <errno.h>
#include <unistd.h>
#include <arch/board/board.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_device.h>
#include "MPU9250.h"
#include "board_config.h"
#ifdef USE_I2C
device::Device *MPU9250_I2C_interface(int bus, int device_type, bool external_bus);
class MPU9250_I2C : public device::I2C
{
public:
MPU9250_I2C(int bus, int device_type);
virtual ~MPU9250_I2C();
virtual int init();
virtual int read(unsigned address, void *data, unsigned count);
virtual int write(unsigned address, void *data, unsigned count);
virtual int ioctl(unsigned operation, unsigned &arg);
protected:
virtual int probe();
private:
int _device_type;
};
device::Device *
MPU9250_I2C_interface(int bus, int device_type, bool external_bus)
{
return new MPU9250_I2C(bus, device_type);
}
MPU9250_I2C::MPU9250_I2C(int bus, int device_type) :
I2C("MPU9250_I2C", nullptr, bus, PX4_I2C_OBDEV_MPU9250, 400000),
_device_type(device_type)
{
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250;
}
MPU9250_I2C::~MPU9250_I2C()
{
}
int
MPU9250_I2C::init()
{
/* this will call probe() */
return I2C::init();
}
int
MPU9250_I2C::ioctl(unsigned operation, unsigned &arg)
{
int ret;
switch (operation) {
case ACCELIOCGEXTERNAL:
return _bus == PX4_I2C_BUS_EXPANSION ? 1 : 0;
case DEVIOCGDEVICEID:
return CDev::ioctl(nullptr, operation, arg);
case MPUIOCGIS_I2C:
return 1;
default:
ret = -EINVAL;
}
return ret;
}
int
MPU9250_I2C::write(unsigned reg_speed, void *data, unsigned count)
{
uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE];
if (sizeof(cmd) < (count + 1)) {
return -EIO;
}
cmd[0] = MPU9250_REG(reg_speed);
cmd[1] = *(uint8_t *)data;
return transfer(&cmd[0], count + 1, nullptr, 0);
}
int
MPU9250_I2C::read(unsigned reg_speed, void *data, unsigned count)
{
/* We want to avoid copying the data of MPUReport: So if the caller
* supplies a buffer not MPUReport in size, it is assume to be a reg or
* reg 16 read
* Since MPUReport has a cmd at front, we must return the data
* after that. Foe anthing else we must return it
*/
uint32_t offset = count < sizeof(MPUReport) ? 0 : offsetof(MPUReport, status);
uint8_t cmd = MPU9250_REG(reg_speed);
int ret = transfer(&cmd, 1, &((uint8_t *)data)[offset], count);
return ret == OK ? count : ret;
}
int
MPU9250_I2C::probe()
{
uint8_t whoami = 0;
uint8_t expected = _device_type == 6000 ? MPU_WHOAMI_6000 : ICM_WHOAMI_20608;
return (read(MPUREG_WHOAMI, &whoami, 1) > 0 && (whoami == expected)) ? 0 : -EIO;
}
#endif /* PX4_I2C_OBDEV_HMC5883 */
src/drivers/mpu9250/mpu9250_spi.cpp
+281
View File
@@ -0,0 +1,281 @@
/****************************************************************************
*
* Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mpu9250_spi.cpp
*
* Driver for the Invensense MPU9250 connected via SPI.
*
* @author Andrew Tridgell
* @author Pat Hickey
* @author David sidrane
*/
#include <px4_config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <assert.h>
#include <debug.h>
#include <errno.h>
#include <unistd.h>
#include <arch/board/board.h>
#include <drivers/device/spi.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_device.h>
#include "MPU9250.h"
#include <board_config.h>
#define DIR_READ 0x80
#define DIR_WRITE 0x00
#if PX4_SPIDEV_MPU
#ifdef PX4_SPI_BUS_EXT
#define EXTERNAL_BUS PX4_SPI_BUS_EXT
#else
#define EXTERNAL_BUS 0
#endif
/*
The MPU9250 can only handle high SPI bus speeds on the sensor and
interrupt status registers. All other registers have a maximum 1MHz
SPI speed
*/
#define MPU9250_LOW_SPI_BUS_SPEED 1000*1000
#define MPU9250_HIGH_SPI_BUS_SPEED 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for MPU6K */
device::Device *MPU9250_SPI_interface(int bus, int device_type, bool external_bus);
class MPU9250_SPI : public device::SPI
{
public:
MPU9250_SPI(int bus, spi_dev_e device, int device_type);
virtual ~MPU9250_SPI();
virtual int init();
virtual int read(unsigned address, void *data, unsigned count);
virtual int write(unsigned address, void *data, unsigned count);
virtual int ioctl(unsigned operation, unsigned &arg);
protected:
virtual int probe();
private:
int _device_type;
/* Helper to set the desired speed and isolate the register on return */
void set_bus_frequency(unsigned &reg_speed_reg_out);
};
device::Device *
MPU9250_SPI_interface(int bus, int device_type, bool external_bus)
{
spi_dev_e cs = SPIDEV_NONE;
device::Device *interface = nullptr;
if (external_bus) {
#ifdef PX4_SPI_BUS_EXT
# if defined(PX4_SPIDEV_EXT_ICM)
cs = (spi_dev_e)(device_type == 6000 ? PX4_SPIDEV_EXT_MPU : PX4_SPIDEV_EXT_ICM);
# else
cs = (spi_dev_e) PX4_SPIDEV_EXT_MPU;
# endif
#endif
} else {
#if defined(PX4_SPIDEV_ICM)
cs = (spi_dev_e)(device_type == 6000 ? PX4_SPIDEV_MPU : PX4_SPIDEV_ICM);
#else
cs = (spi_dev_e) PX4_SPIDEV_MPU;
#endif
}
if (cs != SPIDEV_NONE) {
interface = new MPU9250_SPI(bus, cs, device_type);
}
return interface;
}
MPU9250_SPI::MPU9250_SPI(int bus, spi_dev_e device, int device_type) :
SPI("MPU9250", nullptr, bus, device, SPIDEV_MODE3, MPU9250_LOW_SPI_BUS_SPEED),
_device_type(device_type)
{
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250;
}
MPU9250_SPI::~MPU9250_SPI()
{
}
int
MPU9250_SPI::init()
{
int ret;
ret = SPI::init();
if (ret != OK) {
DEVICE_DEBUG("SPI init failed");
return -EIO;
}
return OK;
}
int
MPU9250_SPI::ioctl(unsigned operation, unsigned &arg)
{
int ret;
switch (operation) {
case ACCELIOCGEXTERNAL:
#if defined(PX4_SPI_BUS_EXT)
return _bus == PX4_SPI_BUS_EXT ? 1 : 0;
#else
return 0;
#endif
case DEVIOCGDEVICEID:
return CDev::ioctl(nullptr, operation, arg);
case MPUIOCGIS_I2C:
return 0;
default: {
ret = -EINVAL;
}
}
return ret;
}
void
MPU9250_SPI::set_bus_frequency(unsigned &reg_speed)
{
/* Set the desired speed */
set_frequency(MPU9250_IS_HIGH_SPEED(reg_speed) ? MPU9250_HIGH_SPI_BUS_SPEED : MPU9250_LOW_SPI_BUS_SPEED);
/* Isoolate the register on return */
reg_speed = MPU9250_REG(reg_speed);
}
int
MPU9250_SPI::write(unsigned reg_speed, void *data, unsigned count)
{
uint8_t cmd[MPU_MAX_WRITE_BUFFER_SIZE];
if (sizeof(cmd) < (count + 1)) {
return -EIO;
}
/* Set the desired speed and isolate the register */
set_bus_frequency(reg_speed);
cmd[0] = reg_speed | DIR_WRITE;
cmd[1] = *(uint8_t *)data;
return transfer(&cmd[0], &cmd[0], count + 1);
}
int
MPU9250_SPI::read(unsigned reg_speed, void *data, unsigned count)
{
/* We want to avoid copying the data of MPUReport: So if the caller
* supplies a buffer not MPUReport in size, it is assume to be a reg or reg 16 read
* and we need to provied the buffer large enough for the callers data
* and our command.
*/
uint8_t cmd[3] = {0, 0, 0};
uint8_t *pbuff = count < sizeof(MPUReport) ? cmd : (uint8_t *) data ;
if (count < sizeof(MPUReport)) {
/* add command */
count++;
}
set_bus_frequency(reg_speed);
/* Set command */
pbuff[0] = reg_speed | DIR_READ ;
/* Transfer the command and get the data */
int ret = transfer(pbuff, pbuff, count);
if (ret == OK && pbuff == &cmd[0]) {
/* Adjust the count back */
count--;
/* Return the data */
memcpy(data, &cmd[1], count);
}
return ret == OK ? count : ret;
}
int
MPU9250_SPI::probe()
{
uint8_t whoami = 0;
uint8_t expected = _device_type == 6000 ? MPU_WHOAMI_6000 : ICM_WHOAMI_20608;
return (read(MPUREG_WHOAMI, &whoami, 1) > 0 && (whoami == expected)) ? 0 : -EIO;
}
#endif // PX4_SPIDEV_MPU