diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index ca43114198f1..aa8edf50273c 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -10,6 +10,7 @@ set(config_module_list drivers/batt_smbus drivers/camera_trigger drivers/gps + drivers/imu/mpu9250 drivers/linux_gpio drivers/pwm_out_sim drivers/vmount diff --git a/src/drivers/imu/mpu9250/AK8963.h b/src/drivers/imu/mpu9250/AK8963.h new file mode 100644 index 000000000000..3606115e4a4f --- /dev/null +++ b/src/drivers/imu/mpu9250/AK8963.h @@ -0,0 +1,11 @@ + +// ak8963 register address and bit definitions +#define AK8963_I2C_ADDR 0x0C +#define AK8963_DEVICE_ID 0x48 +#define AK8963REG_WIA 0x00 +#define AK8963REG_ST1 0x02 +#define AK8963REG_CNTL1_ADDR 0x0A +#define AK8963REG_CNTL2_ADDR 0x0B +#define AK8963_CONTINUOUS_16BIT 0x16 +#define AK8963_SELFTEST_MODE 0x08 +#define AK8963_RESET 0x01 diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp index f9fc73239e5a..89ee446c0a40 100644 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ b/src/drivers/imu/mpu9250/mpu9250.cpp @@ -57,11 +57,10 @@ MPU9250::MPU9250(mpu9250::mpu9250_bus_option &bus, enum Rotation rotation) : _fifo_maxed(perf_alloc(PC_COUNT, "mpu9250_fifo_maxed")), _rotation(rotation) - { if (_interface->init() != OK) { delete _interface; - warnx("No device on bus %u.", (unsigned)bus.busid); + PX4_ERR("No device on bus %u.", (unsigned)bus.busid); } probe(); @@ -70,8 +69,10 @@ MPU9250::MPU9250(mpu9250::mpu9250_bus_option &bus, enum Rotation rotation) : _mag = new Mag(bus.mag_path, _interface, DRV_MAG_DEVTYPE_MPU9250); } +#ifdef CONFIG_FAT_DMAMEMORY // Allocate 512 bytes for dma FIFO transfer. _dma_data_buffer = (uint8_t *)fat_dma_alloc(_dma_buffer_size); +#endif } MPU9250::~MPU9250() @@ -84,7 +85,9 @@ MPU9250::~MPU9250() delete _mag; } +#ifdef CONFIG_FAT_DMAMEMORY fat_dma_free(_dma_data_buffer, _dma_buffer_size); +#endif hrt_cancel(&_hrt_call); px4_sem_destroy(&_data_semaphore); @@ -96,21 +99,19 @@ MPU9250::~MPU9250() mpu9250::mpu9250_bus_option &MPU9250::initialize_bus(mpu9250::MPU9250_BUS bus_id) { - uint8_t i = 0; - - for (i = 0; i < NUM_BUS_OPTIONS; i++) { + for (uint8_t i = 0; i < NUM_BUS_OPTIONS; i++) { if (bus_id == mpu9250::g_bus_options[i].busid) { return mpu9250::g_bus_options[i]; } } PX4_ERR("Could not find bus."); - return mpu9250::g_bus_options[i]; + return mpu9250::g_bus_options[0]; } MPU9250 *MPU9250::instantiate(int argc, char *argv[]) { - int ch; + int ch = 0; int myoptind = 1; const char *myoptarg = nullptr; @@ -159,7 +160,6 @@ MPU9250 *MPU9250::instantiate(int argc, char *argv[]) return mpu9250; } - int MPU9250::task_spawn(int argc, char *argv[]) { int task_id = px4_task_spawn_cmd("mpu9250", SCHED_DEFAULT, @@ -171,7 +171,6 @@ int MPU9250::task_spawn(int argc, char *argv[]) int MPU9250::custom_command(int argc, char *argv[]) { - return 0; } @@ -262,8 +261,6 @@ void MPU9250::clean_reset() write_reg(USER_CTRL_ADDR, USER_CTRL_FIFO_EN); } - - int MPU9250::probe() { _whoami = read_reg(WHOAMI_ADDR); @@ -281,7 +278,6 @@ int MPU9250::probe() return PX4_ERROR; } - int MPU9250::self_test() { // if (perf_event_count(_sample_perf) == 0) { @@ -295,7 +291,7 @@ int MPU9250::self_test() uint8_t MPU9250::read_reg(unsigned reg) { - uint8_t buf[2] = {0}; + uint8_t buf[2] = {}; _interface->read(reg, buf, 2); @@ -304,10 +300,7 @@ uint8_t MPU9250::read_reg(unsigned reg) void MPU9250::write_reg(unsigned reg, uint8_t value) { - uint8_t buf[2] = {0}; - - buf[0] = value; - + uint8_t buf[2] = { value, 0 }; _interface->write(reg, buf, 2); } @@ -367,13 +360,13 @@ void MPU9250::run() unsigned samples = fifo_count / 12; - float accel_x = 0; - float accel_y = 0; - float accel_z = 0; + float accel_x = 0.0f; + float accel_y = 0.0f; + float accel_z = 0.0f; - float gyro_x = 0; - float gyro_y = 0; - float gyro_z = 0; + float gyro_x = 0.0f; + float gyro_y = 0.0f; + float gyro_z = 0.0f; for (unsigned i = 0; i < samples; i++) { accel_x += int16_t(_dma_data_buffer[_offset + 0 + i * 12] << 8 | _dma_data_buffer[_offset + 1 + i * 12]); @@ -407,9 +400,9 @@ void MPU9250::run() _interface->read(EXT_SENS_DATA_00, _dma_data_buffer, 8 + _offset); - float x = (uint16_t)(_dma_data_buffer[3] << 8 | _dma_data_buffer[2]); - float y = (uint16_t)(_dma_data_buffer[5] << 8 | _dma_data_buffer[4]); - float z = (uint16_t)(_dma_data_buffer[7] << 8 | _dma_data_buffer[6]); + float x = (uint16_t)(_dma_data_buffer[3] << 8 | _dma_data_buffer[2]); + float y = (uint16_t)(_dma_data_buffer[5] << 8 | _dma_data_buffer[4]); + float z = (uint16_t)(_dma_data_buffer[7] << 8 | _dma_data_buffer[6]); // Now bring the mag into same reference frame as the accel and gyro. int16_t temp = x; @@ -426,18 +419,13 @@ void MPU9250::run() perf_end(_cycle); } - } - - int MPU9250::print_status() { - return PX4_OK; } - int MPU9250::print_usage() { PRINT_MODULE_DESCRIPTION( diff --git a/src/drivers/imu/mpu9250/mpu9250.h b/src/drivers/imu/mpu9250/mpu9250.h index dbc215b620a1..6d469470f0c6 100644 --- a/src/drivers/imu/mpu9250/mpu9250.h +++ b/src/drivers/imu/mpu9250/mpu9250.h @@ -34,19 +34,19 @@ #pragma once #include -#include -#include -#include - +#include +#include +#include #include - -#include - #include - #include #include +#include "AK8963.h" + +#ifdef CONFIG_FAT_DMAMEMORY +#include +#endif // MPU9250 registers #define WHOAMI_ADDR 0x75 @@ -121,19 +121,6 @@ #define EXT_SENS_DATA_00 0x49 - -// ak8963 register address and bit definitions -#define AK8963_I2C_ADDR 0x0C -#define AK8963_DEVICE_ID 0x48 -#define AK8963REG_WIA 0x00 -#define AK8963REG_ST1 0x02 -#define AK8963REG_CNTL1_ADDR 0x0A -#define AK8963REG_CNTL2_ADDR 0x0B -#define AK8963_CONTINUOUS_16BIT 0x16 -#define AK8963_SELFTEST_MODE 0x08 -#define AK8963_RESET 0x01 - - #define ACCEL_SAMPLE_RATE 1000 #define ACCEL_FILTER_FREQ 30 #define ACCEL_FS_RANGE_M_S2 32.0f * CONSTANTS_ONE_G @@ -147,6 +134,7 @@ #define MAG_FS_RANGE_UT 9600.0f + #define MPU_DEVICE_PATH_ACCEL "/dev/mpu9250_accel" #define MPU_DEVICE_PATH_GYRO "/dev/mpu9250_gyro" #define MPU_DEVICE_PATH_MAG "/dev/mpu9250_mag" @@ -261,7 +249,7 @@ class MPU9250 : public ModuleBaseMulti void init(); /** @brief Starts the driver and kicks off the main loop. */ - void run(); + void run() override; /** * @brief Prints the status of the driver. @@ -275,7 +263,6 @@ class MPU9250 : public ModuleBaseMulti */ int probe(); - private: device::Device *_interface = nullptr; @@ -289,7 +276,7 @@ class MPU9250 : public ModuleBaseMulti uint8_t _whoami = 0; - struct hrt_call _hrt_call; + struct hrt_call _hrt_call {}; perf_counter_t _spi_transfer; perf_counter_t _cycle; @@ -297,7 +284,7 @@ class MPU9250 : public ModuleBaseMulti unsigned _offset = 1; - uint8_t *_dma_data_buffer; + uint8_t *_dma_data_buffer{nullptr}; static constexpr unsigned _dma_buffer_size = 544; // 32B block size @@ -312,7 +299,6 @@ class MPU9250 : public ModuleBaseMulti */ uint8_t read_reg(unsigned reg); - /** * @brief Write a register. * @param The register to write. @@ -320,7 +306,6 @@ class MPU9250 : public ModuleBaseMulti */ void write_reg(unsigned reg, uint8_t value); - /** * @brief Measurement self test * @return PX4_OK on success, PX4_ERROR on failure diff --git a/src/drivers/imu/mpu9250/mpu9250_i2c.cpp b/src/drivers/imu/mpu9250/mpu9250_i2c.cpp index e588af413a5a..cb566e6ceda4 100644 --- a/src/drivers/imu/mpu9250/mpu9250_i2c.cpp +++ b/src/drivers/imu/mpu9250/mpu9250_i2c.cpp @@ -56,15 +56,13 @@ class MPU9250_I2C : public device::I2C { public: MPU9250_I2C(int bus, uint32_t address); - virtual ~MPU9250_I2C() = default; + ~MPU9250_I2C() override = default; - 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); + int read(unsigned address, void *data, unsigned count) override; + int write(unsigned address, void *data, unsigned count) override; protected: - virtual int probe(); + int probe() override; }; @@ -77,33 +75,12 @@ MPU9250_I2C_interface(int bus, uint32_t address) MPU9250_I2C::MPU9250_I2C(int bus, uint32_t address) : I2C("MPU9250_I2C", nullptr, bus, address, 400000) { - _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU9250; -} - -int -MPU9250_I2C::ioctl(unsigned operation, unsigned &arg) -{ - int ret = PX4_ERROR; - - switch (operation) { - - case ACCELIOCGEXTERNAL: - return external(); - - case DEVIOCGDEVICEID: - return CDev::ioctl(nullptr, operation, arg); - - default: - ret = -EINVAL; - } - - return ret; } int MPU9250_I2C::write(unsigned reg_speed, void *data, unsigned count) { - uint8_t cmd[2]; + uint8_t cmd[2] {}; PX4_INFO("what am i doing here"); diff --git a/src/drivers/imu/mpu9250/mpu9250_spi.cpp b/src/drivers/imu/mpu9250/mpu9250_spi.cpp index 35055b3ce67b..14e853141dec 100644 --- a/src/drivers/imu/mpu9250/mpu9250_spi.cpp +++ b/src/drivers/imu/mpu9250/mpu9250_spi.cpp @@ -43,25 +43,10 @@ #include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include +#include #include #include -//#include "mpu9250.h" -#include - #define DIR_READ 0x80 #define DIR_WRITE 0x00 @@ -69,10 +54,8 @@ #define WHOAMI_9250 0x71 #define WHOAMI_6500 0x70 - device::Device *MPU9250_SPI_interface(int bus, uint32_t cs); - class MPU9250_SPI : public device::SPI { public: @@ -80,19 +63,18 @@ class MPU9250_SPI : public device::SPI ~MPU9250_SPI() = default; int read(unsigned reg, void *data, unsigned count) override; - //virtual int read_jake(unsigned reg, void* data, unsigned count); int write(unsigned reg, void *data, unsigned count) override; - int ioctl(unsigned operation, unsigned &arg); protected: - int probe(); + + int probe() override; }; device::Device *MPU9250_SPI_interface(int bus, uint32_t cs) { device::Device *interface = nullptr; - if (cs != SPIDEV_NONE(0)) { + if (cs != 0) { interface = new MPU9250_SPI(bus, cs); } @@ -110,30 +92,6 @@ MPU9250_SPI::MPU9250_SPI(int bus, uint32_t device) : set_lockmode(LOCK_THREADS); } -int MPU9250_SPI::ioctl(unsigned operation, unsigned &arg) -{ - int ret; - - PX4_INFO("Spi getting ioctl'd"); - - switch (operation) { - case ACCELIOCGEXTERNAL: - external(); - - // FALLTHROUGH - - case DEVIOCGDEVICEID: - return CDev::ioctl(nullptr, operation, arg); - - default: { - ret = -EINVAL; - } - } - - return ret; - //return PX4_OK; -} - int MPU9250_SPI::write(unsigned reg, void *data, unsigned count) { uint32_t clock_speed = 1e6; @@ -142,11 +100,9 @@ int MPU9250_SPI::write(unsigned reg, void *data, unsigned count) uint8_t temp = ((uint8_t *)data)[0]; - ((uint8_t *)data)[0] = reg | DIR_WRITE; ((uint8_t *)data)[1] = temp; - return transfer((uint8_t *)data, (uint8_t *)data, count); } @@ -155,7 +111,6 @@ int MPU9250_SPI::read(unsigned reg, void *data, unsigned count) // If we are doing dma read we do this uint32_t clock_speed = 20e6; - set_frequency(clock_speed); uint8_t reg_u8 = reg | DIR_READ; @@ -170,7 +125,7 @@ int MPU9250_SPI::read(unsigned reg, void *data, unsigned count) int MPU9250_SPI::probe() { - uint8_t whoami[2] = {0}; + uint8_t whoami[2] = {}; int ret = read(WHOAMI_ADDR, whoami, 2); diff --git a/src/lib/drivers/accelerometer/Accel.hpp b/src/lib/drivers/accelerometer/Accel.hpp index 08b7431b128d..d56d27509a4f 100644 --- a/src/lib/drivers/accelerometer/Accel.hpp +++ b/src/lib/drivers/accelerometer/Accel.hpp @@ -69,7 +69,7 @@ class Accel : public cdev::CDev int _orb_class_instance{-1}; - enum Rotation _rotation = ROTATION_NONE; + //enum Rotation _rotation = ROTATION_NONE; math::LowPassFilter2p _filter_x{1000, 100}; math::LowPassFilter2p _filter_y{1000, 100}; diff --git a/src/lib/drivers/device/posix/SPI.hpp b/src/lib/drivers/device/posix/SPI.hpp index ac02383786b2..1a7001e5ae49 100644 --- a/src/lib/drivers/device/posix/SPI.hpp +++ b/src/lib/drivers/device/posix/SPI.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2017 PX4 Development Team. All rights reserved. + * 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 @@ -40,14 +40,136 @@ #ifndef _DEVICE_SPI_H #define _DEVICE_SPI_H -#include "CDev.hpp" +#include "../CDev.hpp" #include namespace device __EXPORT { -// TODO: implement posix spi +/** + * Abstract class for character device on SPI + */ +class __EXPORT SPI : public CDev +{ +protected: + /** + * Constructor + * + * @param name Driver name + * @param devname Device node name + * @param bus SPI bus on which the device lives + * @param device Device handle (used by SPI_SELECT) + * @param mode SPI clock/data mode + * @param frequency SPI clock frequency + */ + SPI(const char *name, + const char *devname, + int bus, + uint32_t device, + enum spi_mode_e mode, + uint32_t frequency); + virtual ~SPI(); + + /** + * Locking modes supported by the driver. + */ + enum LockMode { + LOCK_PREEMPTION, /**< the default; lock against all forms of preemption. */ + LOCK_THREADS, /**< lock only against other threads, using SPI_LOCK */ + LOCK_NONE /**< perform no locking, only safe if the bus is entirely private */ + }; + + virtual int init(); + + /** + * Check for the presence of the device on the bus. + */ + virtual int probe() { return PX4_OK; } + + /** + * Perform a SPI transfer. + * + * If called from interrupt context, this interface does not lock + * the bus and may interfere with non-interrupt-context callers. + * + * Clients in a mixed interrupt/non-interrupt configuration must + * ensure appropriate interlocking. + * + * At least one of send or recv must be non-null. + * + * @param send Bytes to send to the device, or nullptr if + * no data is to be sent. + * @param recv Buffer for receiving bytes from the device, + * or nullptr if no bytes are to be received. + * @param len Number of bytes to transfer. + * @return OK if the exchange was successful, -errno + * otherwise. + */ + int transfer(uint8_t *send, uint8_t *recv, unsigned len); + + /** + * Perform a SPI 16 bit transfer. + * + * If called from interrupt context, this interface does not lock + * the bus and may interfere with non-interrupt-context callers. + * + * Clients in a mixed interrupt/non-interrupt configuration must + * ensure appropriate interlocking. + * + * At least one of send or recv must be non-null. + * + * @param send Words to send to the device, or nullptr if + * no data is to be sent. + * @param recv Words for receiving bytes from the device, + * or nullptr if no bytes are to be received. + * @param len Number of words to transfer. + * @return OK if the exchange was successful, -errno + * otherwise. + */ + int transferhword(uint16_t *send, uint16_t *recv, unsigned len); + + /** + * Set the SPI bus frequency + * This is used to change frequency on the fly. Some sensors + * (such as the MPU6000) need a lower frequency for setup + * registers and can handle higher frequency for sensor + * value registers + * + * @param frequency Frequency to set (Hz) + */ + void set_frequency(uint32_t frequency) { _frequency = frequency; } + + /** + * Set the SPI bus locking mode + * + * This set the SPI locking mode. For devices competing with NuttX SPI + * drivers on a bus the right lock mode is LOCK_THREADS. + * + * @param mode Locking mode + */ + void set_lockmode(enum LockMode mode) { locking_mode = mode; } + + LockMode locking_mode; /**< selected locking mode */ + +private: + uint32_t _device; + enum spi_mode_e _mode; + uint32_t _frequency; + //struct spi_dev_s *_dev; + + /* this class does not allow copying */ + SPI(const SPI &); + SPI operator=(const SPI &); + +protected: + int _transfer(uint8_t *send, uint8_t *recv, unsigned len); + + int _transferhword(uint16_t *send, uint16_t *recv, unsigned len); + + bool external() { return px4_spi_bus_external(get_device_bus()); } + +}; } // namespace device diff --git a/src/lib/drivers/gyroscope/Gyro.cpp b/src/lib/drivers/gyroscope/Gyro.cpp index e369fb6a8cd0..b24c77fea566 100644 --- a/src/lib/drivers/gyroscope/Gyro.cpp +++ b/src/lib/drivers/gyroscope/Gyro.cpp @@ -80,7 +80,7 @@ int Gyro::init() return PX4_OK; } -int Gyro::ioctl(struct file *filp, int cmd, unsigned long arg) +int Gyro::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) { PX4_INFO("gyro getting ioctl'd"); diff --git a/src/lib/drivers/gyroscope/Gyro.hpp b/src/lib/drivers/gyroscope/Gyro.hpp index 0d8fcf2f3a2b..47cbe66a6575 100644 --- a/src/lib/drivers/gyroscope/Gyro.hpp +++ b/src/lib/drivers/gyroscope/Gyro.hpp @@ -70,7 +70,7 @@ class Gyro : public cdev::CDev int _orb_class_instance{-1}; - enum Rotation _rotation = ROTATION_NONE; + //enum Rotation _rotation = ROTATION_NONE; math::LowPassFilter2p _filter_x{1000, 100}; math::LowPassFilter2p _filter_y{1000, 100}; diff --git a/src/lib/drivers/magnetometer/Mag.cpp b/src/lib/drivers/magnetometer/Mag.cpp index 3aadb6ec9e12..6f3c01d2a9bf 100644 --- a/src/lib/drivers/magnetometer/Mag.cpp +++ b/src/lib/drivers/magnetometer/Mag.cpp @@ -80,7 +80,7 @@ int Mag::init() return PX4_OK; } -int Mag::ioctl(struct file *filp, int cmd, unsigned long arg) +int Mag::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) { PX4_INFO("mag getting ioctl'd"); diff --git a/src/platforms/px4_module_multi.h b/src/platforms/px4_module_multi.h index 5462a28ba4cb..ef4ddf60bcce 100644 --- a/src/platforms/px4_module_multi.h +++ b/src/platforms/px4_module_multi.h @@ -240,16 +240,14 @@ class ModuleBaseMulti static int start_command_base(int argc, char *argv[]) { - // If no module created, create one. If module exits, user must explicity specify they want to create another. - if (!strcmp(argv[1], "+") == 0 && _module_list.get_head() != nullptr) { + if (!(strcmp(argv[1], "+") == 0) && _module_list.get_head() != nullptr) { PX4_ERR("Task already running."); return PX4_ERROR; } lock_module(); - // Task spawn should only fire off the px4_task_spawn function and then enter at run_trampoline() // OR // If WORK_QUEUE, this is the final function call from ModuleBaseMulti!!