diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-24 11:40:16 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-24 11:40:42 +0200 |
commit | a1b99a3f03a5908ea7e263658343451440326aea (patch) | |
tree | 45b3d287c854c99d0655934dbbd4ca65238703b8 /nuttx | |
parent | a69c55f671b1a2116c0e6c497906844a81ce6c74 (diff) | |
download | px4-firmware-a1b99a3f03a5908ea7e263658343451440326aea.tar.gz px4-firmware-a1b99a3f03a5908ea7e263658343451440326aea.tar.bz2 px4-firmware-a1b99a3f03a5908ea7e263658343451440326aea.zip |
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
Diffstat (limited to 'nuttx')
-rw-r--r-- | nuttx/configs/px4fmu/include/drv_mpu6000.h | 92 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/nsh/appconfig | 2 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/src/Makefile | 4 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/src/drv_mpu6000.c | 433 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/src/up_cpuload.c | 1 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/src/up_nsh.c | 95 |
6 files changed, 17 insertions, 610 deletions
diff --git a/nuttx/configs/px4fmu/include/drv_mpu6000.h b/nuttx/configs/px4fmu/include/drv_mpu6000.h deleted file mode 100644 index 0a5a48b70..000000000 --- a/nuttx/configs/px4fmu/include/drv_mpu6000.h +++ /dev/null @@ -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); diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index a99b5f6ad..834b18165 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -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 diff --git a/nuttx/configs/px4fmu/src/Makefile b/nuttx/configs/px4fmu/src/Makefile index 2e3138aaa..5a60b7d19 100644 --- a/nuttx/configs/px4fmu/src/Makefile +++ b/nuttx/configs/px4fmu/src/Makefile @@ -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 diff --git a/nuttx/configs/px4fmu/src/drv_mpu6000.c b/nuttx/configs/px4fmu/src/drv_mpu6000.c deleted file mode 100644 index aded70318..000000000 --- a/nuttx/configs/px4fmu/src/drv_mpu6000.c +++ /dev/null @@ -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; -} diff --git a/nuttx/configs/px4fmu/src/up_cpuload.c b/nuttx/configs/px4fmu/src/up_cpuload.c index 750ec4852..bf867ae8b 100644 --- a/nuttx/configs/px4fmu/src/up_cpuload.c +++ b/nuttx/configs/px4fmu/src/up_cpuload.c @@ -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> diff --git a/nuttx/configs/px4fmu/src/up_nsh.c b/nuttx/configs/px4fmu/src/up_nsh.c index d185bf7f1..25f3c2795 100644 --- a/nuttx/configs/px4fmu/src/up_nsh.c +++ b/nuttx/configs/px4fmu/src/up_nsh.c @@ -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 @@ -96,20 +93,6 @@ ****************************************************************************/ /**************************************************************************** - * 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; } } |