diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 8cd1221ca99..9a4d12192ed 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -1,6 +1,6 @@ #!nsh # -# Standard startup script for PX4FMU v1, v2, v3 onboard sensor drivers. +# Standard startup script for PX4FMU v1, v2, v3, v4 onboard sensor drivers. # if ver hwcmp PX4FMU_V1 @@ -35,6 +35,11 @@ then then fi + # External I2C bus + if lis3mdl -X start + then + fi + # Internal I2C bus if hmc5883 -C -T -I -R 4 start then @@ -96,6 +101,10 @@ then then fi + if lis3mdl -R 2 start + then + fi + # Internal SPI bus is rotated 90 deg yaw if hmc5883 -C -T -S -R 2 start then @@ -163,6 +172,10 @@ then if hmc5883 -C -T -X start then fi + + if lis3mdl -R 2 start + then + fi fi if meas_airspeed start diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index adaa55752a9..3859b889943 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -47,6 +47,7 @@ set(config_module_list drivers/camera_trigger drivers/bst drivers/snapdragon_rc_pwm + drivers/lis3mdl # # System commands diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index dbc1040ff06..91653edc8e9 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -44,6 +44,7 @@ set(config_module_list drivers/camera_trigger drivers/bst drivers/snapdragon_rc_pwm + drivers/lis3mdl # # System commands diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 0d7f628036f..ab54edb121b 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -111,6 +111,7 @@ __BEGIN_DECLS #define GPIO_SPI_CS_EXT1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14) #define GPIO_SPI_CS_EXT2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) #define GPIO_SPI_CS_EXT3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) +#define GPIO_SPI_CS_LIS (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) #define PX4_SPI_BUS_SENSORS 1 #define PX4_SPI_BUS_RAMTRON 2 @@ -123,6 +124,7 @@ __BEGIN_DECLS #define PX4_SPIDEV_BARO 3 #define PX4_SPIDEV_MPU 4 #define PX4_SPIDEV_HMC 5 +#define PX4_SPIDEV_LIS 7 /* External bus */ #define PX4_SPIDEV_EXT0 1 @@ -147,6 +149,7 @@ __BEGIN_DECLS */ #define PX4_I2C_OBDEV_LED 0x55 #define PX4_I2C_OBDEV_HMC5883 0x1e +#define PX4_I2C_OBDEV_LIS3MDL 0x1e /* * ADC channels diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index a380e05bd5b..26808a6825f 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -150,6 +150,15 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); break; + case PX4_SPIDEV_LIS: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_LIS, !selected); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + break; + case PX4_SPIDEV_MPU: /* Making sure the other peripherals are not selected */ stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index d2fde3d30c8..6713524f87c 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -84,6 +84,7 @@ __BEGIN_DECLS #define GPIO_SPI_CS_MPU9250 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) #define GPIO_SPI_CS_HMC5983 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN15) +#define GPIO_SPI_CS_LIS3MDL (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN15) #define GPIO_SPI_CS_MS5611 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) #define GPIO_SPI_CS_ICM_20608_G (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) @@ -105,6 +106,7 @@ __BEGIN_DECLS #define GPIO_SPI_CS_OFF_MPU9250 _PIN_OFF(GPIO_SPI_CS_MPU9250) #define GPIO_SPI_CS_OFF_HMC5983 _PIN_OFF(GPIO_SPI_CS_HMC5983) +#define GPIO_SPI_CS_OFF_LIS3MDL _PIN_OFF(GPIO_SPI_CS_LIS3MDL) #define GPIO_SPI_CS_OFF_MS5611 _PIN_OFF(GPIO_SPI_CS_MS5611) #define GPIO_SPI_CS_OFF_ICM_20608_G _PIN_OFF(GPIO_SPI_CS_ICM_20608_G) @@ -128,6 +130,7 @@ __BEGIN_DECLS #define PX4_SPIDEV_MPU 4 #define PX4_SPIDEV_HMC 5 #define PX4_SPIDEV_ICM 6 +#define PX4_SPIDEV_LIS 7 /* onboard MS5611 and FRAM are both on bus SPI2 * spi_dev_e:SPIDEV_FLASH has the value 2 and is used in the NuttX ramtron driver @@ -148,6 +151,7 @@ __BEGIN_DECLS */ #define PX4_I2C_OBDEV_LED 0x55 #define PX4_I2C_OBDEV_HMC5883 0x1e +#define PX4_I2C_OBDEV_LIS3MDL 0x1e /* * ADC channels diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 95a9c974b5c..e28481b36dc 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -56,6 +56,7 @@ #define DRV_MAG_DEVTYPE_LSM303D 0x02 #define DRV_MAG_DEVTYPE_ACCELSIM 0x03 #define DRV_MAG_DEVTYPE_MPU9250 0x04 +#define DRV_MAG_DEVTYPE_LIS3MDL 0x05 #define DRV_ACC_DEVTYPE_LSM303D 0x11 #define DRV_ACC_DEVTYPE_BMA180 0x12 #define DRV_ACC_DEVTYPE_MPU6000 0x13 diff --git a/src/drivers/lis3mdl/CMakeLists.txt b/src/drivers/lis3mdl/CMakeLists.txt new file mode 100644 index 00000000000..9bf2c445f50 --- /dev/null +++ b/src/drivers/lis3mdl/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2016 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__lis3mdl + MAIN lis3mdl + + STACK 1200 + COMPILE_FLAGS + -Weffc++ + -Os + SRCS + lis3mdl_i2c.cpp + lis3mdl_spi.cpp + lis3mdl.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/lis3mdl/lis3mdl.cpp b/src/drivers/lis3mdl/lis3mdl.cpp new file mode 100644 index 00000000000..30e1e856111 --- /dev/null +++ b/src/drivers/lis3mdl/lis3mdl.cpp @@ -0,0 +1,1735 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2015 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 lis3mdl.cpp + * + * Driver for the LIS3MDL magnetometer connected via I2C or SPI. + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include "lis3mdl.h" + +#undef DEVICE_DEBUG +#define DEVICE_DEBUG printf + +/* + * LIS3MDL internal constants and data structures. + */ + +/* Max measurement rate is 80Hz */ +#define LIS3MDL_CONVERSION_INTERVAL (1000000 / 80) /* 12,500 microseconds */ + + +#define ADDR_CTRL_REG1 0x20 +#define ADDR_CTRL_REG2 0x21 +#define ADDR_CTRL_REG3 0x22 +#define ADDR_CTRL_REG4 0x23 +#define ADDR_CTRL_REG5 0x24 + +#define ADDR_STATUS_REG 0x27 +#define ADDR_OUT_X_L 0x28 +#define ADDR_OUT_X_H 0x29 +#define ADDR_OUT_Y_L 0x2a +#define ADDR_OUT_Y_H 0x2b +#define ADDR_OUT_Z_L 0x2c +#define ADDR_OUT_Z_H 0x2d +#define ADDR_OUT_T_L 0x2e +#define ADDR_OUT_T_H 0x2f + +#define MODE_REG_CONTINOUS_MODE (0 << 0) +#define MODE_REG_SINGLE_MODE (1 << 0) /* default */ + +enum LIS3MDL_BUS { + LIS3MDL_BUS_ALL = 0, + LIS3MDL_BUS_I2C_INTERNAL, + LIS3MDL_BUS_I2C_EXTERNAL, + LIS3MDL_BUS_SPI +}; + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +class LIS3MDL : public device::CDev +{ +public: + LIS3MDL(device::Device *interface, const char *path, enum Rotation rotation); + virtual ~LIS3MDL(); + + 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); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + Device *_interface; + +private: + work_s _work; + unsigned _measure_ticks; + + ringbuffer::RingBuffer *_reports; + mag_scale _scale; + float _range_scale; + float _range_ga; + bool _collect_phase; + int _class_instance; + int _orb_class_instance; + + orb_advert_t _mag_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + perf_counter_t _range_errors; + perf_counter_t _conf_errors; + + /* status reporting */ + bool _sensor_ok; /**< sensor was found and reports ok */ + bool _calibrated; /**< the calibration is valid */ + + enum Rotation _rotation; + + struct mag_report _last_report; /**< used for info() */ + + uint8_t _range_bits; + uint8_t _cntl_reg1; + uint8_t _cntl_reg4; + uint8_t _cntl_reg5; + uint8_t _temperature_counter; + uint8_t _temperature_error_count; + uint8_t _check_state_cnt; + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Reset the device + */ + int reset(); + + /** + * Perform the on-sensor scale calibration routine. + * + * @note The sensor will continue to provide measurements, these + * will however reflect the uncalibrated sensor state until + * the calibration routine has been completed. + * + * @param enable set to 1 to enable self-test strap, 0 to disable + */ + int calibrate(struct file *filp, unsigned enable); + + /** + * Perform the on-sensor scale calibration routine. + * + * @note The sensor will continue to provide measurements, these + * will however reflect the uncalibrated sensor state until + * the calibration routine has been completed. + * + * @param enable set to 1 to enable self-test positive strap, -1 to enable + * negative strap, 0 to set to normal mode + */ + int set_excitement(unsigned enable); + + /** + * Set the sensor range. + * + * Sets the internal range to handle at least the argument in Gauss. + */ + int set_range(unsigned range); + + /** + * check the sensor configuration. + * + * checks that the config of the sensor is correctly set, to + * cope with communication errors causing the configuration to + * change + */ + void check_conf(void); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + * + * This is the heart of the measurement state machine. This function + * alternately starts a measurement, or collects the data from the + * previous measurement. + * + * When the interval between measurements is greater than the minimum + * measurement interval, a gap is inserted between collection + * and measurement to provide the most recent measurement possible + * at the next interval. + */ + void cycle(); + + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + /** + * Write a register. + * + * @param reg The register to write. + * @param val The value to write. + * @return OK on write success. + */ + int write_reg(uint8_t reg, uint8_t val); + + /** + * Read a register. + * + * @param reg The register to read. + * @param val The value read. + * @return OK on read success. + */ + int read_reg(uint8_t reg, uint8_t &val); + + /** + * Issue a measurement command. + * + * @return OK if the measurement command was successful. + */ + int measure(); + + /** + * Collect the result of the most recent measurement. + */ + int collect(); + + /** + * Convert a big-endian signed 16-bit value to a float. + * + * @param in A signed 16-bit big-endian value. + * @return The floating-point representation of the value. + */ + float meas_to_float(uint8_t in[2]); + + /** + * Check the current calibration and update device status + * + * @return 0 if calibration is ok, 1 else + */ + int check_calibration(); + + /** + * Check the current scale calibration + * + * @return 0 if scale calibration is ok, 1 else + */ + int check_scale(); + + /** + * Check the current offset calibration + * + * @return 0 if offset calibration is ok, 1 else + */ + int check_offset(); + + /* this class has pointer data members, do not allow copying it */ + LIS3MDL(const LIS3MDL &); + LIS3MDL operator=(const LIS3MDL &); +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int lis3mdl_main(int argc, char *argv[]); + + +LIS3MDL::LIS3MDL(device::Device *interface, const char *path, enum Rotation rotation) : + CDev("LIS3MDL", path), + _interface(interface), + _work{}, + _measure_ticks(0), + _reports(nullptr), + _scale{}, + _range_scale(0), /* default range scale from counts to gauss */ + _range_ga(4.0f), + _collect_phase(false), + _class_instance(-1), + _orb_class_instance(-1), + _mag_topic(nullptr), + _sample_perf(perf_alloc(PC_ELAPSED, "lis3mdl_read")), + _comms_errors(perf_alloc(PC_COUNT, "lis3mdl_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "lis3mdl_buffer_overflows")), + _range_errors(perf_alloc(PC_COUNT, "lis3mdl_range_errors")), + _conf_errors(perf_alloc(PC_COUNT, "lis3mdl_conf_errors")), + _sensor_ok(false), + _calibrated(false), + _rotation(rotation), + _last_report{0}, + _range_bits(0), + _cntl_reg1(0), + _cntl_reg4(0), + _cntl_reg5(0), + _temperature_counter(0), + _temperature_error_count(0), + _check_state_cnt(0) +{ + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LIS3MDL; + + // enable debug() calls + _debug_enabled = false; + + // default scaling + _scale.x_offset = 0; + _scale.x_scale = 1.0f; + _scale.y_offset = 0; + _scale.y_scale = 1.0f; + _scale.z_offset = 0; + _scale.z_scale = 1.0f; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); + + _cntl_reg1 = 0xFC; + _cntl_reg4 = 0x0C; // Z-axis ultra high performance mode + _cntl_reg5 = 0x40; // block data update for magnetic data + +} + +LIS3MDL::~LIS3MDL() +{ + /* make sure we are truly inactive */ + stop(); + + if (_reports != nullptr) { + delete _reports; + } + + if (_class_instance != -1) { + unregister_class_devname(MAG_BASE_DEVICE_PATH, _class_instance); + } + + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); + perf_free(_range_errors); + perf_free(_conf_errors); +} + +int +LIS3MDL::init() +{ + int ret = ERROR; + + ret = CDev::init(); + + if (ret != OK) { + DEVICE_DEBUG("CDev init failed"); + goto out; + } + + /* allocate basic report buffers */ + _reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); + + if (_reports == nullptr) { + goto out; + } + + /* reset the device configuration */ + reset(); + + _class_instance = register_class_devname(MAG_BASE_DEVICE_PATH); + + ret = OK; + /* sensor is ok, but not calibrated */ + _sensor_ok = true; +out: + return ret; +} + +int LIS3MDL::set_range(unsigned range) +{ + if (range < 6) { + _range_bits = 0x00; + _range_scale = 1.0f / 6842.0f; + _range_ga = 4.0f; + + } else if (range <= 10) { + _range_bits = 0x01; + _range_scale = 1.0f / 3421.0f; + _range_ga = 8.0f; + + } else if (range <= 14) { + _range_bits = 0x02; + _range_scale = 1.0f / 2281.0f; + _range_ga = 12.0f; + + } else { + _range_bits = 0x03; + _range_scale = 1.0f / 1711.0f; + _range_ga = 16.0f; + } + + int ret; + + /* + * Send the command to set the range + */ + ret = write_reg(ADDR_CTRL_REG2, (_range_bits << 5)); + + if (OK != ret) { + perf_count(_comms_errors); + } + + uint8_t range_bits_in = 0; + ret = read_reg(ADDR_CTRL_REG2, range_bits_in); + + if (OK != ret) { + perf_count(_comms_errors); + } + + return !(range_bits_in == (_range_bits << 5)); +} + +/** + check that the configuration register has the right value. This is + done periodically to cope with I2C bus noise causing the + configuration of the compass to change. + */ +void LIS3MDL::check_conf(void) +{ + int ret; + uint8_t reg_in = 0; + + switch (_check_state_cnt++) { + case 0: + ret = read_reg(ADDR_CTRL_REG1, reg_in); + + if (OK != ret) { + perf_count(_comms_errors); + return; + } + + if (reg_in != _cntl_reg1) { + perf_count(_conf_errors); + ret = write_reg(ADDR_CTRL_REG1, _cntl_reg1); + + if (OK != ret) { + perf_count(_comms_errors); + } + } + break; + + case 50: + ret = read_reg(ADDR_CTRL_REG4, reg_in); + + if (OK != ret) { + perf_count(_comms_errors); + return; + } + if (reg_in != _cntl_reg4) { + perf_count(_conf_errors); + ret = write_reg(ADDR_CTRL_REG1, _cntl_reg4); + + if (OK != ret) { + perf_count(_comms_errors); + } + } + break; + + case 100: + ret = read_reg(ADDR_CTRL_REG5, reg_in); + + if (OK != ret) { + perf_count(_comms_errors); + return; + } + if (reg_in != _cntl_reg5) { + perf_count(_conf_errors); + ret = write_reg(ADDR_CTRL_REG5, _cntl_reg5); + + if (OK != ret) { + perf_count(_comms_errors); + } + } + break; + + case 150: + ret = read_reg(ADDR_CTRL_REG2, reg_in); + + if (OK != ret) { + perf_count(_comms_errors); + return; + } + + if (reg_in != (_range_bits << 5)) { + perf_count(_range_errors); + ret = write_reg(ADDR_CTRL_REG2, (_range_bits << 5)); + + if (OK != ret) { + perf_count(_comms_errors); + } + } + break; + + default: + break; + } +} + +ssize_t +LIS3MDL::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct mag_report); + struct mag_report *mag_buf = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_reports->get(mag_buf)) { + ret += sizeof(struct mag_report); + mag_buf++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + /* XXX really it'd be nice to lock against other readers here */ + do { + _reports->flush(); + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(LIS3MDL_CONVERSION_INTERVAL); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + if (_reports->get(mag_buf)) { + ret = sizeof(struct mag_report); + } + } while (0); + + return ret; +} + +int +LIS3MDL::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + unsigned dummy = arg; + + switch (cmd) { + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(LIS3MDL_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(LIS3MDL_CONVERSION_INTERVAL)) { + return -EINVAL; + } + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) { + return SENSOR_POLLRATE_MANUAL; + } + + return 1000000 / TICK2USEC(_measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + + case SENSORIOCRESET: + return reset(); + + case MAGIOCSSAMPLERATE: + /* same as pollrate because device is in single measurement mode*/ + return ioctl(filp, SENSORIOCSPOLLRATE, arg); + + case MAGIOCGSAMPLERATE: + /* same as pollrate because device is in single measurement mode*/ + return 1000000 / TICK2USEC(_measure_ticks); + + case MAGIOCSRANGE: + return set_range(arg); + + case MAGIOCGRANGE: + return _range_ga; + + case MAGIOCSLOWPASS: + case MAGIOCGLOWPASS: + /* not supported, no internal filtering */ + return -EINVAL; + + case MAGIOCSSCALE: + /* set new scale factors */ + memcpy(&_scale, (mag_scale *)arg, sizeof(_scale)); + /* check calibration, but not actually return an error */ + (void)check_calibration(); + return 0; + + case MAGIOCGSCALE: + /* copy out scale factors */ + memcpy((mag_scale *)arg, &_scale, sizeof(_scale)); + return 0; + + case MAGIOCCALIBRATE: + return calibrate(filp, arg); + + case MAGIOCEXSTRAP: + return set_excitement(arg); + + case MAGIOCSELFTEST: + return check_calibration(); + + case MAGIOCGEXTERNAL: + DEVICE_DEBUG("MAGIOCGEXTERNAL in main driver"); + return _interface->ioctl(cmd, dummy); + + case DEVIOCGDEVICEID: + return _interface->ioctl(cmd, dummy); + + default: + /* give it to the superclass */ + return CDev::ioctl(filp, cmd, arg); + } +} + +void +LIS3MDL::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&LIS3MDL::cycle_trampoline, this, 1); +} + +void +LIS3MDL::stop() +{ + work_cancel(HPWORK, &_work); +} + +int +LIS3MDL::reset() +{ + /* set range */ + return set_range(_range_ga); +} + +void +LIS3MDL::cycle_trampoline(void *arg) +{ + LIS3MDL *dev = (LIS3MDL *)arg; + + dev->cycle(); +} + +void +LIS3MDL::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + DEVICE_DEBUG("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(LIS3MDL_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&LIS3MDL::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(LIS3MDL_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) { + DEVICE_DEBUG("measure error"); + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&LIS3MDL::cycle_trampoline, + this, + USEC2TICK(LIS3MDL_CONVERSION_INTERVAL)); +} + +int +LIS3MDL::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + ret = write_reg(ADDR_CTRL_REG3, MODE_REG_SINGLE_MODE); + + if (OK != ret) { + perf_count(_comms_errors); + } + + return ret; +} + +int +LIS3MDL::collect() +{ +#pragma pack(push, 1) + struct { /* status register and data as read back from the device */ + uint8_t x[2]; + uint8_t y[2]; + uint8_t z[2]; + uint8_t t[2]; + } lis_report; + + struct { + int16_t x; + int16_t y; + int16_t z; + int16_t t; + } report; +#pragma pack(pop) + + int ret; +// uint8_t check_counter; + + perf_begin(_sample_perf); + struct mag_report new_report; + bool sensor_is_onboard = false; + + float xraw_f; + float yraw_f; + float zraw_f; + + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + new_report.timestamp = hrt_absolute_time(); + new_report.error_count = perf_event_count(_comms_errors); + + /* + * @note We could read the status register here, which could tell us that + * we were too early and that the output registers are still being + * written. In the common case that would just slow us down, and + * we're better off just never being early. + */ + + /* get measurements from the device */ + ret = _interface->read(ADDR_OUT_X_L, (uint8_t *)&lis_report, sizeof(lis_report)); + + if (ret != OK) { + perf_count(_comms_errors); + DEVICE_DEBUG("data/status read error"); + goto out; + } + + /* convert the data we just received */ + report.x = (((int16_t)lis_report.x[1]) << 8) + lis_report.x[0]; + report.y = (((int16_t)lis_report.y[1]) << 8) + lis_report.y[0]; + report.z = (((int16_t)lis_report.z[1]) << 8) + lis_report.z[0]; + report.t = (((int16_t)lis_report.t[1]) << 8) + lis_report.t[0]; + + /* get measurements from the device */ + new_report.temperature = report.t; + new_report.temperature = 25 + (report.t / (16 * 8.0f)); + + /* + * RAW outputs + * + * to align the sensor axes with the board, x and y need to be flipped + * and y needs to be negated + */ + new_report.x_raw = report.y; + new_report.y_raw = -report.x; + /* z remains z */ + new_report.z_raw = report.z; + + /* scale values for output */ + + // XXX revisit for SPI part, might require a bus type IOCTL + unsigned dummy; + sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy); + + if (sensor_is_onboard) { + // convert onboard so it matches offboard for the + // scaling below + report.y = -report.y; + report.x = -report.x; + } + + /* the standard external mag by 3DR has x pointing to the + * right, y pointing backwards, and z down, therefore switch x + * and y and invert y */ + xraw_f = -report.y; + yraw_f = report.x; + zraw_f = report.z; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + new_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale; + /* flip axes and negate value for y */ + new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale; + /* z remains z */ + new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale; + + if (!(_pub_blocked)) { + + if (_mag_topic != nullptr) { + /* publish it */ + orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); + + } else { + _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report, + &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX); + + if (_mag_topic == nullptr) { + DEVICE_DEBUG("ADVERT FAIL"); + } + } + } + + _last_report = new_report; + + /* post a report to the ring */ + if (_reports->force(&new_report)) { + perf_count(_buffer_overflows); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + check_conf(); + + ret = OK; + +out: + perf_end(_sample_perf); + return ret; +} + +int LIS3MDL::calibrate(struct file *filp, unsigned enable) +{ + struct mag_report report; + ssize_t sz; + int ret = 1; + uint8_t good_count = 0; + + // XXX do something smarter here + int fd = (int)enable; + + struct mag_scale mscale_previous = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + struct mag_scale mscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + float sum_excited[3] = {0.0f, 0.0f, 0.0f}; + + /* expected axis scaling. The datasheet says that 766 will + * be places in the X and Y axes and 713 in the Z + * axis. Experiments show that in fact 766 is placed in X, + * and 713 in Y and Z. This is relative to a base of 660 + * LSM/Ga, giving 1.16 and 1.08 */ + float expected_cal[3] = { 1.16f, 1.08f, 1.08f }; + + /* start the sensor polling at 50 Hz */ + if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) { + warn("FAILED: SENSORIOCSPOLLRATE 50Hz"); + ret = 1; + goto out; + } + + /* Set to 4 Gauss */ + if (OK != ioctl(filp, MAGIOCSRANGE, 4)) { + warnx("FAILED: MAGIOCSRANGE 4 Ga"); + ret = 1; + goto out; + } + + if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) { + warnx("FAILED: MAGIOCEXSTRAP 1"); + ret = 1; + goto out; + } + + if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) { + warn("FAILED: MAGIOCGSCALE 1"); + ret = 1; + goto out; + } + + if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { + warn("FAILED: MAGIOCSSCALE 1"); + ret = 1; + goto out; + } + + // discard 10 samples to let the sensor settle + for (uint8_t i = 0; i < 10; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = ::poll(&fds, 1, 2000); + + if (ret != 1) { + warn("ERROR: TIMEOUT 1"); + goto out; + } + + /* now go get it */ + sz = ::read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + warn("ERROR: READ 1"); + ret = -EIO; + goto out; + } + } + + /* read the sensor up to 100x, stopping when we have 30 good values */ + for (uint8_t i = 0; i < 100 && good_count < 30; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = ::poll(&fds, 1, 2000); + + if (ret != 1) { + warn("ERROR: TIMEOUT 2"); + goto out; + } + + /* now go get it */ + sz = ::read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + warn("ERROR: READ 2"); + ret = -EIO; + goto out; + } + + float cal[3] = {fabsf(expected_cal[0] / report.x), + fabsf(expected_cal[1] / report.y), + fabsf(expected_cal[2] / report.z) + }; + + if (cal[0] > 0.7f && cal[0] < 1.35f && + cal[1] > 0.7f && cal[1] < 1.35f && + cal[2] > 0.7f && cal[2] < 1.35f) { + good_count++; + sum_excited[0] += cal[0]; + sum_excited[1] += cal[1]; + sum_excited[2] += cal[2]; + } + } + + if (good_count < 5) { + ret = -EIO; + goto out; + } + + float scaling[3]; + + scaling[0] = sum_excited[0] / good_count; + scaling[1] = sum_excited[1] / good_count; + scaling[2] = sum_excited[2] / good_count; + + /* set scaling in device */ + mscale_previous.x_scale = 1.0f / scaling[0]; + mscale_previous.y_scale = 1.0f / scaling[1]; + mscale_previous.z_scale = 1.0f / scaling[2]; + + ret = OK; + +out: + + if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) { + warn("FAILED: MAGIOCSSCALE 2"); + } + + /* set back to normal mode */ + /* Set to 4 Gauss */ + if (OK != ::ioctl(fd, MAGIOCSRANGE, 4)) { + warnx("FAILED: MAGIOCSRANGE 4 Ga"); + } + + if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) { + warnx("FAILED: MAGIOCEXSTRAP 0"); + } + + if (ret == OK) { + if (check_scale()) { + /* failed */ + warnx("FAILED: SCALE"); + ret = ERROR; + } + + } + + return ret; +} + +int LIS3MDL::check_scale() +{ + bool scale_valid; + + if ((-FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < FLT_EPSILON + 1.0f) && + (-FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < FLT_EPSILON + 1.0f) && + (-FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < FLT_EPSILON + 1.0f)) { + /* scale is one */ + scale_valid = false; + + } else { + scale_valid = true; + } + + /* return 0 if calibrated, 1 else */ + return !scale_valid; +} + +int LIS3MDL::check_offset() +{ + bool offset_valid; + + if ((-2.0f * FLT_EPSILON < _scale.x_offset && _scale.x_offset < 2.0f * FLT_EPSILON) && + (-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) && + (-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) { + /* offset is zero */ + offset_valid = false; + + } else { + offset_valid = true; + } + + /* return 0 if calibrated, 1 else */ + return !offset_valid; +} + +int LIS3MDL::check_calibration() +{ + bool offset_valid = (check_offset() == OK); + bool scale_valid = (check_scale() == OK); + + if (_calibrated != (offset_valid && scale_valid)) { + warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ", + (offset_valid) ? "" : "offset invalid"); + _calibrated = (offset_valid && scale_valid); + } + + /* return 0 if calibrated, 1 else */ + return (!_calibrated); +} + +int LIS3MDL::set_excitement(unsigned enable) +{ + int ret; + /* arm the excitement strap */ + ret = read_reg(ADDR_CTRL_REG1, _cntl_reg1); + + if (OK != ret) { + perf_count(_comms_errors); + } + + _cntl_reg1 &= ~0x01; // reset previous excitement mode + + if (((int)enable) < 0) { + warnx("WARN: set_excitement negative not supported\n"); + } else if (enable > 0) { + _cntl_reg1 |= 0x01; + } + + ::printf("set_excitement enable=%d cntl1=0x%x\n", (int)enable, (unsigned)_cntl_reg1); + + ret = write_reg(ADDR_CTRL_REG1, _cntl_reg1); + + if (OK != ret) { + perf_count(_comms_errors); + } + + uint8_t conf_reg_ret = 0; + read_reg(ADDR_CTRL_REG1, conf_reg_ret); + + //print_info(); + + return !(_cntl_reg1 == conf_reg_ret); +} + +int +LIS3MDL::write_reg(uint8_t reg, uint8_t val) +{ + uint8_t buf = val; + return _interface->write(reg, &buf, 1); +} + +int +LIS3MDL::read_reg(uint8_t reg, uint8_t &val) +{ + uint8_t buf = val; + int ret = _interface->read(reg, &buf, 1); + val = buf; + return ret; +} + +float +LIS3MDL::meas_to_float(uint8_t in[2]) +{ + union { + uint8_t b[2]; + int16_t w; + } u; + + u.b[0] = in[1]; + u.b[1] = in[0]; + + return (float) u.w; +} + +void +LIS3MDL::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + printf("output (%.2f %.2f %.2f)\n", (double)_last_report.x, (double)_last_report.y, (double)_last_report.z); + printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset); + printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", + (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, + (double)(1.0f / _range_scale), (double)_range_ga); + printf("temperature %.2f\n", (double)_last_report.temperature); + _reports->print_info("report queue"); +} + +/** + * Local functions in support of the shell command. + */ +namespace lis3mdl +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +/* + list of supported bus configurations + */ +struct lis3mdl_bus_option { + enum LIS3MDL_BUS busid; + const char *devpath; + LIS3MDL_constructor interface_constructor; + uint8_t busnum; + LIS3MDL *dev; +} bus_options[] = { + { LIS3MDL_BUS_I2C_EXTERNAL, "/dev/lis3mdl_ext", &LIS3MDL_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL }, +#ifdef PX4_I2C_BUS_ONBOARD + { LIS3MDL_BUS_I2C_INTERNAL, "/dev/lis3mdl_int", &LIS3MDL_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL }, +#endif +#ifdef PX4_SPIDEV_LIS + { LIS3MDL_BUS_SPI, "/dev/lis3mdl_spi", &LIS3MDL_SPI_interface, PX4_SPI_BUS_SENSORS, NULL }, +#endif +}; +#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0])) + +void start(enum LIS3MDL_BUS busid, enum Rotation rotation); +bool start_bus(struct lis3mdl_bus_option &bus, enum Rotation rotation); +struct lis3mdl_bus_option &find_bus(enum LIS3MDL_BUS busid); +void test(enum LIS3MDL_BUS busid); +void reset(enum LIS3MDL_BUS busid); +int info(enum LIS3MDL_BUS busid); +int calibrate(enum LIS3MDL_BUS busid); +void usage(); + +/** + * start driver for a specific bus option + */ +bool +start_bus(struct lis3mdl_bus_option &bus, enum Rotation rotation) +{ + if (bus.dev != nullptr) { + errx(1, "bus option already started"); + } + + device::Device *interface = bus.interface_constructor(bus.busnum); + + if (interface->init() != OK) { + delete interface; + warnx("no device on bus %u", (unsigned)bus.busid); + return false; + } + + bus.dev = new LIS3MDL(interface, bus.devpath, rotation); + + if (bus.dev != nullptr && OK != bus.dev->init()) { + delete bus.dev; + bus.dev = NULL; + return false; + } + + int fd = open(bus.devpath, O_RDONLY); + + if (fd < 0) { + return false; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + close(fd); + errx(1, "Failed to setup poll rate"); + } + + /* start the sensor polling at 50 Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 50)) { + warn("FAILED: SENSORIOCSPOLLRATE 50Hz"); + } + printf("set poll rate to 50Hz\n"); + + /* Set to 4 Gauss */ + if (OK != ioctl(fd, MAGIOCSRANGE, 4)) { + warnx("FAILED: MAGIOCSRANGE 4 Ga"); + } + printf("set range to 4 Ga\n"); + + close(fd); + + return true; +} + + +/** + * Start the driver. + * + * This function call only returns once the driver + * is either successfully up and running or failed to start. + */ +void +start(enum LIS3MDL_BUS busid, enum Rotation rotation) +{ + bool started = false; + + for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { + if (busid == LIS3MDL_BUS_ALL && bus_options[i].dev != NULL) { + // this device is already started + continue; + } + + if (busid != LIS3MDL_BUS_ALL && bus_options[i].busid != busid) { + // not the one that is asked for + continue; + } + + started |= start_bus(bus_options[i], rotation); + } + + if (!started) { + exit(1); + } +} + +/** + * find a bus structure for a busid + */ +struct lis3mdl_bus_option &find_bus(enum LIS3MDL_BUS busid) +{ + for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) { + if ((busid == LIS3MDL_BUS_ALL || + busid == bus_options[i].busid) && bus_options[i].dev != NULL) { + return bus_options[i]; + } + } + + errx(1, "bus %u not started", (unsigned)busid); +} + + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test(enum LIS3MDL_BUS busid) +{ + struct lis3mdl_bus_option &bus = find_bus(busid); + struct mag_report report; + ssize_t sz; + int ret; + const char *path = bus.devpath; + + int fd = open(path, O_RDONLY); + + if (fd < 0) { + err(1, "%s open failed (try 'lis3mdl start')", path); + } + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "immediate read failed"); + } + + warnx("single read"); + warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); + warnx("time: %lld", report.timestamp); + + /* check if mag is onboard or external */ + if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0) { + errx(1, "failed to get if mag is onboard or external"); + } + + warnx("device active: %s", ret ? "external" : "onboard"); + + /* set the queue depth to 5 */ + if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) { + errx(1, "failed to set queue depth"); + } + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + errx(1, "failed to set 2Hz poll rate"); + } + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) { + errx(1, "timed out waiting for sensor data"); + } + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "periodic read failed"); + } + + warnx("periodic read %u", i); + warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); + warnx("time: %lld", report.timestamp); + } + + errx(0, "PASS"); +} + + +/** + * Automatic scale calibration. + * + * Basic idea: + * + * output = (ext field +- 1.1 Ga self-test) * scale factor + * + * and consequently: + * + * 1.1 Ga = (excited - normal) * scale factor + * scale factor = (excited - normal) / 1.1 Ga + * + * sxy = (excited - normal) / 766 | for conf reg. B set to 0x60 / Gain = 3 + * sz = (excited - normal) / 713 | for conf reg. B set to 0x60 / Gain = 3 + * + * By subtracting the non-excited measurement the pure 1.1 Ga reading + * can be extracted and the sensitivity of all axes can be matched. + * + * SELF TEST OPERATION + * To check the LIS3MDLL for proper operation, a self test feature in incorporated + * in which the sensor offset straps are excited to create a nominal field strength + * (bias field) to be measured. To implement self test, the least significant bits + * (MS1 and MS0) of configuration register A are changed from 00 to 01 (positive bias) + * or 10 (negetive bias), e.g. 0x11 or 0x12. + * Then, by placing the mode register into single-measurement mode (0x01), + * two data acquisition cycles will be made on each magnetic vector. + * The first acquisition will be a set pulse followed shortly by measurement + * data of the external field. The second acquisition will have the offset strap + * excited (about 10 mA) in the positive bias mode for X, Y, and Z axes to create + * about a ±1.1 gauss self test field plus the external field. The first acquisition + * values will be subtracted from the second acquisition, and the net measurement + * will be placed into the data output registers. + * Since self test adds ~1.1 Gauss additional field to the existing field strength, + * using a reduced gain setting prevents sensor from being saturated and data registers + * overflowed. For example, if the configuration register B is set to 0x60 (Gain=3), + * values around +766 LSB (1.16 Ga * 660 LSB/Ga) will be placed in the X and Y data + * output registers and around +713 (1.08 Ga * 660 LSB/Ga) will be placed in Z data + * output register. To leave the self test mode, change MS1 and MS0 bit of the + * configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10. + * Using the self test method described above, the user can scale sensor + */ +int calibrate(enum LIS3MDL_BUS busid) +{ + int ret; + struct lis3mdl_bus_option &bus = find_bus(busid); + const char *path = bus.devpath; + + int fd = open(path, O_RDONLY); + + if (fd < 0) { + err(1, "%s open failed (try 'lis3mdl start' if the driver is not running", path); + } + + if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) { + warnx("failed to enable sensor calibration mode"); + } + + close(fd); + + return ret; +} + +/** + * Reset the driver. + */ +void +reset(enum LIS3MDL_BUS busid) +{ + struct lis3mdl_bus_option &bus = find_bus(busid); + const char *path = bus.devpath; + + int fd = open(path, O_RDONLY); + + if (fd < 0) { + err(1, "failed "); + } + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { + err(1, "driver reset failed"); + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + err(1, "driver poll restart failed"); + } + + exit(0); +} + +/** + * Print a little info about the driver. + */ +int +info(enum LIS3MDL_BUS busid) +{ + struct lis3mdl_bus_option &bus = find_bus(busid); + + warnx("running on bus: %u (%s)\n", (unsigned)bus.busid, bus.devpath); + bus.dev->print_info(); + exit(0); +} + +void +usage() +{ + warnx("missing command: try 'start', 'info', 'test', 'reset', 'info', 'calibrate'"); + warnx("options:"); + warnx(" -R rotation"); + warnx(" -C calibrate on start"); + warnx(" -X only external bus"); +#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_LIS) + warnx(" -I only internal bus"); +#endif +} + +} // namespace + +int +lis3mdl_main(int argc, char *argv[]) +{ + int ch; + enum LIS3MDL_BUS busid = LIS3MDL_BUS_ALL; + enum Rotation rotation = ROTATION_NONE; + bool calibrate = false; + + while ((ch = getopt(argc, argv, "XISR:CT")) != EOF) { + switch (ch) { + case 'R': + rotation = (enum Rotation)atoi(optarg); + break; +#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_LIS) + + case 'I': + busid = LIS3MDL_BUS_I2C_INTERNAL; + break; +#endif + + case 'X': + busid = LIS3MDL_BUS_I2C_EXTERNAL; + break; + + case 'S': + busid = LIS3MDL_BUS_SPI; + break; + + case 'C': + calibrate = true; + break; + + default: + lis3mdl::usage(); + exit(0); + } + } + + const char *verb = argv[optind]; + + /* + * Start/load the driver. + */ + if (!strcmp(verb, "start")) { + lis3mdl::start(busid, rotation); + + if (calibrate && lis3mdl::calibrate(busid) != 0) { + errx(1, "calibration failed"); + } + + exit(0); + } + + /* + * Test the driver/device. + */ + if (!strcmp(verb, "test")) { + lis3mdl::test(busid); + } + + /* + * Reset the driver. + */ + if (!strcmp(verb, "reset")) { + lis3mdl::reset(busid); + } + + /* + * Print driver information. + */ + if (!strcmp(verb, "info") || !strcmp(verb, "status")) { + lis3mdl::info(busid); + } + + /* + * Autocalibrate the scaling + */ + if (!strcmp(verb, "calibrate")) { + if (lis3mdl::calibrate(busid) == 0) { + errx(0, "calibration successful"); + + } else { + errx(1, "calibration failed"); + } + } + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'calibrate' 'or 'info'"); +} diff --git a/src/drivers/lis3mdl/lis3mdl.h b/src/drivers/lis3mdl/lis3mdl.h new file mode 100644 index 00000000000..9ad8487ce8d --- /dev/null +++ b/src/drivers/lis3mdl/lis3mdl.h @@ -0,0 +1,48 @@ +/**************************************************************************** + * + * Copyright (c) 2015 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 lis3mdl.h + * + * Shared defines for the LIS3MDL driver. + */ + +#pragma once + +#define ADDR_WHO_AM_I 0x0f +#define ID_WHO_AM_I 0x3d + +/* interface factories */ +extern device::Device *LIS3MDL_SPI_interface(int bus); +extern device::Device *LIS3MDL_I2C_interface(int bus); +typedef device::Device *(*LIS3MDL_constructor)(int); diff --git a/src/drivers/lis3mdl/lis3mdl_i2c.cpp b/src/drivers/lis3mdl/lis3mdl_i2c.cpp new file mode 100644 index 00000000000..603faf8dc51 --- /dev/null +++ b/src/drivers/lis3mdl/lis3mdl_i2c.cpp @@ -0,0 +1,186 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 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 lis3mdl_i2c.cpp + * + * I2C interface for LIS3MDL + */ + +/* XXX trim includes */ +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include "lis3mdl.h" +#include "board_config.h" + +#undef DEVICE_DEBUG +#define DEVICE_DEBUG printf + +#ifdef PX4_I2C_OBDEV_LIS3MDL + +#define LIS3MDLL_ADDRESS PX4_I2C_OBDEV_LIS3MDL + +device::Device *LIS3MDL_I2C_interface(int bus); + +class LIS3MDL_I2C : public device::I2C +{ +public: + LIS3MDL_I2C(int bus); + virtual ~LIS3MDL_I2C(); + + virtual int init(); + virtual int read(unsigned address, void *data, unsigned count); + virtual int write(unsigned address, void *data, unsigned count); + + virtual int ioctl(unsigned operation, unsigned &arg); + +protected: + virtual int probe(); + +}; + +device::Device * +LIS3MDL_I2C_interface(int bus) +{ + return new LIS3MDL_I2C(bus); +} + +LIS3MDL_I2C::LIS3MDL_I2C(int bus) : + I2C("LIS3MDL_I2C", nullptr, bus, LIS3MDLL_ADDRESS, 400000) +{ + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LIS3MDL; +} + +LIS3MDL_I2C::~LIS3MDL_I2C() +{ +} + +int +LIS3MDL_I2C::init() +{ + /* this will call probe() */ + return I2C::init(); +} + +int +LIS3MDL_I2C::ioctl(unsigned operation, unsigned &arg) +{ + int ret; + + switch (operation) { + + case MAGIOCGEXTERNAL: +// On PX4v1 the MAG can be on an internal I2C +// On everything else its always external +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + if (_bus == PX4_I2C_BUS_EXPANSION) { + return 1; + + } else { + return 0; + } + +#else + return 1; +#endif + + case DEVIOCGDEVICEID: + return CDev::ioctl(nullptr, operation, arg); + + default: + ret = -EINVAL; + } + + return ret; +} + +int +LIS3MDL_I2C::probe() +{ + uint8_t data = 0; + + _retries = 10; + + if (read(ADDR_WHO_AM_I, &data, 1)) { + DEVICE_DEBUG("read_reg fail"); + return -EIO; + } + + _retries = 2; + + if (data != ID_WHO_AM_I) { + DEVICE_DEBUG("LIS3MDL bad ID: %02x", data); + return -EIO; + } + + return OK; +} + +int +LIS3MDL_I2C::write(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address; + memcpy(&buf[1], data, count); + + return transfer(&buf[0], count + 1, nullptr, 0); +} + +int +LIS3MDL_I2C::read(unsigned address, void *data, unsigned count) +{ + uint8_t cmd = address; + return transfer(&cmd, 1, (uint8_t *)data, count); +} + +#endif /* PX4_I2C_OBDEV_LIS3MDL */ diff --git a/src/drivers/lis3mdl/lis3mdl_spi.cpp b/src/drivers/lis3mdl/lis3mdl_spi.cpp new file mode 100644 index 00000000000..ac64d294a6f --- /dev/null +++ b/src/drivers/lis3mdl/lis3mdl_spi.cpp @@ -0,0 +1,185 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 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 lis3mdl_spi.cpp + * + * SPI interface for LIS3MDL + */ + +/* XXX trim includes */ +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include "lis3mdl.h" +#include "board_config.h" + +#undef DEVICE_DEBUG +#define DEVICE_DEBUG printf + +#ifdef PX4_SPIDEV_LIS + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + +device::Device *LIS3MDL_SPI_interface(int bus); + +class LIS3MDL_SPI : public device::SPI +{ +public: + LIS3MDL_SPI(int bus, spi_dev_e device); + virtual ~LIS3MDL_SPI(); + + virtual int init(); + virtual int read(unsigned address, void *data, unsigned count); + virtual int write(unsigned address, void *data, unsigned count); + virtual int ioctl(unsigned operation, unsigned &arg); +}; + +device::Device * +LIS3MDL_SPI_interface(int bus) +{ + return new LIS3MDL_SPI(bus, (spi_dev_e)PX4_SPIDEV_LIS); +} + +LIS3MDL_SPI::LIS3MDL_SPI(int bus, spi_dev_e device) : + SPI("LIS3MDL_SPI", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000 /* will be rounded to 10.4 MHz */) +{ + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LIS3MDL; +} + +LIS3MDL_SPI::~LIS3MDL_SPI() +{ +} + +int +LIS3MDL_SPI::init() +{ + int ret; + + ret = SPI::init(); + + if (ret != OK) { + DEVICE_DEBUG("SPI init failed"); + return -EIO; + } + + // read WHO_AM_I value + uint8_t data = 0; + + if (read(ADDR_WHO_AM_I, &data, 1)) { + DEVICE_DEBUG("LIS3MDL read_reg fail"); + } + + if (data != ID_WHO_AM_I) { + DEVICE_DEBUG("LIS3MDL bad ID: %02x", data); + return -EIO; + } + + return OK; +} + +int +LIS3MDL_SPI::ioctl(unsigned operation, unsigned &arg) +{ + int ret; + + switch (operation) { + + case MAGIOCGEXTERNAL: + /* + * Even if this sensor is on the external SPI + * bus it is still internal to the autopilot + * assembly, so always return 0 for internal. + */ + return 0; + + case DEVIOCGDEVICEID: + return CDev::ioctl(nullptr, operation, arg); + + default: { + ret = -EINVAL; + } + } + + return ret; +} + +int +LIS3MDL_SPI::write(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address | DIR_WRITE; + memcpy(&buf[1], data, count); + + return transfer(&buf[0], &buf[0], count + 1); +} + +int +LIS3MDL_SPI::read(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address | DIR_READ | ADDR_INCREMENT; + + int ret = transfer(&buf[0], &buf[0], count + 1); + memcpy(data, &buf[1], count); + return ret; +} + +#endif /* PX4_SPIDEV_LIS */