dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ST LSM9DS1 9DOF IMU driver 

- this is a replacement for df_lsm9ds1_wrapper on the Emlid Navio2 once DriverFramework is removed
This commit is contained in:
Daniel Agar 2020-01-04 13:03:42 -05:00
parent f3cd5b19c8
commit 298d6d8703
16 changed files with 1398 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
boards/emlid/navio2/default.cmake
View File

@ -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

2
posix-configs/rpi/px4.config
View File

@ -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

2
posix-configs/rpi/px4_fw.config
View File

@ -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

2
posix-configs/rpi/px4_test.config
View File

@ -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

15
src/drivers/drv_sensor.h
View File

@ -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

46
src/drivers/imu/st/lsm9ds1/CMakeLists.txt Normal file
View File

@ -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
)

328
src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp Normal file
View File

@ -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<uint8_t>(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<uint8_t>(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<uint8_t>(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<uint8_t>(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<uint8_t>(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();
}

91
src/drivers/imu/st/lsm9ds1/LSM9DS1.hpp Normal file
View File

@ -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 <drivers/drv_hrt.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/device/spi.h>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/ecl/geo/geo.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
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};
};

156
src/drivers/imu/st/lsm9ds1/ST_LSM9DS1_Registers.hpp Normal file
View File

@ -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 <cstdint>
// 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

143
src/drivers/imu/st/lsm9ds1/lsm9ds1_main.cpp Normal file
View File

@ -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 <px4_platform_common/getopt.h>
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;
}

45
src/drivers/magnetometer/lsm9ds1_mag/CMakeLists.txt Normal file
View File

@ -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
)

213
src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp Normal file
View File

@ -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<uint8_t>(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<uint8_t>(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();
}

78
src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.hpp Normal file
View File

@ -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 <drivers/drv_hrt.h>
#include <lib/drivers/device/spi.h>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
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")};
};

132
src/drivers/magnetometer/lsm9ds1_mag/ST_LSM9DS1_MAG_Registers.hpp Normal file
View File

@ -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 <cstdint>
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

143
src/drivers/magnetometer/lsm9ds1_mag/lsm9ds1_mag_main.cpp Normal file
View File

@ -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 <px4_platform_common/getopt.h>
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;
}

16
src/lib/drivers/device/posix/SPI.cpp
View File

@ -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;