mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-29 05:23:33 +08:00
Kicked out mix_and_link, deleted old MPU driver, disabled (still needed for reference) old HMC and MS5611 drivers. Removed driver init from up_nsh.c. Reworked fixedwing_control to be closer to up-to-date api, still more clean up needed. Fixed a bug that limited the motor thrust for multirotor control
This commit is contained in:
Lorenz Meier
@@ -1,92 +0,0 @@
|
||||
/*
|
||||
* Copyright (C) 2012 Lorenz Meier. 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 of the author or the names of 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.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Driver for the Invense MPU-6000 gyroscope
|
||||
*/
|
||||
|
||||
/* IMPORTANT NOTES:
|
||||
*
|
||||
* SPI max. clock frequency: 10 Mhz
|
||||
* CS has to be high before transfer,
|
||||
* go low right before transfer and
|
||||
* go high again right after transfer
|
||||
*
|
||||
*/
|
||||
|
||||
#include <sys/ioctl.h>
|
||||
|
||||
#define _MPU6000BASE 0x7600
|
||||
#define MPU6000C(_x) _IOC(_MPU6000BASE, _x)
|
||||
|
||||
/*
|
||||
* Sets the sensor internal sampling rate, and if a buffer
|
||||
* has been configured, the rate at which entries will be
|
||||
* added to the buffer.
|
||||
*/
|
||||
#define MPU6000_SETRATE MPU6000C(1)
|
||||
|
||||
#define MPU6000_RATE_95HZ_LP_12_5HZ ((0<<7) | (0<<6) | (0<<5) | (0<<4))
|
||||
|
||||
/*
|
||||
* Sets the sensor internal range.
|
||||
*/
|
||||
#define MPU6000_SETRANGE MPU6000C(2)
|
||||
|
||||
#define MPU6000_RANGE_250DPS (0<<4)
|
||||
|
||||
#define MPU6000_RATE_95HZ ((0<<6) | (0<<4))
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* Sets the address of a shared MPU6000_buffer
|
||||
* structure that is maintained by the driver.
|
||||
*
|
||||
* If zero is passed as the address, disables
|
||||
* the buffer updating.
|
||||
*/
|
||||
#define MPU6000_SETBUFFER MPU6000C(3)
|
||||
|
||||
struct MPU6000_buffer {
|
||||
uint32_t size; /* number of entries in the samples[] array */
|
||||
uint32_t next; /* the next entry that will be populated */
|
||||
struct {
|
||||
uint16_t x;
|
||||
uint16_t y;
|
||||
uint16_t z;
|
||||
uint16_t roll;
|
||||
uint16_t pitch;
|
||||
uint16_t yaw;
|
||||
} samples[];
|
||||
};
|
||||
|
||||
extern int mpu6000_attach(struct spi_dev_s *spi, int spi_id);
|
||||
@@ -71,12 +71,10 @@ CONFIGURED_APPS += gps
|
||||
CONFIGURED_APPS += commander
|
||||
#CONFIGURED_APPS += sdlog
|
||||
CONFIGURED_APPS += sensors
|
||||
CONFIGURED_APPS += ardrone_control
|
||||
CONFIGURED_APPS += ardrone_interface
|
||||
CONFIGURED_APPS += multirotor_att_control
|
||||
CONFIGURED_APPS += px4/attitude_estimator_bm
|
||||
CONFIGURED_APPS += fixedwing_control
|
||||
CONFIGURED_APPS += mix_and_link
|
||||
CONFIGURED_APPS += position_estimator
|
||||
CONFIGURED_APPS += attitude_estimator_ekf
|
||||
|
||||
|
||||
@@ -42,9 +42,9 @@ AOBJS = $(ASRCS:.S=$(OBJEXT))
|
||||
|
||||
CSRCS = up_boot.c up_leds.c up_spi.c up_hrt.c \
|
||||
drv_gpio.c drv_bma180.c drv_l3gd20.c \
|
||||
drv_led.c drv_hmc5833l.c drv_ms5611.c drv_eeprom.c \
|
||||
drv_led.c drv_eeprom.c \
|
||||
drv_tone_alarm.c up_pwm_servo.c up_usbdev.c \
|
||||
up_cpuload.c drv_mpu6000.c
|
||||
up_cpuload.c
|
||||
|
||||
ifeq ($(CONFIG_NSH_ARCHINIT),y)
|
||||
CSRCS += up_nsh.c
|
||||
|
||||
@@ -1,433 +0,0 @@
|
||||
/*
|
||||
* Copyright (C) 2012 Lorenz Meier. 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 of the author or the names of 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.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Driver for the ST mpu6000 MEMS gyroscope
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <debug.h>
|
||||
#include <errno.h>
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <nuttx/spi.h>
|
||||
#include <arch/board/board.h>
|
||||
#include <nuttx/arch.h>
|
||||
|
||||
#include "chip.h"
|
||||
#include "stm32_internal.h"
|
||||
#include "px4fmu-internal.h"
|
||||
|
||||
#include <arch/board/drv_mpu6000.h>
|
||||
|
||||
#define DIR_READ (0x80)
|
||||
#define DIR_WRITE (0<<7)
|
||||
#define ADDR_INCREMENT (1<<6)
|
||||
|
||||
#define WHO_I_AM 0xD4
|
||||
|
||||
// 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 // Product ID Register
|
||||
|
||||
|
||||
// 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 // 0001 0100
|
||||
#define MPU6000ES_REV_C5 0x15 // 0001 0101
|
||||
#define MPU6000ES_REV_D6 0x16 // 0001 0110
|
||||
#define MPU6000ES_REV_D7 0x17 // 0001 0111
|
||||
#define MPU6000ES_REV_D8 0x18 // 0001 1000
|
||||
#define MPU6000_REV_C4 0x54 // 0101 0100
|
||||
#define MPU6000_REV_C5 0x55 // 0101 0101
|
||||
#define MPU6000_REV_D6 0x56 // 0101 0110
|
||||
#define MPU6000_REV_D7 0x57 // 0101 0111
|
||||
#define MPU6000_REV_D8 0x58 // 0101 1000
|
||||
#define MPU6000_REV_D9 0x59 // 0101 1001
|
||||
#define MPU6000_REV_D10 0x5A // 0101 1010
|
||||
|
||||
static FAR struct mpu6000_dev_s mpu6000_dev;
|
||||
|
||||
static ssize_t mpu6000_read(struct file *filp, FAR char *buffer, size_t buflen);
|
||||
static int mpu6000_ioctl(struct file *filp, int cmd, unsigned long arg);
|
||||
|
||||
static const struct file_operations mpu6000_fops = {
|
||||
.open = 0,
|
||||
.close = 0,
|
||||
.read = mpu6000_read,
|
||||
.write = 0,
|
||||
.seek = 0,
|
||||
.ioctl = mpu6000_ioctl,
|
||||
#ifndef CONFIG_DISABLE_POLL
|
||||
.poll = 0
|
||||
#endif
|
||||
};
|
||||
|
||||
struct mpu6000_dev_s
|
||||
{
|
||||
struct spi_dev_s *spi;
|
||||
int spi_id;
|
||||
uint8_t rate;
|
||||
struct mpu6000_buffer *buffer;
|
||||
};
|
||||
|
||||
static void mpu6000_write_reg(uint8_t address, uint8_t data);
|
||||
static uint8_t mpu6000_read_reg(uint8_t address);
|
||||
|
||||
static void
|
||||
mpu6000_write_reg(uint8_t address, uint8_t data)
|
||||
{
|
||||
uint8_t cmd[2] = { address | DIR_WRITE, data };
|
||||
|
||||
SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, true);
|
||||
SPI_SNDBLOCK(mpu6000_dev.spi, &cmd, sizeof(cmd));
|
||||
SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, false);
|
||||
}
|
||||
|
||||
static uint8_t
|
||||
mpu6000_read_reg(uint8_t address)
|
||||
{
|
||||
uint8_t cmd[2] = {address | DIR_READ, 0};
|
||||
uint8_t data[2];
|
||||
|
||||
SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, true);
|
||||
SPI_EXCHANGE(mpu6000_dev.spi, cmd, data, sizeof(cmd));
|
||||
SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, false);
|
||||
|
||||
return data[1];
|
||||
}
|
||||
|
||||
static int16_t
|
||||
mpu6000_read_int16(uint8_t address)
|
||||
{
|
||||
uint8_t cmd[3] = {address | DIR_READ, 0, 0};
|
||||
uint8_t data[3];
|
||||
|
||||
SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, true);
|
||||
SPI_EXCHANGE(mpu6000_dev.spi, cmd, data, sizeof(cmd));
|
||||
SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, false);
|
||||
|
||||
return (((int16_t)data[1])<<8) | data[2];
|
||||
}
|
||||
|
||||
static int
|
||||
mpu6000_set_range(uint8_t range)
|
||||
{
|
||||
// /* mask out illegal bit positions */
|
||||
// uint8_t write_range = range & REG4_RANGE_MASK;
|
||||
// /* immediately return if user supplied invalid value */
|
||||
// if (write_range != range) return EINVAL;
|
||||
// /* set remaining bits to a sane value */
|
||||
// write_range |= REG4_BDU;
|
||||
// /* write to device */
|
||||
// write_reg(ADDR_CTRL_REG4, write_range);
|
||||
// /* return 0 if register value is now written value, 1 if unchanged */
|
||||
// return !(read_reg(ADDR_CTRL_REG4) == write_range);
|
||||
}
|
||||
|
||||
static int
|
||||
mpu6000_set_rate(uint8_t rate)
|
||||
{
|
||||
// /* mask out illegal bit positions */
|
||||
// uint8_t write_rate = rate & REG1_RATE_LP_MASK;
|
||||
// /* immediately return if user supplied invalid value */
|
||||
// if (write_rate != rate) return EINVAL;
|
||||
// /* set remaining bits to a sane value */
|
||||
// write_rate |= REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
|
||||
// /* write to device */
|
||||
// write_reg(ADDR_CTRL_REG1, write_rate);
|
||||
// /* return 0 if register value is now written value, 1 if unchanged */
|
||||
// return !(read_reg(ADDR_CTRL_REG1) == write_rate);
|
||||
}
|
||||
|
||||
static int
|
||||
mpu6000_read_fifo(int16_t *data)
|
||||
{
|
||||
// struct { /* status register and data as read back from the device */
|
||||
// uint8_t cmd;
|
||||
// uint8_t temp;
|
||||
// uint8_t status;
|
||||
// int16_t x;
|
||||
// int16_t y;
|
||||
// int16_t z;
|
||||
// } __attribute__((packed)) report;
|
||||
//
|
||||
// report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
|
||||
//
|
||||
// /* exchange the report structure with the device */
|
||||
// SPI_LOCK(mpu6000_dev.spi, true);
|
||||
//
|
||||
// SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, true);
|
||||
//
|
||||
// read_reg(ADDR_WHO_AM_I);
|
||||
//
|
||||
// SPI_EXCHANGE(mpu6000_dev.spi, &report, &report, sizeof(report));
|
||||
// SPI_SELECT(mpu6000_dev.spi, mpu6000_dev.spi_id, false);
|
||||
//
|
||||
// SPI_LOCK(mpu6000_dev.spi, false);
|
||||
//
|
||||
//
|
||||
//
|
||||
//
|
||||
|
||||
// Device has MSB first at lower address (big endian)
|
||||
|
||||
|
||||
struct { /* status register and data as read back from the device */
|
||||
// uint8_t cmd;
|
||||
// uint8_t int_status;
|
||||
int16_t xacc;
|
||||
int16_t yacc;
|
||||
int16_t zacc;
|
||||
int16_t temp;
|
||||
int16_t rollspeed;
|
||||
int16_t pitchspeed;
|
||||
int16_t yawspeed;
|
||||
} report;
|
||||
|
||||
uint8_t cmd[sizeof(report)];
|
||||
cmd[0] = MPUREG_ACCEL_XOUT_H | DIR_READ; // was addr_incr
|
||||
|
||||
SPI_LOCK(mpu6000_dev.spi, true);
|
||||
SPI_SELECT(mpu6000_dev.spi, PX4_SPIDEV_MPU, true);
|
||||
report.xacc = mpu6000_read_int16(MPUREG_ACCEL_XOUT_H);
|
||||
report.yacc = mpu6000_read_int16(MPUREG_ACCEL_YOUT_H);
|
||||
report.zacc = mpu6000_read_int16(MPUREG_ACCEL_ZOUT_H);
|
||||
report.rollspeed = mpu6000_read_int16(MPUREG_GYRO_XOUT_H);
|
||||
report.pitchspeed = mpu6000_read_int16(MPUREG_GYRO_YOUT_H);
|
||||
report.yawspeed = mpu6000_read_int16(MPUREG_GYRO_ZOUT_H);
|
||||
SPI_SELECT(mpu6000_dev.spi, PX4_SPIDEV_MPU, false);
|
||||
SPI_LOCK(mpu6000_dev.spi, false);
|
||||
|
||||
data[0] = report.xacc;
|
||||
data[1] = report.yacc;
|
||||
data[2] = report.zacc;
|
||||
data[3] = report.rollspeed;
|
||||
data[4] = report.pitchspeed;
|
||||
data[5] = report.yawspeed;
|
||||
|
||||
return 1;//(report.int_status & 0x01);
|
||||
}
|
||||
|
||||
static ssize_t
|
||||
mpu6000_read(struct file *filp, char *buffer, size_t buflen)
|
||||
{
|
||||
/* if the buffer is large enough, and data are available, return success */
|
||||
if (buflen >= 12) {
|
||||
if (mpu6000_read_fifo((int16_t *)buffer))
|
||||
return 12;
|
||||
|
||||
/* no data */
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* buffer too small */
|
||||
errno = ENOSPC;
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
static int
|
||||
mpu6000_ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
int result = ERROR;
|
||||
|
||||
switch (cmd) {
|
||||
case MPU6000_SETRATE:
|
||||
if ((arg & 0x00/* XXX REG MASK MISSING */) == arg) {
|
||||
SPI_LOCK(mpu6000_dev.spi, true);
|
||||
mpu6000_set_rate(arg);
|
||||
SPI_LOCK(mpu6000_dev.spi, false);
|
||||
result = 0;
|
||||
mpu6000_dev.rate = arg;
|
||||
}
|
||||
break;
|
||||
|
||||
case MPU6000_SETRANGE:
|
||||
if ((arg & 0x00/* XXX REG MASK MISSING */) == arg) {
|
||||
SPI_LOCK(mpu6000_dev.spi, true);
|
||||
mpu6000_set_range(arg);
|
||||
SPI_LOCK(mpu6000_dev.spi, false);
|
||||
result = 0;
|
||||
}
|
||||
break;
|
||||
|
||||
case MPU6000_SETBUFFER:
|
||||
mpu6000_dev.buffer = (struct mpu6000_buffer *)arg;
|
||||
result = 0;
|
||||
break;
|
||||
}
|
||||
|
||||
if (result)
|
||||
errno = EINVAL;
|
||||
return result;
|
||||
}
|
||||
|
||||
int
|
||||
mpu6000_attach(struct spi_dev_s *spi, int spi_id)
|
||||
{
|
||||
int result = ERROR;
|
||||
|
||||
mpu6000_dev.spi = spi;
|
||||
mpu6000_dev.spi_id = spi_id;
|
||||
|
||||
SPI_LOCK(mpu6000_dev.spi, true);
|
||||
|
||||
// Set sensor-specific SPI mode
|
||||
SPI_SETFREQUENCY(mpu6000_dev.spi, 10000000); // 500 KHz
|
||||
SPI_SETBITS(mpu6000_dev.spi, 8);
|
||||
// Either mode 1 or mode 3
|
||||
SPI_SETMODE(mpu6000_dev.spi, SPIDEV_MODE3);
|
||||
|
||||
// Chip reset
|
||||
mpu6000_write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
|
||||
up_udelay(10000);
|
||||
// Wake up device and select GyroZ clock (better performance)
|
||||
mpu6000_write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
|
||||
up_udelay(1000);
|
||||
// Disable I2C bus (recommended on datasheet)
|
||||
mpu6000_write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
|
||||
up_udelay(1000);
|
||||
// SAMPLE RATE
|
||||
mpu6000_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)
|
||||
mpu6000_write_reg(MPUREG_CONFIG, BITS_DLPF_CFG_98HZ);
|
||||
usleep(1000);
|
||||
mpu6000_write_reg(MPUREG_GYRO_CONFIG,BITS_FS_2000DPS); // Gyro scale 2000¼/s
|
||||
usleep(1000);
|
||||
|
||||
uint8_t _product_id = mpu6000_read_reg(MPUREG_PRODUCT_ID);
|
||||
printf("MPU-6000 product id: %d\n", (int)_product_id);
|
||||
|
||||
if ((_product_id == MPU6000ES_REV_C4) || (_product_id == MPU6000ES_REV_C5) ||
|
||||
(_product_id == MPU6000_REV_C4) || (_product_id == MPU6000_REV_C5)){
|
||||
// Accel scale 8g (4096 LSB/g)
|
||||
// Rev C has different scaling than rev D
|
||||
mpu6000_write_reg(MPUREG_ACCEL_CONFIG,1<<3);
|
||||
} else {
|
||||
// Accel scale 8g (4096 LSB/g)
|
||||
mpu6000_write_reg(MPUREG_ACCEL_CONFIG,2<<3);
|
||||
}
|
||||
usleep(1000);
|
||||
|
||||
// INT CFG => Interrupt on Data Ready
|
||||
mpu6000_write_reg(MPUREG_INT_ENABLE,BIT_RAW_RDY_EN); // INT: Raw data ready
|
||||
usleep(1000);
|
||||
mpu6000_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);
|
||||
|
||||
/* revert back to normal bus mode */
|
||||
SPI_SETFREQUENCY(mpu6000_dev.spi, 10000000);
|
||||
SPI_SETBITS(mpu6000_dev.spi, 8);
|
||||
SPI_SETMODE(mpu6000_dev.spi, SPIDEV_MODE3);
|
||||
|
||||
/* verify that the device is attached and functioning */
|
||||
if ((_product_id == MPU6000ES_REV_C4) || (_product_id == MPU6000ES_REV_C5) ||
|
||||
(_product_id == MPU6000_REV_C4) || (_product_id == MPU6000_REV_C5) ||
|
||||
(_product_id == MPU6000_REV_D7) || (_product_id == MPU6000_REV_D8) ||
|
||||
(_product_id == MPU6000_REV_D9) || (_product_id == MPU6000_REV_D10)){
|
||||
|
||||
/* make ourselves available */
|
||||
register_driver("/dev/mpu6000", &mpu6000_fops, 0666, NULL);
|
||||
|
||||
result = OK;
|
||||
} else {
|
||||
|
||||
errno = EIO;
|
||||
}
|
||||
|
||||
SPI_LOCK(mpu6000_dev.spi, false);
|
||||
|
||||
SPI_LOCK(mpu6000_dev.spi, false);
|
||||
|
||||
return result;
|
||||
}
|
||||
@@ -45,6 +45,7 @@
|
||||
#include <debug.h>
|
||||
|
||||
#include <sys/time.h>
|
||||
#include <sched.h>
|
||||
|
||||
#include <arch/board/board.h>
|
||||
#include <arch/board/up_hrt.h>
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
/****************************************************************************
|
||||
* config/stm3210e_eval/src/up_nsh.c
|
||||
* config/px4fmu/src/up_nsh.c
|
||||
* arch/arm/src/board/up_nsh.c
|
||||
*
|
||||
* Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved.
|
||||
@@ -63,11 +63,8 @@
|
||||
#include <arch/board/board.h>
|
||||
#include <arch/board/drv_bma180.h>
|
||||
#include <arch/board/drv_l3gd20.h>
|
||||
#include <arch/board/drv_hmc5883l.h>
|
||||
#include <arch/board/drv_mpu6000.h>
|
||||
#include <arch/board/drv_ms5611.h>
|
||||
#include <arch/board/drv_eeprom.h>
|
||||
#include <arch/board/drv_led.h>
|
||||
#include <arch/board/drv_eeprom.h>
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-Processor Definitions
|
||||
@@ -95,20 +92,6 @@
|
||||
* Protected Functions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Name: multiport_setup
|
||||
*
|
||||
* Description:
|
||||
* Perform setup of the PX4FMU's multi function ports
|
||||
*
|
||||
****************************************************************************/
|
||||
int multiport_setup(void)
|
||||
{
|
||||
int result = OK;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
@@ -221,7 +204,7 @@ int nsh_archinitialize(void)
|
||||
up_udelay(1000);
|
||||
}
|
||||
|
||||
if (gyro_fail) message("[boot] FAILED to attach L3GD20 gyro\r\n");
|
||||
if (!gyro_fail) message("[boot] Found L3GD20 gyro\n");
|
||||
|
||||
int acc_attempts = 0;
|
||||
int acc_fail = 0;
|
||||
@@ -234,13 +217,13 @@ int nsh_archinitialize(void)
|
||||
up_udelay(1000);
|
||||
}
|
||||
|
||||
if (acc_fail) message("[boot] FAILED to attach BMA180 accelerometer\r\n");
|
||||
if (!acc_fail) message("[boot] Found BMA180 accelerometer\n");
|
||||
|
||||
/* initialize I2C2 bus */
|
||||
|
||||
i2c2 = up_i2cinitialize(2);
|
||||
if (!i2c2) {
|
||||
message("[boot] FAILED to initialize I2C bus 2\r\n");
|
||||
message("[boot] FAILED to initialize I2C bus 2\n");
|
||||
up_ledon(LED_AMBER);
|
||||
return -ENODEV;
|
||||
}
|
||||
@@ -251,7 +234,7 @@ int nsh_archinitialize(void)
|
||||
|
||||
i2c3 = up_i2cinitialize(3);
|
||||
if (!i2c3) {
|
||||
message("[boot] FAILED to initialize I2C bus 3\r\n");
|
||||
message("[boot] FAILED to initialize I2C bus 3\n");
|
||||
up_ledon(LED_AMBER);
|
||||
return -ENODEV;
|
||||
}
|
||||
@@ -259,91 +242,41 @@ int nsh_archinitialize(void)
|
||||
/* set I2C3 speed */
|
||||
I2C_SETFREQUENCY(i2c3, 400000);
|
||||
|
||||
int mag_attempts = 0;
|
||||
int mag_fail = 0;
|
||||
|
||||
while (mag_attempts < 5)
|
||||
{
|
||||
mag_fail = hmc5883l_attach(i2c2);
|
||||
mag_attempts++;
|
||||
if (mag_fail == 0) break;
|
||||
up_udelay(1000);
|
||||
}
|
||||
|
||||
if (mag_fail) message("[boot] FAILED to attach HMC5883L magnetometer\r\n");
|
||||
|
||||
int baro_attempts = 0;
|
||||
int baro_fail = 0;
|
||||
while (baro_attempts < 5)
|
||||
{
|
||||
baro_fail = ms5611_attach(i2c2);
|
||||
baro_attempts++;
|
||||
if (baro_fail == 0) break;
|
||||
up_udelay(1000);
|
||||
}
|
||||
|
||||
if (baro_fail) message("[boot] FAILED to attach MS5611 baro at addr #1 or #2 (0x76 or 0x77)\r\n");
|
||||
|
||||
/* try to attach, don't fail if device is not responding */
|
||||
(void)eeprom_attach(i2c3, FMU_BASEBOARD_EEPROM_ADDRESS,
|
||||
FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES,
|
||||
FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES,
|
||||
FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/baseboard_eeprom", 1);
|
||||
|
||||
#if 0
|
||||
int eeprom_attempts = 0;
|
||||
int eeprom_fail;
|
||||
while (eeprom_attempts < 5)
|
||||
{
|
||||
/* try to attach, fail if device does not respond */
|
||||
eeprom_fail = eeprom_attach(i2c2, FMU_ONBOARD_EEPROM_ADDRESS,
|
||||
FMU_ONBOARD_EEPROM_TOTAL_SIZE_BYTES,
|
||||
FMU_ONBOARD_EEPROM_PAGE_SIZE_BYTES,
|
||||
FMU_ONBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/eeprom", 1);
|
||||
eeprom_attempts++;
|
||||
if (eeprom_fail == OK) break;
|
||||
up_udelay(1000);
|
||||
}
|
||||
|
||||
if (eeprom_fail) message("[boot] FAILED to attach FMU EEPROM\r\n");
|
||||
|
||||
#endif
|
||||
|
||||
/* Report back sensor status */
|
||||
if (gyro_fail || mag_fail || baro_fail/* || eeprom_fail*/)
|
||||
{
|
||||
up_ledon(LED_AMBER);
|
||||
}
|
||||
|
||||
#if defined(CONFIG_STM32_SPI3)
|
||||
/* Get the SPI port */
|
||||
|
||||
message("[boot] Initializing SPI port 3\r\n");
|
||||
message("[boot] Initializing SPI port 3\n");
|
||||
spi3 = up_spiinitialize(3);
|
||||
if (!spi3)
|
||||
{
|
||||
message("[boot] FAILED to initialize SPI port 3\r\n");
|
||||
message("[boot] FAILED to initialize SPI port 3\n");
|
||||
up_ledon(LED_AMBER);
|
||||
return -ENODEV;
|
||||
}
|
||||
message("[boot] Successfully initialized SPI port 3\r\n");
|
||||
message("[boot] Successfully initialized SPI port 3\n");
|
||||
|
||||
/* Now bind the SPI interface to the MMCSD driver */
|
||||
result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3);
|
||||
if (result != OK)
|
||||
{
|
||||
message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\r\n");
|
||||
message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n");
|
||||
up_ledon(LED_AMBER);
|
||||
return -ENODEV;
|
||||
}
|
||||
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\r\n");
|
||||
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
|
||||
#endif /* SPI3 */
|
||||
|
||||
/* initialize I2C1 bus */
|
||||
|
||||
i2c1 = up_i2cinitialize(1);
|
||||
if (!i2c1) {
|
||||
message("[boot] FAILED to initialize I2C bus 1\r\n");
|
||||
message("[boot] FAILED to initialize I2C bus 1\n");
|
||||
up_ledon(LED_AMBER);
|
||||
return -ENODEV;
|
||||
}
|
||||
@@ -355,7 +288,7 @@ int nsh_archinitialize(void)
|
||||
|
||||
/* Get board information if available */
|
||||
|
||||
/* Initialize the user GPIOs */
|
||||
/* Initialize the user GPIOs */
|
||||
px4fmu_gpio_init();
|
||||
|
||||
#ifdef CONFIG_ADC
|
||||
@@ -367,7 +300,7 @@ int nsh_archinitialize(void)
|
||||
if (adc_state != OK)
|
||||
{
|
||||
/* Give up */
|
||||
message("[boot] FAILED adc_devinit: %d\r\n", adc_state);
|
||||
message("[boot] FAILED adc_devinit: %d\n", adc_state);
|
||||
return -ENODEV;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user