diff --git a/boards/holybro/durandal-v1/stackcheck.cmake b/boards/holybro/durandal-v1/stackcheck.cmake index 001d5a4bdb69..761947b82004 100644 --- a/boards/holybro/durandal-v1/stackcheck.cmake +++ b/boards/holybro/durandal-v1/stackcheck.cmake @@ -38,7 +38,8 @@ px4_add_board( #lights/blinkm #lights/rgbled lights/rgbled_ncp5623c - magnetometer # all available magnetometer drivers + #magnetometer # all available magnetometer drivers + magnetometer/ist8310 #mkblctrl #optical_flow # all available optical flow drivers #osd diff --git a/boards/px4/fmu-v5/optimized.cmake b/boards/px4/fmu-v5/optimized.cmake index 551a5dd7baf3..e373b4183565 100644 --- a/boards/px4/fmu-v5/optimized.cmake +++ b/boards/px4/fmu-v5/optimized.cmake @@ -39,7 +39,8 @@ px4_add_board( lights/rgbled lights/rgbled_ncp5623c #lights/rgbled_pwm - magnetometer # all available magnetometer drivers + #magnetometer # all available magnetometer drivers + magnetometer/ist8310 optical_flow # all available optical flow drivers #pwm_input pwm_out_sim diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index e1f1da0172e4..487addd3ccc1 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -63,6 +63,7 @@ #define DRV_MAG_DEVTYPE_AK09916 0x09 #define DRV_IMU_DEVTYPE_ICM20948 0x0A #define DRV_MAG_DEVTYPE_IST8308 0x0B +#define DRV_MAG_DEVTYPE_LIS2MDL 0x0C #define DRV_IMU_DEVTYPE_LSM303D 0x11 #define DRV_ACC_DEVTYPE_BMA180 0x12 @@ -160,6 +161,7 @@ #define DRV_MAG_DEVTYPE_UAVCAN 0x88 #define DRV_DIST_DEVTYPE_UAVCAN 0x89 + #define DRV_DEVTYPE_UNUSED 0xff #endif /* _DRV_SENSOR_H */ diff --git a/src/drivers/magnetometer/CMakeLists.txt b/src/drivers/magnetometer/CMakeLists.txt index ecf1ff2a7146..58272947afa1 100644 --- a/src/drivers/magnetometer/CMakeLists.txt +++ b/src/drivers/magnetometer/CMakeLists.txt @@ -37,6 +37,7 @@ add_subdirectory(hmc5883) add_subdirectory(qmc5883) add_subdirectory(isentek) add_subdirectory(ist8310) +add_subdirectory(lis2mdl) add_subdirectory(lis3mdl) add_subdirectory(lsm303agr) add_subdirectory(rm3100) diff --git a/src/drivers/magnetometer/lis2mdl/CMakeLists.txt b/src/drivers/magnetometer/lis2mdl/CMakeLists.txt new file mode 100644 index 000000000000..55fc23d0efae --- /dev/null +++ b/src/drivers/magnetometer/lis2mdl/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2020 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__lis2mdl + MAIN lis2mdl + SRCS + lis2mdl_i2c.cpp + lis2mdl_spi.cpp + lis2mdl_main.cpp + lis2mdl.cpp + ) diff --git a/src/drivers/magnetometer/lis2mdl/lis2mdl.cpp b/src/drivers/magnetometer/lis2mdl/lis2mdl.cpp new file mode 100644 index 000000000000..5fbf9d339de0 --- /dev/null +++ b/src/drivers/magnetometer/lis2mdl/lis2mdl.cpp @@ -0,0 +1,189 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 lis2mdl.cpp + * + * Driver for the LIS2MDL magnetometer connected via I2C or SPI. + * + * Based on the LIS2MDL driver. + */ + +#include +#include "lis2mdl.h" + +LIS2MDL::LIS2MDL(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus) : + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus), + _px4_mag(interface->get_device_id(), interface->external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT, rotation), + _interface(interface), + _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors")), + _conf_errors(perf_alloc(PC_COUNT, MODULE_NAME": conf_errors")), + _range_errors(perf_alloc(PC_COUNT, MODULE_NAME": range_errors")), + _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), + _measure_interval(0) +{ + _px4_mag.set_external(_interface->external()); + _px4_mag.set_scale(0.0015f); /* 49.152f / (2^15) */ +} + +LIS2MDL::~LIS2MDL() +{ + // free perf counters + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_range_errors); + perf_free(_conf_errors); +} + +int +LIS2MDL::measure() +{ + struct { + uint8_t status; + uint8_t x[2]; + uint8_t y[2]; + uint8_t z[2]; + } lis_report; + + struct { + int16_t x; + int16_t y; + int16_t z; + int16_t t; + } report; + + uint8_t buf_rx[2] = {}; + + _px4_mag.set_error_count(perf_event_count(_comms_errors)); + + perf_begin(_sample_perf); + + const hrt_abstime timestamp_sample = hrt_absolute_time(); + int ret = _interface->read(ADDR_STATUS_REG, (uint8_t *)&lis_report, sizeof(lis_report)); + + /** + * Silicon Bug: the X axis will be read instead of the temperature registers if you do a sequential read through XYZ. + * The temperature registers must be addressed directly. + */ + ret = _interface->read(ADDR_OUT_T_L, (uint8_t *)&buf_rx, sizeof(buf_rx)); + + if (ret != OK) { + perf_end(_sample_perf); + perf_count(_comms_errors); + PX4_WARN("Register read error."); + return ret; + } + + perf_end(_sample_perf); + + if ((lis_report.status & (1 << 3)) == 0) { // check data ready + return 0; + } + + 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)((buf_rx[1] << 8) | buf_rx[0]); + + float temperature = report.t; + _px4_mag.set_temperature(25.0f + (temperature / 8.0f)); + + /* swap x and y axis */ + _px4_mag.update(timestamp_sample, report.y, report.x, report.z); + + return PX4_OK; +} + +void +LIS2MDL::RunImpl() +{ + /* _measure_interval == 0 is used as _task_should_exit */ + if (_measure_interval == 0) { + return; + } + + if (measure() != OK) { + PX4_DEBUG("measure error"); + } + + /* schedule a fresh cycle call when the measurement is done */ + ScheduleDelayed(LIS2MDL_CONVERSION_INTERVAL); +} + +int +LIS2MDL::init() +{ + + int ret = write_reg(ADDR_CFG_REG_A, CFG_REG_A_ODR | CFG_REG_A_MD | CFG_REG_A_TEMP_COMP_EN); + ret = write_reg(ADDR_CFG_REG_B, 0); + ret = write_reg(ADDR_CFG_REG_C, CFG_REG_C_BDU); + + _measure_interval = LIS2MDL_CONVERSION_INTERVAL; + start(); + + return ret; +} + +void +LIS2MDL::print_status() +{ + I2CSPIDriverBase::print_status(); + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + PX4_INFO("poll interval: %u", _measure_interval); + _px4_mag.print_status(); +} + +void +LIS2MDL::start() +{ + /* schedule a cycle to start things */ + ScheduleNow(); +} + +int +LIS2MDL::read_reg(uint8_t reg, uint8_t &val) +{ + uint8_t buf = val; + int ret = _interface->read(reg, &buf, 1); + val = buf; + return ret; +} + +int +LIS2MDL::write_reg(uint8_t reg, uint8_t val) +{ + uint8_t buf = val; + return _interface->write(reg, &buf, 1); +} diff --git a/src/drivers/magnetometer/lis2mdl/lis2mdl.h b/src/drivers/magnetometer/lis2mdl/lis2mdl.h new file mode 100644 index 000000000000..cd5bf359b03e --- /dev/null +++ b/src/drivers/magnetometer/lis2mdl/lis2mdl.h @@ -0,0 +1,154 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 lis2mdl.h + * + * Shared defines for the LIS2MDL driver. + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +/** + * LIS2MDL internal constants and data structures. + */ + +/* Max measurement rate is 50Hz */ +#define LIS2MDL_CONVERSION_INTERVAL (1000000 / 50) + +#define ADDR_WHO_AM_I 0x4f +#define ID_WHO_AM_I 0x40 + +#define ADDR_CFG_REG_A 0x60 +#define ADDR_CFG_REG_B 0x61 +#define ADDR_CFG_REG_C 0x62 +#define ADDR_INT_CTRL_REG 0x63 + +#define ADDR_STATUS_REG 0x67 +#define ADDR_OUT_X_L 0x68 +#define ADDR_OUT_X_H 0x69 +#define ADDR_OUT_Y_L 0x6a +#define ADDR_OUT_Y_H 0x6b +#define ADDR_OUT_Z_L 0x6c +#define ADDR_OUT_Z_H 0x6d +#define ADDR_OUT_T_L 0x6e +#define ADDR_OUT_T_H 0x6f + +#define CFG_REG_A_TEMP_COMP_EN (1 << 7) +#define CFG_REG_A_ODR (2 << 2) /* 50Hz (100Hz creates spikes randomly) */ +#define CFG_REG_A_MD (0 << 0) /* continuous mode */ + +#define CFG_REG_B_LPF (1 << 0) /* LPF */ + +#define CFG_REG_C_BDU (1 << 4) /* avoids reading of incorrect data due to async reads */ + +/* interface factories */ +extern device::Device *LIS2MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode); +extern device::Device *LIS2MDL_I2C_interface(int bus, int bus_frequency); + + +class LIS2MDL : public I2CSPIDriver +{ +public: + LIS2MDL(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus); + virtual ~LIS2MDL(); + + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + + virtual int init(); + + void print_status() override; + + /** + * Configures the device with default register values. + */ + int set_default_register_values(); + + void RunImpl(); + +private: + PX4Magnetometer _px4_mag; + + device::Device *_interface; + + perf_counter_t _comms_errors; + perf_counter_t _conf_errors; + perf_counter_t _range_errors; + perf_counter_t _sample_perf; + + unsigned int _measure_interval; + + /** + * Issue a measurement command & publish data. + * + * @return OK if the measurement command was successful. + */ + int measure(); + + /** + * @brief Initialises 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(); + + /** + * @brief Reads 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); + + /** + * @brief Writes 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); + +}; diff --git a/src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp b/src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp new file mode 100644 index 000000000000..a8691909e143 --- /dev/null +++ b/src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp @@ -0,0 +1,130 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 lis2mdl_i2c.cpp + * + * I2C interface for LIS2MDL + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "board_config.h" +#include "lis2mdl.h" + +#define LIS2MDLL_ADDRESS 0x1e + +class LIS2MDL_I2C : public device::I2C +{ +public: + LIS2MDL_I2C(int bus, int bus_frequency); + virtual ~LIS2MDL_I2C() = default; + + virtual int read(unsigned address, void *data, unsigned count); + virtual int write(unsigned address, void *data, unsigned count); + +protected: + virtual int probe(); + +}; + +device::Device * +LIS2MDL_I2C_interface(int bus, int bus_frequency); + +device::Device * +LIS2MDL_I2C_interface(int bus, int bus_frequency) +{ + return new LIS2MDL_I2C(bus, bus_frequency); +} + +LIS2MDL_I2C::LIS2MDL_I2C(int bus, int bus_frequency) : + I2C(DRV_MAG_DEVTYPE_LIS2MDL, "LIS2MDL_I2C", bus, LIS2MDLL_ADDRESS, bus_frequency) +{ +} + +int +LIS2MDL_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("LIS2MDL bad ID: %02x", data); + return -EIO; + } + + return OK; +} + +int +LIS2MDL_I2C::read(unsigned address, void *data, unsigned count) +{ + uint8_t cmd = address; + return transfer(&cmd, 1, (uint8_t *)data, count); +} + +int +LIS2MDL_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); +} diff --git a/src/drivers/magnetometer/lis2mdl/lis2mdl_main.cpp b/src/drivers/magnetometer/lis2mdl/lis2mdl_main.cpp new file mode 100644 index 000000000000..f8a05eb8b767 --- /dev/null +++ b/src/drivers/magnetometer/lis2mdl/lis2mdl_main.cpp @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 lis2mdl_main.cpp + * + * Driver for the LIS2MDL magnetometer connected via I2C or SPI. + */ + +#include "lis2mdl.h" +#include +#include + +I2CSPIDriverBase *LIS2MDL::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) +{ + device::Device *interface = nullptr; + + if (iterator.busType() == BOARD_I2C_BUS) { + interface = LIS2MDL_I2C_interface(iterator.bus(), cli.bus_frequency); + + } else if (iterator.busType() == BOARD_SPI_BUS) { + interface = LIS2MDL_SPI_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode); + } + + if (interface == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (interface->init() != OK) { + delete interface; + PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + return nullptr; + } + + LIS2MDL *dev = new LIS2MDL(interface, cli.rotation, iterator.configuredBusOption(), iterator.bus()); + + if (dev == nullptr) { + delete interface; + return nullptr; + } + + if (OK != dev->init()) { + delete dev; + return nullptr; + } + + return dev; +} + +void LIS2MDL::print_usage() +{ + PRINT_MODULE_USAGE_NAME("lis2mdl", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true); + PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int lis2mdl_main(int argc, char *argv[]) +{ + using ThisDriver = LIS2MDL; + int ch; + BusCLIArguments cli{true, true}; + cli.default_i2c_frequency = 400000; + cli.default_spi_frequency = 11 * 1000 * 1000; + + while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + switch (ch) { + case 'R': + cli.rotation = (enum Rotation)atoi(cli.optarg()); + break; + } + } + + const char *verb = cli.optarg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_LIS2MDL); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + } + + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp b/src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp new file mode 100644 index 000000000000..2d0a1776f548 --- /dev/null +++ b/src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 lis2mdl_spi.cpp + * + * SPI interface for LIS2MDL + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "board_config.h" +#include "lis2mdl.h" + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + +class LIS2MDL_SPI : public device::SPI +{ +public: + LIS2MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode); + virtual ~LIS2MDL_SPI() = default; + + virtual int init(); + virtual int read(unsigned address, void *data, unsigned count); + virtual int write(unsigned address, void *data, unsigned count); +}; + +device::Device * +LIS2MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode); + +device::Device * +LIS2MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode) +{ + return new LIS2MDL_SPI(bus, devid, bus_frequency, spi_mode); +} + +LIS2MDL_SPI::LIS2MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode) : + SPI(DRV_MAG_DEVTYPE_LIS2MDL, "LIS2MDL_SPI", bus, devid, spi_mode, bus_frequency) +{ +} + +int +LIS2MDL_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("LIS2MDL read_reg fail"); + } + + if (data != ID_WHO_AM_I) { + DEVICE_DEBUG("LIS2MDL bad ID: %02x", data); + return -EIO; + } + + return OK; +} + +int +LIS2MDL_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; +} + +int +LIS2MDL_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); +}