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

Merge branch 'master' of github.com:PX4/Firmware

Browse Source
This commit is contained in:
Lorenz Meier
2012-08-14 09:08:52 +02:00
parent ab8d1b3b3b 735f8ffa3d
commit ebbdbac97b
10 changed files with 1020 additions and 461 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+1
View File
@@ -34,3 +34,4 @@ nuttx/nuttx.hex
Firmware.sublime-workspace
.DS_Store
nsh_romfsimg.h
cscope.out
apps/drivers/device/device.h.orig
-430
View File
@@ -1,430 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 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 Definitions for the generic base classes in the device framework.
*/
#ifndef _DEVICE_DEVICE_H
#define _DEVICE_DEVICE_H
/*
* Includes here should only cover the needs of the framework definitions.
*/
#include <nuttx/config.h>
#include <errno.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
#include <poll.h>
#include <nuttx/fs/fs.h>
/**
* Namespace encapsulating all device framework classes, functions and data.
*/
namespace device __EXPORT
{
/**
* Fundamental base class for all device drivers.
*
* This class handles the basic "being a driver" things, including
* interrupt registration and dispatch.
*/
class __EXPORT Device
{
public:
/**
* Interrupt handler.
*/
virtual void interrupt(void *ctx); /**< interrupt handler */
protected:
const char *_name; /**< driver name */
bool _debug_enabled; /**< if true, debug messages are printed */
/**
* Constructor
*
* @param name Driver name
* @param irq Interrupt assigned to the device.
*/
Device(const char *name,
int irq = 0);
~Device();
/**
* Initialise the driver and make it ready for use.
*
* @return OK if the driver initialised OK.
*/
virtual int init();
/**
* Enable the device interrupt
*/
void interrupt_enable();
/**
* Disable the device interrupt
*/
void interrupt_disable();
/**
* Take the driver lock.
*
* Each driver instance has its own lock/semaphore.
*
* Note that we must loop as the wait may be interrupted by a signal.
*/
void lock() {
do {} while (sem_wait(&_lock) != 0);
}
/**
* Release the driver lock.
*/
void unlock() {
sem_post(&_lock);
}
/**
* Log a message.
*
* The message is prefixed with the driver name, and followed
* by a newline.
*/
void log(const char *fmt, ...);
/**
* Print a debug message.
*
* The message is prefixed with the driver name, and followed
* by a newline.
*/
void debug(const char *fmt, ...);
private:
int _irq;
bool _irq_attached;
sem_t _lock;
/** disable copy construction for this and all subclasses */
Device(const Device &);
/** disable assignment for this and all subclasses */
Device &operator = (const Device &);
/**
* Register ourselves as a handler for an interrupt
*
* @param irq The interrupt to claim
* @return OK if the interrupt was registered
*/
int dev_register_interrupt(int irq);
/**
* Unregister ourselves as a handler for any interrupt
*/
void dev_unregister_interrupt();
/**
* Interrupt dispatcher
*
* @param irq The interrupt that has been triggered.
* @param context Pointer to the interrupted context.
*/
static void dev_interrupt(int irq, void *context);
};
/**
* Abstract class for any character device
*/
class __EXPORT CDev : public Device
{
public:
/**
* Constructor
*
* @param name Driver name
* @param devname Device node name
* @param irq Interrupt assigned to the device
*/
CDev(const char *name, const char *devname, int irq = 0);
/**
* Destructor
*/
~CDev();
virtual int init();
/**
* Handle an open of the device.
*
* This function is called for every open of the device. The default
* implementation maintains _open_count and always returns OK.
*
* @param filp Pointer to the NuttX file structure.
* @return OK if the open is allowed, -errno otherwise.
*/
virtual int open(struct file *filp);
/**
* Handle a close of the device.
*
* This function is called for every close of the device. The default
* implementation maintains _open_count and returns OK as long as it is not zero.
*
* @param filp Pointer to the NuttX file structure.
* @return OK if the close was successful, -errno otherwise.
*/
virtual int close(struct file *filp);
/**
* Perform a read from the device.
*
* The default implementation returns -ENOSYS.
*
* @param filp Pointer to the NuttX file structure.
* @param buffer Pointer to the buffer into which data should be placed.
* @param buflen The number of bytes to be read.
* @return The number of bytes read or -errno otherwise.
*/
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
/**
* Perform a write to the device.
*
* The default implementation returns -ENOSYS.
*
* @param filp Pointer to the NuttX file structure.
* @param buffer Pointer to the buffer from which data should be read.
* @param buflen The number of bytes to be written.
* @return The number of bytes written or -errno otherwise.
*/
virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen);
/**
* Perform a logical seek operation on the device.
*
* The default implementation returns -ENOSYS.
*
* @param filp Pointer to the NuttX file structure.
* @param offset The new file position relative to whence.
* @param whence SEEK_OFS, SEEK_CUR or SEEK_END.
* @return The previous offset, or -errno otherwise.
*/
virtual off_t seek(struct file *filp, off_t offset, int whence);
/**
* Perform an ioctl operation on the device.
*
* The default implementation handles DIOC_GETPRIV, and otherwise
* returns -ENOTTY. Subclasses should call the default implementation
* for any command they do not handle themselves.
*
* @param filp Pointer to the NuttX file structure.
* @param cmd The ioctl command value.
* @param arg The ioctl argument value.
* @return OK on success, or -errno otherwise.
*/
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Perform a poll setup/teardown operation.
*
* This is handled internally and should not normally be overridden.
*
* @param filp Pointer to the NuttX file structure.
* @param fds Poll descriptor being waited on.
* @param arg True if this is establishing a request, false if
* it is being torn down.
* @return OK on success, or -errno otherwise.
*/
virtual int poll(struct file *filp, struct pollfd *fds, bool setup);
protected:
/**
* Check the current state of the device for poll events from the
* perspective of the file.
*
* This function is called by the default poll() implementation when
* a poll is set up to determine whether the poll should return immediately.
*
* The default implementation returns no events.
*
* @param filp The file that's interested.
* @return The current set of poll events.
*/
virtual pollevent_t poll_state(struct file *filp);
/**
* Report new poll events.
*
* This function should be called anytime the state of the device changes
* in a fashion that might be interesting to a poll waiter.
*
* @param events The new event(s) being announced.
*/
virtual void poll_notify(pollevent_t events);
/**
* Internal implementation of poll_notify.
*
* @param fds A poll waiter to notify.
* @param events The event(s) to send to the waiter.
*/
virtual void poll_notify_one(struct pollfd *fds, pollevent_t events);
/**
* Notification of the first open.
*
* This function is called when the device open count transitions from zero
* to one. The driver lock is held for the duration of the call.
*
* The default implementation returns OK.
*
* @param filp Pointer to the NuttX file structure.
* @return OK if the open should proceed, -errno otherwise.
*/
virtual int open_first(struct file *filp);
/**
* Notification of the last close.
*
* This function is called when the device open count transitions from
* one to zero. The driver lock is held for the duration of the call.
*
* The default implementation returns OK.
*
* @param filp Pointer to the NuttX file structure.
* @return OK if the open should return OK, -errno otherwise.
*/
virtual int close_last(struct file *filp);
private:
static const unsigned _max_pollwaiters = 8;
const char *_devname; /**< device node name */
bool _registered; /**< true if device name was registered */
unsigned _open_count; /**< number of successful opens */
struct pollfd *_pollset[_max_pollwaiters];
/**
* Store a pollwaiter in a slot where we can find it later.
*
* Expands the pollset as required. Must be called with the driver locked.
*
* @return OK, or -errno on error.
*/
int store_poll_waiter(struct pollfd *fds);
/**
* Remove a poll waiter.
*
* @return OK, or -errno on error.
*/
int remove_poll_waiter(struct pollfd *fds);
};
/**
* Abstract class for character device accessed via PIO
*/
class __EXPORT PIO : public CDev
{
public:
/**
* Constructor
*
* @param name Driver name
* @param devname Device node name
* @param base Base address of the device PIO area
* @param irq Interrupt assigned to the device (or zero if none)
*/
PIO(const char *name,
const char *devname,
uint32_t base,
int irq = 0);
~PIO();
int init();
protected:
/**
* Read a register
*
* @param offset Register offset in bytes from the base address.
*/
uint32_t reg(uint32_t offset) {
return *(volatile uint32_t *)(_base + offset);
}
/**
* Write a register
*
* @param offset Register offset in bytes from the base address.
* @param value Value to write.
*/
void reg(uint32_t offset, uint32_t value) {
*(volatile uint32_t *)(_base + offset) = value;
}
/**
* Modify a register
*
* Note that there is a risk of a race during the read/modify/write cycle
* that must be taken care of by the caller.
*
* @param offset Register offset in bytes from the base address.
* @param clearbits Bits to clear in the register
* @param setbits Bits to set in the register
*/
void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits) {
uint32_t val = reg(offset);
val &= ~clearbits;
val |= setbits;
reg(offset, val);
}
private:
uint32_t _base;
};
} // namespace device
#endif /* _DEVICE_DEVICE_H */
apps/drivers/device/spi.cpp
+2 -1
View File
@@ -127,6 +127,7 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
SPI_LOCK(_dev, true);
SPI_SETFREQUENCY(_dev, _frequency);
SPI_SETMODE(_dev, _mode);
SPI_SETBITS(_dev, 8);
SPI_SELECT(_dev, _device, true);
/* do the transfer */
@@ -139,4 +140,4 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len)
return OK;
}
} // namespace device
} // namespace device
apps/drivers/mpu6000/Makefile
+42
View File
@@ -0,0 +1,42 @@
############################################################################
#
# Copyright (C) 2012 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.
#
############################################################################
#
# Makefile to build the BMA180 driver.
#
APPNAME = mpu6000
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
include $(APPDIR)/mk/app.mk
apps/drivers/mpu6000/mpu6000.cpp
+918
View File
@@ -0,0 +1,918 @@
/****************************************************************************
*
* Copyright (C) 2012 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 mpu6000.cpp
*
* Driver for the Invensense MPU6000 connected via SPI.
*/
#include <nuttx/config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <systemlib/perf_counter.h>
#include <nuttx/arch.h>
#include <nuttx/clock.h>
#include <arch/board/board.h>
#include <arch/board/up_hrt.h>
#include <drivers/device/spi.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#define DIR_READ 0x80
#define DIR_WRITE 0x00
// MPU 6000 registers
#define MPUREG_WHOAMI 0x75
#define MPUREG_SMPLRT_DIV 0x19
#define MPUREG_CONFIG 0x1A
#define MPUREG_GYRO_CONFIG 0x1B
#define MPUREG_ACCEL_CONFIG 0x1C
#define MPUREG_FIFO_EN 0x23
#define MPUREG_INT_PIN_CFG 0x37
#define MPUREG_INT_ENABLE 0x38
#define MPUREG_INT_STATUS 0x3A
#define MPUREG_ACCEL_XOUT_H 0x3B
#define MPUREG_ACCEL_XOUT_L 0x3C
#define MPUREG_ACCEL_YOUT_H 0x3D
#define MPUREG_ACCEL_YOUT_L 0x3E
#define MPUREG_ACCEL_ZOUT_H 0x3F
#define MPUREG_ACCEL_ZOUT_L 0x40
#define MPUREG_TEMP_OUT_H 0x41
#define MPUREG_TEMP_OUT_L 0x42
#define MPUREG_GYRO_XOUT_H 0x43
#define MPUREG_GYRO_XOUT_L 0x44
#define MPUREG_GYRO_YOUT_H 0x45
#define MPUREG_GYRO_YOUT_L 0x46
#define MPUREG_GYRO_ZOUT_H 0x47
#define MPUREG_GYRO_ZOUT_L 0x48
#define MPUREG_USER_CTRL 0x6A
#define MPUREG_PWR_MGMT_1 0x6B
#define MPUREG_PWR_MGMT_2 0x6C
#define MPUREG_FIFO_COUNTH 0x72
#define MPUREG_FIFO_COUNTL 0x73
#define MPUREG_FIFO_R_W 0x74
#define MPUREG_PRODUCT_ID 0x0C
// Configuration bits MPU 3000 and MPU 6000 (not revised)?
#define BIT_SLEEP 0x40
#define BIT_H_RESET 0x80
#define BITS_CLKSEL 0x07
#define MPU_CLK_SEL_PLLGYROX 0x01
#define MPU_CLK_SEL_PLLGYROZ 0x03
#define MPU_EXT_SYNC_GYROX 0x02
#define BITS_FS_250DPS 0x00
#define BITS_FS_500DPS 0x08
#define BITS_FS_1000DPS 0x10
#define BITS_FS_2000DPS 0x18
#define BITS_FS_MASK 0x18
#define BITS_DLPF_CFG_256HZ_NOLPF2 0x00
#define BITS_DLPF_CFG_188HZ 0x01
#define BITS_DLPF_CFG_98HZ 0x02
#define BITS_DLPF_CFG_42HZ 0x03
#define BITS_DLPF_CFG_20HZ 0x04
#define BITS_DLPF_CFG_10HZ 0x05
#define BITS_DLPF_CFG_5HZ 0x06
#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07
#define BITS_DLPF_CFG_MASK 0x07
#define BIT_INT_ANYRD_2CLEAR 0x10
#define BIT_RAW_RDY_EN 0x01
#define BIT_I2C_IF_DIS 0x10
#define BIT_INT_STATUS_DATA 0x01
// Product ID Description for MPU6000
// high 4 bits low 4 bits
// Product Name Product Revision
#define MPU6000ES_REV_C4 0x14
#define MPU6000ES_REV_C5 0x15
#define MPU6000ES_REV_D6 0x16
#define MPU6000ES_REV_D7 0x17
#define MPU6000ES_REV_D8 0x18
#define MPU6000_REV_C4 0x54
#define MPU6000_REV_C5 0x55
#define MPU6000_REV_D6 0x56
#define MPU6000_REV_D7 0x57
#define MPU6000_REV_D8 0x58
#define MPU6000_REV_D9 0x59
#define MPU6000_REV_D10 0x5A
class MPU6000_gyro;
class MPU6000 : public device::SPI
{
public:
MPU6000(int bus, spi_dev_e device);
~MPU6000();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int open_first(struct file *filp);
virtual int close_last(struct file *filp);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
protected:
virtual int probe();
friend class MPU6000_gyro;
virtual ssize_t gyro_read(struct file *filp, char *buffer, size_t buflen);
virtual int gyro_ioctl(struct file *filp, int cmd, unsigned long arg);
private:
MPU6000_gyro *_gyro;
uint8_t _product; /** product code */
struct hrt_call _call;
unsigned _call_interval;
struct accel_report _accel_report;
struct accel_scale _accel_scale;
float _accel_range_scale;
int _accel_topic;
struct gyro_report _gyro_report;
struct gyro_scale _gyro_scale;
float _gyro_range_scale;
int _gyro_topic;
unsigned _reads;
perf_counter_t _sample_perf;
/**
* Start automatic measurement.
*/
void start();
/**
* Stop automatic measurement.
*/
void stop();
/**
* Static trampoline from the hrt_call context; because we don't have a
* generic hrt wrapper yet.
*
* Called by the HRT in interrupt context at the specified rate if
* automatic polling is enabled.
*
* @param arg Instance pointer for the driver that is polling.
*/
static void measure_trampoline(void *arg);
/**
* Fetch measurements from the sensor and update the report ring.
*/
void measure();
/**
* Read a register from the MPU6000
*
* @param The register to read.
* @return The value that was read.
*/
uint8_t read_reg(unsigned reg);
uint16_t read_reg16(unsigned reg);
/**
* Write a register in the MPU6000
*
* @param reg The register to write.
* @param value The new value to write.
*/
void write_reg(unsigned reg, uint8_t value);
/**
* Modify a register in the MPU6000
*
* Bits are cleared before bits are set.
*
* @param reg The register to modify.
* @param clearbits Bits in the register to clear.
* @param setbits Bits in the register to set.
*/
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
/**
* Set the MPU6000 measurement range.
*
* @param max_g The maximum G value the range must support.
* @return OK if the value can be supported, -ERANGE otherwise.
*/
int set_range(unsigned max_g);
/**
* Swap a 16-bit value read from the MPU6000 to native byte order.
*/
uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); }
};
/**
* Helper class implementing the gyro driver node.
*/
class MPU6000_gyro : public device::CDev
{
public:
MPU6000_gyro(MPU6000 *parent);
~MPU6000_gyro();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
protected:
friend class MPU6000;
void parent_poll_notify();
private:
MPU6000 *_parent;
};
/** driver 'main' command */
extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); }
MPU6000::MPU6000(int bus, spi_dev_e device) :
SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 10000000),
_gyro(new MPU6000_gyro(this)),
_product(0),
_call_interval(0),
_accel_range_scale(1.0f),
_accel_topic(-1),
_gyro_range_scale(1.0f),
_gyro_topic(-1),
_reads(0),
_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read"))
{
// enable debug() calls
_debug_enabled = true;
// default accel scale factors
_accel_scale.x_offset = 0;
_accel_scale.x_scale = 1.0f;
_accel_scale.y_offset = 0;
_accel_scale.y_scale = 1.0f;
_accel_scale.z_offset = 0;
_accel_scale.z_scale = 1.0f;
// default gyro scale factors
_gyro_scale.x_offset = 0;
_gyro_scale.x_scale = 1.0f;
_gyro_scale.y_offset = 0;
_gyro_scale.y_scale = 1.0f;
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
memset(&_accel_report, 0, sizeof(_accel_report));
memset(&_gyro_report, 0, sizeof(_gyro_report));
}
MPU6000::~MPU6000()
{
/* make sure we are truly inactive */
stop();
/* delete the gyro subdriver */
delete _gyro;
/* delete the perf counter */
perf_free(_sample_perf);
}
int
MPU6000::init()
{
int ret;
/* do SPI init (and probe) first */
ret = SPI::init();
/* if probe/setup failed, bail now */
if (ret != OK) {
debug("SPI setup failed");
return ret;
}
/* advertise sensor topics */
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_report);
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_gyro_report);
// Chip reset
write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
up_udelay(10000);
// Wake up device and select GyroZ clock (better performance)
write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
up_udelay(1000);
// Disable I2C bus (recommended on datasheet)
write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
up_udelay(1000);
// SAMPLE RATE
write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz
usleep(1000);
// FS & DLPF FS=2000¼/s, DLPF = 98Hz (low pass filter)
write_reg(MPUREG_CONFIG, BITS_DLPF_CFG_98HZ);
usleep(1000);
write_reg(MPUREG_GYRO_CONFIG,BITS_FS_2000DPS); // Gyro scale 2000¼/s
usleep(1000);
// product-specific scaling
switch (_product) {
case MPU6000ES_REV_C4:
case MPU6000ES_REV_C5:
case MPU6000_REV_C4:
case MPU6000_REV_C5:
// Accel scale 8g (4096 LSB/g)
// Rev C has different scaling than rev D
write_reg(MPUREG_ACCEL_CONFIG, 1 << 3);
break;
case MPU6000ES_REV_D6:
case MPU6000ES_REV_D7:
case MPU6000ES_REV_D8:
case MPU6000_REV_D6:
case MPU6000_REV_D7:
case MPU6000_REV_D8:
case MPU6000_REV_D9:
case MPU6000_REV_D10:
// Accel scale 8g (4096 LSB/g)
write_reg(MPUREG_ACCEL_CONFIG,2<<3);
break;
}
usleep(1000);
// INT CFG => Interrupt on Data Ready
write_reg(MPUREG_INT_ENABLE,BIT_RAW_RDY_EN); // INT: Raw data ready
usleep(1000);
write_reg(MPUREG_INT_PIN_CFG,BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read
usleep(1000);
// Oscillator set
// write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ);
usleep(1000);
return ret;
}
int
MPU6000::open_first(struct file *filp)
{
/* reset to manual-poll mode */
_call_interval = 0;
/* XXX set default sampling/acquisition parameters */
return OK;
}
int
MPU6000::close_last(struct file *filp)
{
/* stop measurement */
stop();
return OK;
}
int
MPU6000::probe()
{
/* look for a product ID we recognise */
_product = read_reg(MPUREG_PRODUCT_ID);
// verify product revision
switch (_product) {
case MPU6000ES_REV_C4:
case MPU6000ES_REV_C5:
case MPU6000_REV_C4:
case MPU6000_REV_C5:
case MPU6000ES_REV_D6:
case MPU6000ES_REV_D7:
case MPU6000ES_REV_D8:
case MPU6000_REV_D6:
case MPU6000_REV_D7:
case MPU6000_REV_D8:
case MPU6000_REV_D9:
case MPU6000_REV_D10:
log("ID 0x%02x", _product);
return OK;
}
debug("unexpected ID 0x%02x", _product);
return -EIO;
}
ssize_t
MPU6000::read(struct file *filp, char *buffer, size_t buflen)
{
int ret = 0;
/* buffer must be large enough */
if (buflen < sizeof(_accel_report))
return -ENOSPC;
/* if automatic measurement is not enabled */
if (_call_interval == 0)
measure();
/* copy out the latest report */
memcpy(buffer, &_accel_report, sizeof(_accel_report));
ret = sizeof(_accel_report);
return ret;
}
ssize_t
MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
{
int ret = 0;
/* buffer must be large enough */
if (buflen < sizeof(_gyro_report))
return -ENOSPC;
/* if automatic measurement is not enabled */
if (_call_interval == 0)
measure();
/* copy out the latest report */
memcpy(buffer, &_gyro_report, sizeof(_gyro_report));
ret = sizeof(_gyro_report);
return ret;
}
int
MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case ACCELIOCSPOLLRATE: {
switch (arg) {
/* switching to manual polling */
case ACC_POLLRATE_MANUAL:
stop();
_call_interval = 0;
return OK;
/* external signalling not supported */
case ACC_POLLRATE_EXTERNAL:
/* zero would be bad */
case 0:
return -EINVAL;
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_call_interval == 0);
/* convert hz to hrt interval via microseconds */
unsigned ticks = 1000000 / arg;
/* check against maximum sane rate */
if (ticks < 1000)
return -EINVAL;
/* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */
_call.period = _call_interval;
/* if we need to start the poll state machine, do it */
if (want_start)
start();
return OK;
}
}
}
case ACCELIOCSQUEUEDEPTH:
/* XXX not implemented */
return -EINVAL;
case ACCELIOCSLOWPASS:
/* XXX not implemented */
return -EINVAL;
case ACCELIORANGE:
/* XXX not implemented really */
return set_range(arg);
case ACCELIOCSSAMPLERATE: /* sensor sample rate is not (really) adjustable */
case ACCELIOCSREPORTFORMAT: /* no alternate report formats */
return -EINVAL;
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
}
}
int
MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case GYROIOCSPOLLRATE:
/* gyro and accel poll rates are shared */
return ioctl(filp, ACCELIOCSPOLLRATE, arg);
case GYROIOCSQUEUEDEPTH:
/* XXX not implemented */
return -EINVAL;
case GYROIOCSLOWPASS:
/* XXX not implemented */
return -EINVAL;
case GYROIOCSSCALE:
/* XXX not implemented really */
return set_range(arg);
case GYROIOCSSAMPLERATE: /* sensor sample rate is not (really) adjustable */
case GYROIOCSREPORTFORMAT: /* no alternate report formats */
return -EINVAL;
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
}
}
uint8_t
MPU6000::read_reg(unsigned reg)
{
uint8_t cmd[2];
cmd[0] = reg | DIR_READ;
transfer(cmd, cmd, sizeof(cmd));
return cmd[1];
}
uint16_t
MPU6000::read_reg16(unsigned reg)
{
uint8_t cmd[3];
cmd[0] = reg | DIR_READ;
transfer(cmd, cmd, sizeof(cmd));
return (uint16_t)(cmd[1] << 8) | cmd[2];
}
void
MPU6000::write_reg(unsigned reg, uint8_t value)
{
uint8_t cmd[2];
cmd[0] = reg | DIR_WRITE;
cmd[1] = value;
transfer(cmd, nullptr, sizeof(cmd));
}
void
MPU6000::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
{
uint8_t val;
val = read_reg(reg);
val &= ~clearbits;
val |= setbits;
write_reg(reg, val);
}
int
MPU6000::set_range(unsigned max_g)
{
#if 0
uint8_t rangebits;
float rangescale;
if (max_g > 16) {
return -ERANGE;
} else if (max_g > 8) { /* 16G */
rangebits = OFFSET_LSB1_RANGE_16G;
rangescale = 1.98;
} else if (max_g > 4) { /* 8G */
rangebits = OFFSET_LSB1_RANGE_8G;
rangescale = 0.99;
} else if (max_g > 3) { /* 4G */
rangebits = OFFSET_LSB1_RANGE_4G;
rangescale = 0.5;
} else if (max_g > 2) { /* 3G */
rangebits = OFFSET_LSB1_RANGE_3G;
rangescale = 0.38;
} else if (max_g > 1) { /* 2G */
rangebits = OFFSET_LSB1_RANGE_2G;
rangescale = 0.25;
} else { /* 1G */
rangebits = OFFSET_LSB1_RANGE_1G;
rangescale = 0.13;
}
/* adjust sensor configuration */
modify_reg(ADDR_OFFSET_LSB1, OFFSET_LSB1_RANGE_MASK, rangebits);
_range_scale = rangescale;
#endif
return OK;
}
void
MPU6000::start()
{
/* make sure we are stopped first */
stop();
/* start polling at the specified rate */
hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&MPU6000::measure_trampoline, this);
}
void
MPU6000::stop()
{
hrt_cancel(&_call);
}
void
MPU6000::measure_trampoline(void *arg)
{
MPU6000 *dev = (MPU6000 *)arg;
/* make another measurement */
dev->measure();
}
void
MPU6000::measure()
{
#pragma pack(push, 1)
/**
* Report conversation within the MPU6000, including command byte and
* interrupt status.
*/
struct Report {
uint8_t cmd;
uint8_t status;
uint16_t accel_x;
uint16_t accel_y;
uint16_t accel_z;
uint16_t temp;
uint16_t gyro_x;
uint16_t gyro_y;
uint16_t gyro_z;
} report;
#pragma pack(pop)
/* start measuring */
perf_begin(_sample_perf);
/*
* Fetch the full set of measurements from the MPU6000 in one pass.
*/
report.cmd = DIR_READ | MPUREG_INT_STATUS;
transfer((uint8_t *)&report, (uint8_t *)&report, sizeof(report));
/*
* Adjust and scale results to mg.
*/
_gyro_report.timestamp = _accel_report.timestamp = hrt_absolute_time();
_accel_report.x = report.accel_x * _accel_range_scale;
_accel_report.y = report.accel_y * _accel_range_scale;
_accel_report.z = report.accel_z * _accel_range_scale;
_gyro_report.x = report.gyro_x * _gyro_range_scale;
_gyro_report.y = report.gyro_y * _gyro_range_scale;
_gyro_report.z = report.gyro_z * _gyro_range_scale;
/* notify anyone waiting for data */
poll_notify(POLLIN);
_gyro->parent_poll_notify();
/* and publish for subscribers */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &_accel_report);
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &_gyro_report);
/* stop measuring */
perf_end(_sample_perf);
}
void
MPU6000::print_info()
{
printf("reads: %u\n", _reads);
}
MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) :
CDev("MPU6000_gyro", GYRO_DEVICE_PATH),
_parent(parent)
{
}
MPU6000_gyro::~MPU6000_gyro()
{
}
void
MPU6000_gyro::parent_poll_notify()
{
poll_notify(POLLIN);
}
ssize_t
MPU6000_gyro::read(struct file *filp, char *buffer, size_t buflen)
{
return _parent->gyro_read(filp, buffer, buflen);
}
int
MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
{
return _parent->gyro_ioctl(filp, cmd, arg);
}
/**
* Local functions in support of the shell command.
*/
namespace
{
MPU6000 *g_dev;
/*
* XXX this should just be part of the generic sensors test...
*/
int
test()
{
int fd = -1;
struct accel_report report;
ssize_t sz;
const char *reason = "test OK";
do {
/* get the driver */
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
reason = "can't open driver";
break;
}
/* do a simple demand read */
sz = read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
reason = "immediate read failed";
break;
}
printf("single read\n");
fflush(stdout);
printf("time: %lld\n", report.timestamp);
printf("x: %f\n", report.x);
printf("y: %f\n", report.y);
printf("z: %f\n", report.z);
} while (0);
printf("MPU6000: %s\n", reason);
return OK;
}
int
info()
{
if (g_dev == nullptr) {
fprintf(stderr, "MPU6000: driver not running\n");
return -ENOENT;
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
return OK;
}
} // namespace
int
mpu6000_main(int argc, char *argv[])
{
/*
* Start/load the driver.
*
* XXX it would be nice to have a wrapper for this...
*/
if (!strcmp(argv[1], "start")) {
if (g_dev != nullptr) {
fprintf(stderr, "MPU6000: already loaded\n");
return -EBUSY;
}
/* create the driver */
g_dev = new MPU6000(1 /* XXX magic number */, (spi_dev_e)PX4_SPIDEV_MPU);
if (g_dev == nullptr) {
fprintf(stderr, "MPU6000: driver alloc failed\n");
return -ENOMEM;
}
if (OK != g_dev->init()) {
fprintf(stderr, "MPU6000: driver init failed\n");
usleep(100000);
delete g_dev;
g_dev = nullptr;
return -EIO;
}
return OK;
}
/*
* Test the driver/device.
*/
if (!strcmp(argv[1], "test"))
return test();
/*
* Print driver information.
*/
if (!strcmp(argv[1], "info"))
return info();
fprintf(stderr, "unrecognised command, try 'start', 'test' or 'info'\n");
return -EINVAL;
}
apps/drivers/ms5611/ms5611.cpp
+14 -6
View File
@@ -58,6 +58,8 @@
#include <arch/board/up_hrt.h>
#include <systemlib/perf_counter.h>
#include <drivers/drv_baro.h>
/**
@@ -123,13 +125,15 @@ private:
int32_t _dT;
int64_t _temp64;
int _orbject;
int _baro_topic;
unsigned _reads;
unsigned _measure_errors;
unsigned _read_errors;
unsigned _buf_overflows;
perf_counter_t _sample_perf;
/**
* Test whether the device supported by the driver is present at a
* specific address.
@@ -245,7 +249,8 @@ MS5611::MS5611(int bus) :
_reads(0),
_measure_errors(0),
_read_errors(0),
_buf_overflows(0)
_buf_overflows(0),
_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read"))
{
// enable debug() calls
_debug_enabled = true;
@@ -278,9 +283,9 @@ MS5611::init()
/* if this fails (e.g. no object in the system) that's OK */
memset(&b, 0, sizeof(b));
_orbject = orb_advertise(ORB_ID(sensor_baro), &b);
_baro_topic = orb_advertise(ORB_ID(sensor_baro), &b);
if (_orbject < 0)
if (_baro_topic < 0)
debug("failed to create sensor_baro object");
}
@@ -610,6 +615,8 @@ MS5611::collect()
/* read the most recent measurement */
cmd = 0;
perf_begin(_sample_perf);
/* this should be fairly close to the end of the conversion, so the best approximation of the time */
_reports[_next_report].timestamp = hrt_absolute_time();
@@ -655,7 +662,7 @@ MS5611::collect()
_reports[_next_report].altitude = (44330.0 * (1.0 - pow((press_int64 / 101325.0), 0.190295)));
/* publish it */
orb_publish(ORB_ID(sensor_baro), _orbject, &_reports[_next_report]);
orb_publish(ORB_ID(sensor_baro), _baro_topic, &_reports[_next_report]);
/* post a report to the ring - note, not locked */
INCREMENT(_next_report, _num_reports);
@@ -670,10 +677,11 @@ MS5611::collect()
poll_notify(POLLIN);
}
/* update the measurement state machine */
INCREMENT(_measure_phase, MS5611_MEASUREMENT_RATIO + 1);
perf_end(_sample_perf);
return OK;
}
apps/px4/px4io/driver/px4io.cpp
+1 -1
View File
@@ -536,7 +536,7 @@ px4io_main(int argc, char *argv[])
case OK:
break;
case -ENOENT:
fprintf(stderr, "PX4IO firmware file '%s' not found\n", fn);
fprintf(stderr, "PX4IO firmware file not found\n");
break;
case -EEXIST:
case -EIO:
nuttx/arch/arm/src/stm32/stm32_spi.c
+36 -18
View File
@@ -1368,15 +1368,21 @@ FAR struct spi_dev_s *up_spiinitialize(int port)
priv = &g_spi1dev;
/* Configure SPI1 pins: SCK, MISO, and MOSI */
/* Only configure if the port is not already configured */
stm32_configgpio(GPIO_SPI1_SCK);
stm32_configgpio(GPIO_SPI1_MISO);
stm32_configgpio(GPIO_SPI1_MOSI);
if (!(spi_getreg(priv, STM32_SPI_CR1_OFFSET) & SPI_CR1_SPE))
{
/* Set up default configuration: Master, 8-bit, etc. */
/* Configure SPI1 pins: SCK, MISO, and MOSI */
spi_portinitialize(priv);
stm32_configgpio(GPIO_SPI1_SCK);
stm32_configgpio(GPIO_SPI1_MISO);
stm32_configgpio(GPIO_SPI1_MOSI);
/* Set up default configuration: Master, 8-bit, etc. */
spi_portinitialize(priv);
}
}
else
#endif
@@ -1387,15 +1393,21 @@ FAR struct spi_dev_s *up_spiinitialize(int port)
priv = &g_spi2dev;
/* Configure SPI2 pins: SCK, MISO, and MOSI */
/* Only configure if the port is not already configured */
stm32_configgpio(GPIO_SPI2_SCK);
stm32_configgpio(GPIO_SPI2_MISO);
stm32_configgpio(GPIO_SPI2_MOSI);
if (!(spi_getreg(priv, STM32_SPI_CR1_OFFSET) & SPI_CR1_SPE))
{
/* Set up default configuration: Master, 8-bit, etc. */
/* Configure SPI2 pins: SCK, MISO, and MOSI */
spi_portinitialize(priv);
stm32_configgpio(GPIO_SPI2_SCK);
stm32_configgpio(GPIO_SPI2_MISO);
stm32_configgpio(GPIO_SPI2_MOSI);
/* Set up default configuration: Master, 8-bit, etc. */
spi_portinitialize(priv);
}
}
else
#endif
@@ -1406,15 +1418,21 @@ FAR struct spi_dev_s *up_spiinitialize(int port)
priv = &g_spi3dev;
/* Configure SPI3 pins: SCK, MISO, and MOSI */
/* Only configure if the port is not already configured */
stm32_configgpio(GPIO_SPI3_SCK);
stm32_configgpio(GPIO_SPI3_MISO);
stm32_configgpio(GPIO_SPI3_MOSI);
if (!(spi_getreg(priv, STM32_SPI_CR1_OFFSET) & SPI_CR1_SPE))
{
/* Set up default configuration: Master, 8-bit, etc. */
/* Configure SPI3 pins: SCK, MISO, and MOSI */
spi_portinitialize(priv);
stm32_configgpio(GPIO_SPI3_SCK);
stm32_configgpio(GPIO_SPI3_MISO);
stm32_configgpio(GPIO_SPI3_MOSI);
/* Set up default configuration: Master, 8-bit, etc. */
spi_portinitialize(priv);
}
}
#endif
nuttx/configs/px4fmu/nsh/appconfig
+2 -1
View File
@@ -75,7 +75,8 @@ CONFIGURED_APPS += attitude_estimator_ekf
# Communication and Drivers
CONFIGURED_APPS += drivers/device
#CONFIGURED_APPS += drivers/ms5611
CONFIGURED_APPS += drivers/ms5611
CONFIGURED_APPS += drivers/mpu6000
CONFIGURED_APPS += px4/px4io/driver
CONFIGURED_APPS += px4/fmu
nuttx/configs/px4fmu/nsh/defconfig
+4 -4
View File
@@ -537,10 +537,10 @@ CONFIG_SEM_NNESTPRIO=8
CONFIG_FDCLONE_DISABLE=n
CONFIG_FDCLONE_STDIO=y
CONFIG_SDCLONE_DISABLE=y
CONFIG_SCHED_WORKQUEUE=n
CONFIG_SCHED_WORKPRIORITY=50
CONFIG_SCHED_WORKPERIOD=(50*1000)
CONFIG_SCHED_WORKSTACKSIZE=1024
CONFIG_SCHED_WORKQUEUE=y
CONFIG_SCHED_WORKPRIORITY=199
CONFIG_SCHED_WORKPERIOD=(5*1000)
CONFIG_SCHED_WORKSTACKSIZE=2048
CONFIG_SIG_SIGWORK=4
CONFIG_SCHED_WAITPID=y
CONFIG_SCHED_ATEXIT=n