diff --git a/boards/emlid/navio2/default.cmake b/boards/emlid/navio2/default.cmake index b058361051..f7e0e925ac 100644 --- a/boards/emlid/navio2/default.cmake +++ b/boards/emlid/navio2/default.cmake @@ -19,10 +19,12 @@ px4_add_board( gps #imu # all available imu drivers imu/mpu9250 + imu/st/lsm9ds1 linux_pwm_out linux_sbus #magnetometer # all available magnetometer drivers magnetometer/hmc5883 + magnetometer/lsm9ds1_mag pwm_out_sim #telemetry # all available telemetry drivers DF_DRIVERS # NOTE: DriverFramework is migrating to intree PX4 drivers diff --git a/posix-configs/rpi/px4.config b/posix-configs/rpi/px4.config index ffd44632ca..e372bbb85c 100644 --- a/posix-configs/rpi/px4.config +++ b/posix-configs/rpi/px4.config @@ -20,6 +20,8 @@ param set BAT_CNT_V_CURR 0.001 dataman start df_lsm9ds1_wrapper start -R 4 +#lsm9ds1 start -R 4 +#lsm9ds1_mag start -R 4 #df_mpu9250_wrapper start -R 10 ms5611 start navio_rgbled start diff --git a/posix-configs/rpi/px4_fw.config b/posix-configs/rpi/px4_fw.config index 31257e3d87..1429c7bc11 100644 --- a/posix-configs/rpi/px4_fw.config +++ b/posix-configs/rpi/px4_fw.config @@ -20,6 +20,8 @@ param set BAT_CNT_V_CURR 0.001 dataman start df_lsm9ds1_wrapper start -R 4 +#lsm9ds1 start -R 4 +#lsm9ds1_mag start -R 4 #df_mpu9250_wrapper start -R 10 ms5611 start navio_rgbled start diff --git a/posix-configs/rpi/px4_test.config b/posix-configs/rpi/px4_test.config index 5cd431d589..8c6e43be80 100644 --- a/posix-configs/rpi/px4_test.config +++ b/posix-configs/rpi/px4_test.config @@ -20,6 +20,8 @@ param set BAT_CNT_V_CURR 0.001 dataman start df_lsm9ds1_wrapper start -R 4 +#lsm9ds1 start -R 4 +#lsm9ds1_mag start -R 4 #df_mpu9250_wrapper start -R 10 ms5611 start navio_rgbled start diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index ed2e3d37e5..5bddeaea7f 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -95,14 +95,15 @@ #define DRV_ACC_DEVTYPE_BMI055 0x41 #define DRV_GYR_DEVTYPE_BMI055 0x42 #define DRV_MAG_DEVTYPE_BMM150 0x43 +#define DRV_IMU_DEVTYPE_ST_LSM9DS1_AG 0x44 +#define DRV_MAG_DEVTYPE_ST_LSM9DS1_M 0x45 +#define DRV_DIFF_PRESS_DEVTYPE_ETS3 0x46 +#define DRV_DIFF_PRESS_DEVTYPE_MS4525 0x47 +#define DRV_DIFF_PRESS_DEVTYPE_MS5525 0x48 +#define DRV_DIFF_PRESS_DEVTYPE_SDP31 0x49 +#define DRV_DIFF_PRESS_DEVTYPE_SDP32 0x4A +#define DRV_DIFF_PRESS_DEVTYPE_SDP33 0x4B -#define DRV_DIFF_PRESS_DEVTYPE_ETS3 0x45 -#define DRV_DIFF_PRESS_DEVTYPE_MS4525 0x46 -#define DRV_DIFF_PRESS_DEVTYPE_MS5525 0x47 -#define DRV_DIFF_PRESS_DEVTYPE_SDP31 0x48 -#define DRV_DIFF_PRESS_DEVTYPE_SDP32 0x49 - -#define DRV_DIFF_PRESS_DEVTYPE_SDP33 0x50 #define DRV_BARO_DEVTYPE_MPL3115A2 0x51 #define DRV_ACC_DEVTYPE_FXOS8701C 0x52 diff --git a/src/drivers/imu/st/lsm9ds1/CMakeLists.txt b/src/drivers/imu/st/lsm9ds1/CMakeLists.txt new file mode 100644 index 0000000000..dea6ed2af3 --- /dev/null +++ b/src/drivers/imu/st/lsm9ds1/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# 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__imu__st__lsm9ds1 + MAIN lsm9ds1 + SRCS + lsm9ds1_main.cpp + LSM9DS1.cpp + LSM9DS1.hpp + ST_LSM9DS1_Registers.hpp + DEPENDS + drivers_accelerometer + drivers_gyroscope + px4_work_queue + ) diff --git a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp new file mode 100644 index 0000000000..5e08a6429c --- /dev/null +++ b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp @@ -0,0 +1,328 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include "LSM9DS1.hpp" + +using namespace time_literals; +using namespace ST_LSM9DS1; + +static constexpr int16_t combine(uint8_t lsb, uint8_t msb) { return (msb << 8u) | lsb; } + +LSM9DS1::LSM9DS1(int bus, uint32_t device, enum Rotation rotation) : + SPI(MODULE_NAME, nullptr, bus, device, SPIDEV_MODE3, SPI_SPEED), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), + _px4_accel(get_device_id(), ORB_PRIO_DEFAULT, rotation), + _px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation) +{ + set_device_type(DRV_IMU_DEVTYPE_ST_LSM9DS1_AG); + _px4_accel.set_device_type(DRV_IMU_DEVTYPE_ST_LSM9DS1_AG); + _px4_gyro.set_device_type(DRV_IMU_DEVTYPE_ST_LSM9DS1_AG); + + _px4_accel.set_sample_rate(ST_LSM9DS1::LA_ODR); + _px4_gyro.set_sample_rate(ST_LSM9DS1::G_ODR); + + _px4_accel.set_update_rate(1000000 / _fifo_interval); + _px4_gyro.set_update_rate(1000000 / _fifo_interval); +} + +LSM9DS1::~LSM9DS1() +{ + Stop(); + + perf_free(_interval_perf); + perf_free(_transfer_perf); + perf_free(_fifo_empty_perf); + perf_free(_fifo_overflow_perf); + perf_free(_fifo_reset_perf); +} + +int LSM9DS1::probe() +{ + uint8_t whoami = RegisterRead(Register::WHO_AM_I); + + if (whoami == LSM9DS1_WHO_AM_I) { + return PX4_OK; + } + + return PX4_ERROR; +} + +bool LSM9DS1::Init() +{ + if (SPI::init() != PX4_OK) { + PX4_ERR("SPI::init failed"); + return false; + } + + if (!Reset()) { + PX4_ERR("reset failed"); + return false; + } + + Start(); + + return true; +} + +bool LSM9DS1::Reset() +{ + // Reset + // CTRL_REG8: SW_RESET + RegisterSetBits(Register::CTRL_REG8, CTRL_REG8_BIT::SW_RESET); + usleep(50); // Wait 50 μs (or wait until the SW_RESET bit of the CTRL_REG8 register returns to 0). + + RegisterSetBits(Register::CTRL_REG9, CTRL_REG9_BIT::I2C_DISABLE); + + // Gyroscope configuration + // CTRL_REG1_G: Gyroscope 2000 degrees/second and ODR 952 Hz + RegisterWrite(Register::CTRL_REG1_G, + CTRL_REG1_G_BIT::ODR_G_952HZ | CTRL_REG1_G_BIT::FS_G_2000DPS | CTRL_REG1_G_BIT::BW_G_100Hz); + _px4_gyro.set_scale(math::radians(70.0f / 1000.0f)); // 70 mdps/LSB + _px4_gyro.set_range(math::radians(2000.0f)); + + // Accelerometer configuration + // CTRL_REG6_XL: Accelerometer 16 G range and ODR 952 Hz + RegisterWrite(Register::CTRL_REG6_XL, CTRL_REG6_XL_BIT::ODR_XL_952HZ | CTRL_REG6_XL_BIT::FS_XL_16); + _px4_accel.set_scale(0.732f * (CONSTANTS_ONE_G / 1000.0f)); // 0.732 mg/LSB + _px4_accel.set_range(16.0f * CONSTANTS_ONE_G); + + return true; +} + +void LSM9DS1::ResetFIFO() +{ + perf_count(_fifo_reset_perf); + + // CTRL_REG9: disable FIFO + RegisterClearBits(Register::CTRL_REG9, CTRL_REG9_BIT::FIFO_EN); + + // FIFO_CTRL: to reset FIFO content, Bypass mode (0) should be selected + RegisterWrite(Register::FIFO_CTRL, 0); + + // CTRL_REG9: enable FIFO + RegisterSetBits(Register::CTRL_REG9, CTRL_REG9_BIT::FIFO_EN); + + // CTRL_REG8: Note: When the FIFO is used, the IF_INC and BDU bits must be equal to 1. + RegisterWrite(Register::CTRL_REG8, CTRL_REG8_BIT::BDU | CTRL_REG8_BIT::IF_ADD_INC); + usleep(1); + + // FIFO_CTRL: FIFO continuous mode enabled + RegisterWrite(Register::FIFO_CTRL, FIFO_CTRL_BIT::FIFO_MODE_CONTINUOUS); + usleep(1); +} + +uint8_t LSM9DS1::RegisterRead(Register reg) +{ + uint8_t cmd[2] {}; + cmd[0] = static_cast(reg) | DIR_READ; + transfer(cmd, cmd, sizeof(cmd)); + return cmd[1]; +} + +void LSM9DS1::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t cmd[2] { (uint8_t)reg, value }; + transfer(cmd, cmd, sizeof(cmd)); +} + +void LSM9DS1::RegisterSetBits(Register reg, uint8_t setbits) +{ + uint8_t val = RegisterRead(reg); + + if (!(val & setbits)) { + val |= setbits; + RegisterWrite(reg, val); + } +} + +void LSM9DS1::RegisterClearBits(Register reg, uint8_t clearbits) +{ + uint8_t val = RegisterRead(reg); + + if (val & clearbits) { + val &= !clearbits; + RegisterWrite(reg, val); + } +} + +void LSM9DS1::Start() +{ + Stop(); + + ResetFIFO(); + + ScheduleOnInterval(_fifo_interval / 2, _fifo_interval); +} + +void LSM9DS1::Stop() +{ + ScheduleClear(); +} + +void LSM9DS1::Run() +{ + perf_count(_interval_perf); + + // Number of unread words (16-bit axes) stored in FIFO. + const hrt_abstime timestamp_fifo_level = hrt_absolute_time(); + const uint8_t FIFO_SRC = RegisterRead(Register::FIFO_SRC); + + if (FIFO_SRC & FIFO_SRC_BIT::OVRN) { + // overflow + perf_count(_fifo_overflow_perf); + ResetFIFO(); + return; + } + + const uint8_t samples = FIFO_SRC & static_cast(FIFO_SRC_BIT::FSS); + + if (samples < 1) { + perf_count(_fifo_empty_perf); + return; + + } else if (samples > 32) { + // not technically an overflow, but more samples than we expected + perf_count(_fifo_overflow_perf); + ResetFIFO(); + return; + } + + // estimate timestamp of first sample in the FIFO from number of samples and fill rate + const hrt_abstime timestamp_sample = timestamp_fifo_level; + + int16_t accel_data[3] {}; + int16_t gyro_data[3] {}; + int accel_samples = 0; + int gyro_samples = 0; + + perf_begin(_transfer_perf); + + for (int i = 0; i < samples; i++) { + // Gyro + { + struct GyroReport { + uint8_t cmd; + uint8_t STATUS_REG; + uint8_t OUT_X_L_G; + uint8_t OUT_X_H_G; + uint8_t OUT_Y_L_G; + uint8_t OUT_Y_H_G; + uint8_t OUT_Z_L_G; + uint8_t OUT_Z_H_G; + } greport{}; + greport.cmd = static_cast(Register::STATUS_REG_G) | DIR_READ; + + if (transfer((uint8_t *)&greport, (uint8_t *)&greport, sizeof(GyroReport)) != PX4_OK) { + perf_end(_transfer_perf); + return; + } + + if (greport.STATUS_REG & STATUS_REG_BIT::GDA) { + // Gyroscope new data available + + // sensor Z is up (RHC), flip z for publication + gyro_data[0] = combine(greport.OUT_X_L_G, greport.OUT_X_H_G); + gyro_data[1] = combine(greport.OUT_Y_L_G, greport.OUT_Y_H_G); + gyro_data[2] = -combine(greport.OUT_Z_L_G, greport.OUT_Z_H_G); + gyro_samples++; + } + } + + // Accel + { + struct AccelReport { + uint8_t cmd; + uint8_t STATUS_REG; + uint8_t OUT_X_L_XL; + uint8_t OUT_X_H_XL; + uint8_t OUT_Y_L_XL; + uint8_t OUT_Y_H_XL; + uint8_t OUT_Z_L_XL; + uint8_t OUT_Z_H_XL; + } areport{}; + areport.cmd = static_cast(Register::STATUS_REG_A) | DIR_READ; + + if (transfer((uint8_t *)&areport, (uint8_t *)&areport, sizeof(AccelReport)) != PX4_OK) { + perf_end(_transfer_perf); + return; + } + + if (areport.STATUS_REG & STATUS_REG_BIT::XLDA) { + // Accelerometer new data available + + // sensor Z is up (RHC), flip z for publication + accel_data[0] = combine(areport.OUT_X_L_XL, areport.OUT_X_H_XL); + accel_data[1] = combine(areport.OUT_Y_L_XL, areport.OUT_Y_H_XL); + accel_data[2] = -combine(areport.OUT_Z_L_XL, areport.OUT_Z_H_XL); + accel_samples++; + } + } + } + + perf_end(_transfer_perf); + + // get current temperature at 1 Hz + if (hrt_elapsed_time(&_time_last_temperature_update) > 1_s) { + uint8_t temperature_buf[3] {}; + temperature_buf[0] = static_cast(Register::OUT_TEMP_L) | DIR_READ; + + if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) { + return; + } + + // 16 bits in two’s complement format with a sensitivity of 256 LSB/°C. The output zero level corresponds to 25 °C. + const int16_t OUT_TEMP = combine(temperature_buf[1], temperature_buf[2] & 0x0F); + const float temperature = (OUT_TEMP / 256.0f) + 25.0f; + + _px4_accel.set_temperature(temperature); + _px4_gyro.set_temperature(temperature); + } + + if ((accel_samples > 0) && (gyro_samples > 0)) { + // published synchronized IMU + _px4_accel.update(timestamp_sample, accel_data[0], accel_data[1], accel_data[2]); + _px4_gyro.update(timestamp_sample, gyro_data[0], gyro_data[1], gyro_data[2]); + } +} + +void LSM9DS1::PrintInfo() +{ + perf_print_counter(_interval_perf); + perf_print_counter(_transfer_perf); + perf_print_counter(_fifo_empty_perf); + perf_print_counter(_fifo_overflow_perf); + perf_print_counter(_fifo_reset_perf); + + _px4_accel.print_status(); + _px4_gyro.print_status(); +} diff --git a/src/drivers/imu/st/lsm9ds1/LSM9DS1.hpp b/src/drivers/imu/st/lsm9ds1/LSM9DS1.hpp new file mode 100644 index 0000000000..b2a0b3cb7f --- /dev/null +++ b/src/drivers/imu/st/lsm9ds1/LSM9DS1.hpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * 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 LSM9DS1.hpp + * + * Driver for the ST LSM9DS1 connected via SPI. + * + */ + +#pragma once + +#include "ST_LSM9DS1_Registers.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +using ST_LSM9DS1::Register; + +class LSM9DS1 : public device::SPI, public px4::ScheduledWorkItem +{ +public: + LSM9DS1(int bus, uint32_t device, enum Rotation rotation = ROTATION_NONE); + ~LSM9DS1() override; + + bool Init(); + void Start(); + void Stop(); + bool Reset(); + void PrintInfo(); + +private: + int probe() override; + void Run() override; + + uint8_t RegisterRead(Register reg); + void RegisterWrite(Register reg, uint8_t value); + void RegisterSetBits(Register reg, uint8_t setbits); + void RegisterClearBits(Register reg, uint8_t clearbits); + + void ResetFIFO(); + + PX4Accelerometer _px4_accel; + PX4Gyroscope _px4_gyro; + + static constexpr uint32_t _fifo_interval{1000000 / ST_LSM9DS1::LA_ODR}; + + perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": run interval")}; + perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")}; + perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo empty")}; + perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo overflow")}; + perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo reset")}; + + hrt_abstime _time_last_temperature_update{0}; +}; diff --git a/src/drivers/imu/st/lsm9ds1/ST_LSM9DS1_Registers.hpp b/src/drivers/imu/st/lsm9ds1/ST_LSM9DS1_Registers.hpp new file mode 100644 index 0000000000..06364de27b --- /dev/null +++ b/src/drivers/imu/st/lsm9ds1/ST_LSM9DS1_Registers.hpp @@ -0,0 +1,156 @@ +/**************************************************************************** + * + * 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 ST_LSM9DS1_registers.hpp + * + * ST LSM9DS1 registers. + * + */ + +#pragma once + +#include + +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +static constexpr uint8_t Bit7 = (1 << 7); + +namespace ST_LSM9DS1 +{ + +static constexpr uint8_t DIR_READ = 0x80; + +static constexpr uint8_t LSM9DS1_WHO_AM_I = 0b01101000; // Who I am ID + +static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10 MHz SPI clock frequency + +static constexpr uint32_t LA_ODR = 952; // Linear acceleration output data rate +static constexpr uint32_t G_ODR = 952; // Angular rate output data rate + +enum class +Register : uint8_t { + WHO_AM_I = 0x0F, + + CTRL_REG1_G = 0x10, // Angular rate sensor Control Register 1. + + OUT_TEMP_L = 0x15, + OUT_TEMP_H = 0x16, + STATUS_REG_G = 0x17, + OUT_X_L_G = 0x18, + OUT_X_H_G = 0x19, + OUT_Y_L_G = 0x1A, + OUT_Y_H_G = 0x1B, + OUT_Z_L_G = 0x1C, + OUT_Z_H_G = 0x1D, + + CTRL_REG6_XL = 0x20, // Linear acceleration sensor Control Register 6. + + CTRL_REG8 = 0x22, // Control register 8. + CTRL_REG9 = 0x23, // Control register 9. + + STATUS_REG_A = 0x27, + OUT_X_L_XL = 0x28, + OUT_X_H_XL = 0x29, + OUT_Y_L_XL = 0x2A, + OUT_Y_H_XL = 0x2B, + OUT_Z_L_XL = 0x2C, + OUT_Z_H_XL = 0x2D, + FIFO_CTRL = 0x2E, // FIFO control register. + FIFO_SRC = 0x2F, // FIFO status control register. +}; + +// CTRL_REG1_G +enum +CTRL_REG1_G_BIT : uint8_t { + ODR_G_952HZ = Bit7 | Bit6, + FS_G_2000DPS = Bit4 | Bit3, + BW_G_100Hz = Bit1 | Bit2, // BW_G 100 Hz +}; + +// STATUS_REG (both STATUS_REG_A 0x17 and STATUS_REG_G 0x27) +enum +STATUS_REG_BIT : uint8_t { + TDA = Bit2, // Temperature sensor new data available. + GDA = Bit1, // Gyroscope new data available. + XLDA = Bit0, // Accelerometer new data available. +}; + +// CTRL_REG6_XL +enum +CTRL_REG6_XL_BIT : uint8_t { + ODR_XL_952HZ = Bit7 | Bit6, // 952 Hz ODR + FS_XL_16 = Bit3, // FS_XL 01: ±16 g +}; + +// CTRL_REG8 +enum +CTRL_REG8_BIT : uint8_t { + BDU = Bit6, // Block data update + IF_ADD_INC = Bit2, // Register address automatically incremented + SW_RESET = Bit0, // Software reset +}; + +// CTRL_REG9 +enum +CTRL_REG9_BIT : uint8_t { + I2C_DISABLE = Bit2, + FIFO_EN = Bit1, +}; + +// FIFO_CTRL +enum +FIFO_CTRL_BIT : uint8_t { + FIFO_MODE_CONTINUOUS = Bit7 | Bit6, // Continuous mode. If the FIFO is full, the new sample over-writes the older sample. +}; + +// FIFO_SRC +enum +FIFO_SRC_BIT : uint8_t { + OVRN = Bit6, // FIFO overrun status. + FSS = Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0, +}; + + +namespace FIFO +{ +static constexpr size_t SIZE = 32 * 6; // 32 samples max +} + +} // namespace ST_LSM9DS1 diff --git a/src/drivers/imu/st/lsm9ds1/lsm9ds1_main.cpp b/src/drivers/imu/st/lsm9ds1/lsm9ds1_main.cpp new file mode 100644 index 0000000000..42b9933aa5 --- /dev/null +++ b/src/drivers/imu/st/lsm9ds1/lsm9ds1_main.cpp @@ -0,0 +1,143 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include "LSM9DS1.hpp" + +#include + +namespace lsm9ds1 +{ + +LSM9DS1 *g_dev{nullptr}; + +static int start(enum Rotation rotation) +{ + if (g_dev != nullptr) { + PX4_WARN("already started"); + return 0; + } + + // create the driver +#if defined(PX4_SPI_BUS_SENSORS) && defined(PX4_SPIDEV_LSM9DS1_AG) + g_dev = new LSM9DS1(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_LSM9DS1_AG, rotation); +#endif // PX4_SPI_BUS_SENSORS && PX4_SPIDEV_LSM9DS1_AG + + if (g_dev == nullptr) { + PX4_ERR("driver start failed"); + return -1; + } + + if (!g_dev->Init()) { + PX4_ERR("driver init failed"); + delete g_dev; + g_dev = nullptr; + return -1; + } + + return 0; +} + +static int stop() +{ + if (g_dev == nullptr) { + PX4_WARN("driver not running"); + } + + g_dev->Stop(); + delete g_dev; + + return 0; +} + +static int status() +{ + if (g_dev == nullptr) { + PX4_WARN("driver not running"); + return -1; + } + + g_dev->PrintInfo(); + + return 0; +} + +static void usage() +{ + PX4_INFO("missing command: try 'start', 'stop', 'status'"); + PX4_INFO("options:"); + PX4_INFO(" -R rotation"); +} + +} // namespace lsm9ds1 + +extern "C" __EXPORT int lsm9ds1_main(int argc, char *argv[]) +{ + enum Rotation rotation = ROTATION_NONE; + int myoptind = 1; + int ch = 0; + const char *myoptarg = nullptr; + + /* start options */ + while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'R': + rotation = (enum Rotation)atoi(myoptarg); + break; + + default: + lsm9ds1::usage(); + return 0; + } + } + + if (myoptind >= argc) { + lsm9ds1::usage(); + return -1; + } + + const char *verb = argv[myoptind]; + + if (!strcmp(verb, "start")) { + return lsm9ds1::start(rotation); + + } else if (!strcmp(verb, "stop")) { + return lsm9ds1::stop(); + + } else if (!strcmp(verb, "status")) { + return lsm9ds1::status(); + } + + lsm9ds1::usage(); + + return 0; +} diff --git a/src/drivers/magnetometer/lsm9ds1_mag/CMakeLists.txt b/src/drivers/magnetometer/lsm9ds1_mag/CMakeLists.txt new file mode 100644 index 0000000000..8c12ac8c17 --- /dev/null +++ b/src/drivers/magnetometer/lsm9ds1_mag/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# 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__mag__st__lsm9ds1_mag + MAIN lsm9ds1_mag + SRCS + lsm9ds1_mag_main.cpp + LSM9DS1_MAG.cpp + LSM9DS1_MAG.hpp + ST_LSM9DS1_MAG_Registers.hpp + DEPENDS + drivers_magnetometer + px4_work_queue + ) diff --git a/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp b/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp new file mode 100644 index 0000000000..07bb3f167b --- /dev/null +++ b/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp @@ -0,0 +1,213 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include "LSM9DS1_MAG.hpp" + +using namespace ST_LSM9DS1_MAG; +using ST_LSM9DS1_MAG::Register; + +using namespace time_literals; + +static constexpr int16_t combine(uint8_t lsb, uint8_t msb) { return (msb << 8u) | lsb; } + +LSM9DS1_MAG::LSM9DS1_MAG(int bus, uint32_t device, enum Rotation rotation) : + SPI(MODULE_NAME, nullptr, bus, device, SPIDEV_MODE3, SPI_SPEED), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), + _px4_mag(get_device_id(), ORB_PRIO_DEFAULT, rotation) +{ + set_device_type(DRV_MAG_DEVTYPE_ST_LSM9DS1_M); + _px4_mag.set_device_type(DRV_MAG_DEVTYPE_ST_LSM9DS1_M); + + _px4_mag.set_temperature(NAN); // temperature not available +} + +LSM9DS1_MAG::~LSM9DS1_MAG() +{ + Stop(); + + perf_free(_interval_perf); + perf_free(_transfer_perf); + perf_free(_data_overrun_perf); +} + +int LSM9DS1_MAG::probe() +{ + if (RegisterRead(Register::WHO_AM_I) == LSM9DS1_MAG_WHO_AM_I) { + return PX4_OK; + } + + return PX4_ERROR; +} + +bool LSM9DS1_MAG::Init() +{ + if (SPI::init() != PX4_OK) { + PX4_ERR("SPI::init failed"); + return false; + } + + if (!Reset()) { + PX4_ERR("reset failed"); + return false; + } + + Start(); + + return true; +} + +bool LSM9DS1_MAG::Reset() +{ + // Reset + // CTRL_REG2_M: SOFT_RST + RegisterWrite(Register::CTRL_REG2_M, CTRL_REG2_M_BIT::SOFT_RST); + usleep(50); + + // CTRL_REG1_M: Temp comp, ultra high perofmrance mode, ODR 80 Hz, fast ODR + RegisterWrite(Register::CTRL_REG1_M, + CTRL_REG1_M_BIT::TEMP_COMP | CTRL_REG1_M_BIT::OM_ULTRA_HIGH_PERFORMANCE | CTRL_REG1_M_BIT::DO_80HZ); + + // CTRL_REG2_M: + RegisterSetBits(Register::CTRL_REG2_M, CTRL_REG2_M_BIT::FS_16_GAUSS); + _px4_mag.set_scale(0.58f / 1000.0f); // Magnetic FS = ±16 gauss 0.58 mgauss/LSB + + // CTRL_REG3_M: I2C_DISABLE, Continuous-conversion mode + RegisterClearBits(Register::CTRL_REG3_M, CTRL_REG3_M_BIT::MD_CONTINUOUS_MODE); + + // CTRL_REG4_M: Z-axis Ultra-high performance mode + RegisterSetBits(Register::CTRL_REG4_M, CTRL_REG4_M_BIT::OMZ_ULTRA_HIGH_PERFORMANCE); + + // CTRL_REG5_M: Block data update for magnetic data. + RegisterSetBits(Register::CTRL_REG5_M, CTRL_REG5_M_BIT::BDU); + + return true; +} + +uint8_t LSM9DS1_MAG::RegisterRead(Register reg) +{ + uint8_t cmd[2] {}; + cmd[0] = static_cast(reg) | RW_BIT_READ; + transfer(cmd, cmd, sizeof(cmd)); + return cmd[1]; +} + +void LSM9DS1_MAG::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t cmd[2] { (uint8_t)reg, value }; + transfer(cmd, cmd, sizeof(cmd)); +} + +void LSM9DS1_MAG::RegisterSetBits(Register reg, uint8_t setbits) +{ + uint8_t val = RegisterRead(reg); + + if (!(val & setbits)) { + val |= setbits; + RegisterWrite(reg, val); + } +} + +void LSM9DS1_MAG::RegisterClearBits(Register reg, uint8_t clearbits) +{ + uint8_t val = RegisterRead(reg); + + if (val & clearbits) { + val &= !clearbits; + RegisterWrite(reg, val); + } +} + +void LSM9DS1_MAG::Start() +{ + Stop(); + + ScheduleOnInterval(1000000 / ST_LSM9DS1_MAG::M_ODR / 2); +} + +void LSM9DS1_MAG::Stop() +{ + ScheduleClear(); +} + +void LSM9DS1_MAG::Run() +{ + perf_count(_interval_perf); + + struct MagReport { + uint8_t cmd; + uint8_t STATUS_REG_M; + uint8_t OUT_X_L_M; + uint8_t OUT_X_H_M; + uint8_t OUT_Y_L_M; + uint8_t OUT_Y_H_M; + uint8_t OUT_Z_L_M; + uint8_t OUT_Z_H_M; + } mreport{}; + mreport.cmd = static_cast(Register::STATUS_REG_M) | RW_BIT_READ | MS_BIT_AUTO_INCREMENT; + + perf_begin(_transfer_perf); + const hrt_abstime timestamp_sample = hrt_absolute_time(); + + if (transfer((uint8_t *)&mreport, (uint8_t *)&mreport, sizeof(MagReport)) != PX4_OK) { + perf_end(_transfer_perf); + return; + } + + perf_end(_transfer_perf); + + if (mreport.STATUS_REG_M & STATUS_REG_M_BIT::ZYXOR) { + // X, Y and Z-axis data overrun. + perf_count(_data_overrun_perf); + return; + } + + if (mreport.STATUS_REG_M & STATUS_REG_M_BIT::ZYXDA) { + // X, Y and Z-axis new data available. + + // sensor X is left, Y is back, and Z is up + int16_t x = -combine(mreport.OUT_Y_L_M, mreport.OUT_Y_H_M); // X := -Y + int16_t y = -combine(mreport.OUT_X_L_M, mreport.OUT_X_H_M); // Y := -X + int16_t z = -combine(mreport.OUT_Z_L_M, mreport.OUT_Z_H_M); // Z := -Z + + _px4_mag.update(timestamp_sample, x, y, z); + } +} + +void LSM9DS1_MAG::PrintInfo() +{ + perf_print_counter(_interval_perf); + perf_print_counter(_transfer_perf); + perf_print_counter(_data_overrun_perf); + + _px4_mag.print_status(); +} diff --git a/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.hpp b/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.hpp new file mode 100644 index 0000000000..f62b28d3ed --- /dev/null +++ b/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.hpp @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * 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 LSM9DS1_MAG.hpp + * + * Driver for the ST LSM9DS1 magnetometer connected via SPI. + * + */ + +#pragma once + +#include "ST_LSM9DS1_MAG_Registers.hpp" + +#include +#include +#include +#include +#include +#include + +class LSM9DS1_MAG : public device::SPI, public px4::ScheduledWorkItem +{ +public: + LSM9DS1_MAG(int bus, uint32_t device, enum Rotation rotation = ROTATION_NONE); + ~LSM9DS1_MAG() override; + + bool Init(); + void Start(); + void Stop(); + bool Reset(); + void PrintInfo(); + +private: + int probe() override; + void Run() override; + + uint8_t RegisterRead(ST_LSM9DS1_MAG::Register reg); + void RegisterWrite(ST_LSM9DS1_MAG::Register reg, uint8_t value); + void RegisterSetBits(ST_LSM9DS1_MAG::Register reg, uint8_t setbits); + void RegisterClearBits(ST_LSM9DS1_MAG::Register reg, uint8_t clearbits); + + PX4Magnetometer _px4_mag; + + perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": run interval")}; + perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")}; + perf_counter_t _data_overrun_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": overrun")}; +}; diff --git a/src/drivers/magnetometer/lsm9ds1_mag/ST_LSM9DS1_MAG_Registers.hpp b/src/drivers/magnetometer/lsm9ds1_mag/ST_LSM9DS1_MAG_Registers.hpp new file mode 100644 index 0000000000..79ec3668ac --- /dev/null +++ b/src/drivers/magnetometer/lsm9ds1_mag/ST_LSM9DS1_MAG_Registers.hpp @@ -0,0 +1,132 @@ +/**************************************************************************** + * + * 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 ST_LSM9DS1_MAG_registers.hpp + * + * ST LSM9DS1 magnetometer registers. + * + */ + +#pragma once + +#include + +namespace ST_LSM9DS1_MAG +{ + +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +static constexpr uint8_t Bit7 = (1 << 7); + +static constexpr uint8_t RW_BIT_READ = Bit7; +static constexpr uint8_t MS_BIT_AUTO_INCREMENT = Bit6; + +static constexpr uint8_t LSM9DS1_MAG_WHO_AM_I = 0b00111101; // Who I am ID + +static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10 MHz SPI clock frequency + +static constexpr uint32_t M_ODR = 80; // Magnetometer output data rate + +enum class +Register : uint8_t { + WHO_AM_I = 0x0F, + + CTRL_REG1_M = 0x20, + CTRL_REG2_M = 0x21, + CTRL_REG3_M = 0x22, + CTRL_REG4_M = 0x23, + CTRL_REG5_M = 0x25, + + STATUS_REG_M = 0x27, + OUT_X_L_M = 0x28, + OUT_X_H_M = 0x29, + OUT_Y_L_M = 0x2A, + OUT_Y_H_M = 0x2B, + OUT_Z_L_M = 0x2C, + OUT_Z_H_M = 0x2D, +}; + +// CTRL_REG1_M +enum +CTRL_REG1_M_BIT : uint8_t { + TEMP_COMP = Bit7, // Temperature compensation enable. + OM_ULTRA_HIGH_PERFORMANCE = Bit6 | Bit5, // X and Y axes operative mode selection. + DO_80HZ = Bit4 | Bit3 | Bit2, // 80 Hz Output data rate selection. + FAST_ODR = Bit1, + ST = Bit0, // Self-test enable. +}; + +// CTRL_REG2_M +enum +CTRL_REG2_M_BIT : uint8_t { + FS_16_GAUSS = Bit6 | Bit5, // Full-scale selection ± 16 gauss + + SOFT_RST = Bit2, +}; + +// CTRL_REG3_M +enum +CTRL_REG3_M_BIT : uint8_t { + I2C_DISABLE = Bit7, + + MD_CONTINUOUS_MODE = Bit1 | Bit0, +}; + +// CTRL_REG4_M +enum +CTRL_REG4_M_BIT : uint8_t { + OMZ_ULTRA_HIGH_PERFORMANCE = Bit4 | Bit3, // Ultra-high performance mode +}; + +// CTRL_REG5_M +enum +CTRL_REG5_M_BIT : uint8_t { + BDU = Bit6, // Block data update for magnetic data. +}; + + +// STATUS_REG_M +enum +STATUS_REG_M_BIT : uint8_t { + ZYXOR = Bit7, // X, Y and Z-axis data overrun. + ZYXDA = Bit3, // X, Y and Z-axis new data available. +}; + +} // namespace ST_LSM9DS1_MAG diff --git a/src/drivers/magnetometer/lsm9ds1_mag/lsm9ds1_mag_main.cpp b/src/drivers/magnetometer/lsm9ds1_mag/lsm9ds1_mag_main.cpp new file mode 100644 index 0000000000..bf2846bbbb --- /dev/null +++ b/src/drivers/magnetometer/lsm9ds1_mag/lsm9ds1_mag_main.cpp @@ -0,0 +1,143 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include "LSM9DS1_MAG.hpp" + +#include + +namespace lsm9ds1_mag +{ + +LSM9DS1_MAG *g_dev{nullptr}; + +static int start(enum Rotation rotation) +{ + if (g_dev != nullptr) { + PX4_WARN("already started"); + return 0; + } + + // create the driver +#if defined(PX4_SPI_BUS_SENSORS) && defined(PX4_SPIDEV_LSM9DS1_M) + g_dev = new LSM9DS1_MAG(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_LSM9DS1_M, rotation); +#endif // PX4_SPI_BUS_SENSORS && PX4_SPIDEV_LSM9DS1_M + + if (g_dev == nullptr) { + PX4_ERR("driver start failed"); + return -1; + } + + if (!g_dev->Init()) { + PX4_ERR("driver init failed"); + delete g_dev; + g_dev = nullptr; + return -1; + } + + return 0; +} + +static int stop() +{ + if (g_dev == nullptr) { + PX4_WARN("driver not running"); + } + + g_dev->Stop(); + delete g_dev; + + return 0; +} + +static int status() +{ + if (g_dev == nullptr) { + PX4_WARN("driver not running"); + return -1; + } + + g_dev->PrintInfo(); + + return 0; +} + +static void usage() +{ + PX4_INFO("missing command: try 'start', 'stop', 'status'"); + PX4_INFO("options:"); + PX4_INFO(" -R rotation"); +} + +} // namespace lsm9ds1_mag + +extern "C" __EXPORT int lsm9ds1_mag_main(int argc, char *argv[]) +{ + enum Rotation rotation = ROTATION_NONE; + int myoptind = 1; + int ch = 0; + const char *myoptarg = nullptr; + + /* start options */ + while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'R': + rotation = (enum Rotation)atoi(myoptarg); + break; + + default: + lsm9ds1_mag::usage(); + return 0; + } + } + + if (myoptind >= argc) { + lsm9ds1_mag::usage(); + return -1; + } + + const char *verb = argv[myoptind]; + + if (!strcmp(verb, "start")) { + return lsm9ds1_mag::start(rotation); + + } else if (!strcmp(verb, "stop")) { + return lsm9ds1_mag::stop(); + + } else if (!strcmp(verb, "status")) { + return lsm9ds1_mag::status(); + } + + lsm9ds1_mag::usage(); + + return 0; +} diff --git a/src/lib/drivers/device/posix/SPI.cpp b/src/lib/drivers/device/posix/SPI.cpp index 19e4c541f0..90a50ab012 100644 --- a/src/lib/drivers/device/posix/SPI.cpp +++ b/src/lib/drivers/device/posix/SPI.cpp @@ -128,15 +128,13 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) return PX4_ERROR; } - spi_ioc_transfer spi_transfer[1] {}; // datastructures for linux spi interface + spi_ioc_transfer spi_transfer{}; - spi_transfer[0].tx_buf = (uint64_t)send; - spi_transfer[0].rx_buf = (uint64_t)recv; - spi_transfer[0].len = len; - spi_transfer[0].speed_hz = _frequency; - spi_transfer[0].bits_per_word = 8; - //spi_transfer[0].delay_usecs = 10; - spi_transfer[0].cs_change = true; + spi_transfer.tx_buf = (uint64_t)send; + spi_transfer.rx_buf = (uint64_t)recv; + spi_transfer.len = len; + spi_transfer.speed_hz = _frequency; + spi_transfer.bits_per_word = 8; result = ::ioctl(_fd, SPI_IOC_MESSAGE(1), &spi_transfer); @@ -171,7 +169,7 @@ SPI::transferhword(uint16_t *send, uint16_t *recv, unsigned len) return PX4_ERROR; } - spi_ioc_transfer spi_transfer[1] {}; // datastructures for linux spi interface + spi_ioc_transfer spi_transfer[1] {}; spi_transfer[0].tx_buf = (uint64_t)send; spi_transfer[0].rx_buf = (uint64_t)recv;