mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-03 16:58:54 +08:00
lsm303d split out header and main
This commit is contained in:
Daniel Agar
@@ -37,8 +37,10 @@ px4_add_module(
|
||||
COMPILE_FLAGS
|
||||
-Wno-cast-align # TODO: fix and enable
|
||||
SRCS
|
||||
lsm303d.cpp
|
||||
lsm303d_main.cpp
|
||||
LSM303D.cpp
|
||||
DEPENDS
|
||||
drivers_accelerometer
|
||||
drivers_magnetometer
|
||||
px4_work_queue
|
||||
)
|
||||
|
||||
@@ -32,315 +32,11 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file lsm303d.cpp
|
||||
* @file LSM303D.cpp
|
||||
* Driver for the ST LSM303D MEMS accelerometer / magnetometer connected via SPI.
|
||||
*/
|
||||
|
||||
#include <drivers/device/spi.h>
|
||||
#include <ecl/geo/geo.h>
|
||||
#include <perf/perf_counter.h>
|
||||
#include <px4_getopt.h>
|
||||
#include <px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
|
||||
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
|
||||
|
||||
/* SPI protocol address bits */
|
||||
#define DIR_READ (1<<7)
|
||||
#define DIR_WRITE (0<<7)
|
||||
#define ADDR_INCREMENT (1<<6)
|
||||
|
||||
/* register addresses: A: accel, M: mag, T: temp */
|
||||
#define ADDR_WHO_AM_I 0x0F
|
||||
#define WHO_I_AM 0x49
|
||||
|
||||
#define ADDR_OUT_TEMP_L 0x05
|
||||
#define ADDR_OUT_TEMP_H 0x06
|
||||
#define ADDR_STATUS_M 0x07
|
||||
#define ADDR_OUT_X_L_M 0x08
|
||||
#define ADDR_OUT_X_H_M 0x09
|
||||
#define ADDR_OUT_Y_L_M 0x0A
|
||||
#define ADDR_OUT_Y_H_M 0x0B
|
||||
#define ADDR_OUT_Z_L_M 0x0C
|
||||
#define ADDR_OUT_Z_H_M 0x0D
|
||||
|
||||
#define ADDR_INT_CTRL_M 0x12
|
||||
#define ADDR_INT_SRC_M 0x13
|
||||
#define ADDR_REFERENCE_X 0x1c
|
||||
#define ADDR_REFERENCE_Y 0x1d
|
||||
#define ADDR_REFERENCE_Z 0x1e
|
||||
|
||||
#define ADDR_STATUS_A 0x27
|
||||
#define ADDR_OUT_X_L_A 0x28
|
||||
#define ADDR_OUT_X_H_A 0x29
|
||||
#define ADDR_OUT_Y_L_A 0x2A
|
||||
#define ADDR_OUT_Y_H_A 0x2B
|
||||
#define ADDR_OUT_Z_L_A 0x2C
|
||||
#define ADDR_OUT_Z_H_A 0x2D
|
||||
|
||||
#define ADDR_CTRL_REG0 0x1F
|
||||
#define ADDR_CTRL_REG1 0x20
|
||||
#define ADDR_CTRL_REG2 0x21
|
||||
#define ADDR_CTRL_REG3 0x22
|
||||
#define ADDR_CTRL_REG4 0x23
|
||||
#define ADDR_CTRL_REG5 0x24
|
||||
#define ADDR_CTRL_REG6 0x25
|
||||
#define ADDR_CTRL_REG7 0x26
|
||||
|
||||
#define ADDR_FIFO_CTRL 0x2e
|
||||
#define ADDR_FIFO_SRC 0x2f
|
||||
|
||||
#define ADDR_IG_CFG1 0x30
|
||||
#define ADDR_IG_SRC1 0x31
|
||||
#define ADDR_IG_THS1 0x32
|
||||
#define ADDR_IG_DUR1 0x33
|
||||
#define ADDR_IG_CFG2 0x34
|
||||
#define ADDR_IG_SRC2 0x35
|
||||
#define ADDR_IG_THS2 0x36
|
||||
#define ADDR_IG_DUR2 0x37
|
||||
#define ADDR_CLICK_CFG 0x38
|
||||
#define ADDR_CLICK_SRC 0x39
|
||||
#define ADDR_CLICK_THS 0x3a
|
||||
#define ADDR_TIME_LIMIT 0x3b
|
||||
#define ADDR_TIME_LATENCY 0x3c
|
||||
#define ADDR_TIME_WINDOW 0x3d
|
||||
#define ADDR_ACT_THS 0x3e
|
||||
#define ADDR_ACT_DUR 0x3f
|
||||
|
||||
#define REG1_RATE_BITS_A ((1<<7) | (1<<6) | (1<<5) | (1<<4))
|
||||
#define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4))
|
||||
#define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4))
|
||||
#define REG1_RATE_6_25HZ_A ((0<<7) | (0<<6) | (1<<5) | (0<<4))
|
||||
#define REG1_RATE_12_5HZ_A ((0<<7) | (0<<6) | (1<<5) | (1<<4))
|
||||
#define REG1_RATE_25HZ_A ((0<<7) | (1<<6) | (0<<5) | (0<<4))
|
||||
#define REG1_RATE_50HZ_A ((0<<7) | (1<<6) | (0<<5) | (1<<4))
|
||||
#define REG1_RATE_100HZ_A ((0<<7) | (1<<6) | (1<<5) | (0<<4))
|
||||
#define REG1_RATE_200HZ_A ((0<<7) | (1<<6) | (1<<5) | (1<<4))
|
||||
#define REG1_RATE_400HZ_A ((1<<7) | (0<<6) | (0<<5) | (0<<4))
|
||||
#define REG1_RATE_800HZ_A ((1<<7) | (0<<6) | (0<<5) | (1<<4))
|
||||
#define REG1_RATE_1600HZ_A ((1<<7) | (0<<6) | (1<<5) | (0<<4))
|
||||
|
||||
#define REG1_BDU_UPDATE (1<<3)
|
||||
#define REG1_Z_ENABLE_A (1<<2)
|
||||
#define REG1_Y_ENABLE_A (1<<1)
|
||||
#define REG1_X_ENABLE_A (1<<0)
|
||||
|
||||
#define REG2_ANTIALIAS_FILTER_BW_BITS_A ((1<<7) | (1<<6))
|
||||
#define REG2_AA_FILTER_BW_773HZ_A ((0<<7) | (0<<6))
|
||||
#define REG2_AA_FILTER_BW_194HZ_A ((0<<7) | (1<<6))
|
||||
#define REG2_AA_FILTER_BW_362HZ_A ((1<<7) | (0<<6))
|
||||
#define REG2_AA_FILTER_BW_50HZ_A ((1<<7) | (1<<6))
|
||||
|
||||
#define REG2_FULL_SCALE_BITS_A ((1<<5) | (1<<4) | (1<<3))
|
||||
#define REG2_FULL_SCALE_2G_A ((0<<5) | (0<<4) | (0<<3))
|
||||
#define REG2_FULL_SCALE_4G_A ((0<<5) | (0<<4) | (1<<3))
|
||||
#define REG2_FULL_SCALE_6G_A ((0<<5) | (1<<4) | (0<<3))
|
||||
#define REG2_FULL_SCALE_8G_A ((0<<5) | (1<<4) | (1<<3))
|
||||
#define REG2_FULL_SCALE_16G_A ((1<<5) | (0<<4) | (0<<3))
|
||||
|
||||
#define REG5_ENABLE_T (1<<7)
|
||||
|
||||
#define REG5_RES_HIGH_M ((1<<6) | (1<<5))
|
||||
#define REG5_RES_LOW_M ((0<<6) | (0<<5))
|
||||
|
||||
#define REG5_RATE_BITS_M ((1<<4) | (1<<3) | (1<<2))
|
||||
#define REG5_RATE_3_125HZ_M ((0<<4) | (0<<3) | (0<<2))
|
||||
#define REG5_RATE_6_25HZ_M ((0<<4) | (0<<3) | (1<<2))
|
||||
#define REG5_RATE_12_5HZ_M ((0<<4) | (1<<3) | (0<<2))
|
||||
#define REG5_RATE_25HZ_M ((0<<4) | (1<<3) | (1<<2))
|
||||
#define REG5_RATE_50HZ_M ((1<<4) | (0<<3) | (0<<2))
|
||||
#define REG5_RATE_100HZ_M ((1<<4) | (0<<3) | (1<<2))
|
||||
#define REG5_RATE_DO_NOT_USE_M ((1<<4) | (1<<3) | (0<<2))
|
||||
|
||||
#define REG6_FULL_SCALE_BITS_M ((1<<6) | (1<<5))
|
||||
#define REG6_FULL_SCALE_2GA_M ((0<<6) | (0<<5))
|
||||
#define REG6_FULL_SCALE_4GA_M ((0<<6) | (1<<5))
|
||||
#define REG6_FULL_SCALE_8GA_M ((1<<6) | (0<<5))
|
||||
#define REG6_FULL_SCALE_12GA_M ((1<<6) | (1<<5))
|
||||
|
||||
#define REG7_CONT_MODE_M ((0<<1) | (0<<0))
|
||||
|
||||
#define REG_STATUS_A_NEW_ZYXADA 0x08
|
||||
|
||||
#define INT_CTRL_M 0x12
|
||||
#define INT_SRC_M 0x13
|
||||
|
||||
/* default values for this device */
|
||||
#define LSM303D_ACCEL_DEFAULT_RANGE_G 16
|
||||
#define LSM303D_ACCEL_DEFAULT_RATE 800
|
||||
#define LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50
|
||||
#define LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
|
||||
|
||||
#define LSM303D_MAG_DEFAULT_RANGE_GA 2
|
||||
#define LSM303D_MAG_DEFAULT_RATE 100
|
||||
|
||||
/*
|
||||
we set the timer interrupt to run a bit faster than the desired
|
||||
sample rate and then throw away duplicates using the data ready bit.
|
||||
This time reduction is enough to cope with worst case timing jitter
|
||||
due to other timers
|
||||
*/
|
||||
#define LSM303D_TIMER_REDUCTION 200
|
||||
|
||||
extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
|
||||
|
||||
class LSM303D : public device::SPI, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
LSM303D(int bus, uint32_t device, enum Rotation rotation);
|
||||
virtual ~LSM303D();
|
||||
|
||||
virtual int init();
|
||||
|
||||
/**
|
||||
* Diagnostics - print some basic information about the driver.
|
||||
*/
|
||||
void print_info();
|
||||
|
||||
protected:
|
||||
virtual int probe();
|
||||
|
||||
private:
|
||||
|
||||
void Run() override;
|
||||
|
||||
void start();
|
||||
void stop();
|
||||
void reset();
|
||||
|
||||
/**
|
||||
* disable I2C on the chip
|
||||
*/
|
||||
void disable_i2c();
|
||||
|
||||
/**
|
||||
* check key registers for correct values
|
||||
*/
|
||||
void check_registers(void);
|
||||
|
||||
/**
|
||||
* Fetch accel measurements from the sensor and update the report ring.
|
||||
*/
|
||||
void measureAccelerometer();
|
||||
|
||||
/**
|
||||
* Fetch mag measurements from the sensor and update the report ring.
|
||||
*/
|
||||
void measureMagnetometer();
|
||||
|
||||
/**
|
||||
* Read a register from the LSM303D
|
||||
*
|
||||
* @param The register to read.
|
||||
* @return The value that was read.
|
||||
*/
|
||||
uint8_t read_reg(unsigned reg);
|
||||
|
||||
/**
|
||||
* Write a register in the LSM303D
|
||||
*
|
||||
* @param reg The register to write.
|
||||
* @param value The new value to write.
|
||||
*/
|
||||
void write_reg(unsigned reg, uint8_t value);
|
||||
|
||||
/**
|
||||
* Modify a register in the LSM303D
|
||||
*
|
||||
* Bits are cleared before bits are set.
|
||||
*
|
||||
* @param reg The register to modify.
|
||||
* @param clearbits Bits in the register to clear.
|
||||
* @param setbits Bits in the register to set.
|
||||
*/
|
||||
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
|
||||
|
||||
/**
|
||||
* Write a register in the LSM303D, updating _checked_values
|
||||
*
|
||||
* @param reg The register to write.
|
||||
* @param value The new value to write.
|
||||
*/
|
||||
void write_checked_reg(unsigned reg, uint8_t value);
|
||||
|
||||
/**
|
||||
* Set the LSM303D accel measurement range.
|
||||
*
|
||||
* @param max_g The measurement range of the accel is in g (9.81m/s^2)
|
||||
* Zero selects the maximum supported range.
|
||||
* @return OK if the value can be supported, -ERANGE otherwise.
|
||||
*/
|
||||
int accel_set_range(unsigned max_g);
|
||||
|
||||
/**
|
||||
* Set the LSM303D mag measurement range.
|
||||
*
|
||||
* @param max_ga The measurement range of the mag is in Ga
|
||||
* Zero selects the maximum supported range.
|
||||
* @return OK if the value can be supported, -ERANGE otherwise.
|
||||
*/
|
||||
int mag_set_range(unsigned max_g);
|
||||
|
||||
/**
|
||||
* Set the LSM303D on-chip anti-alias filter bandwith.
|
||||
*
|
||||
* @param bandwidth The anti-alias filter bandwidth in Hz
|
||||
* Zero selects the highest bandwidth
|
||||
* @return OK if the value can be supported, -ERANGE otherwise.
|
||||
*/
|
||||
int accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth);
|
||||
|
||||
/**
|
||||
* Set the LSM303D internal accel sampling frequency.
|
||||
*
|
||||
* @param frequency The internal accel sampling frequency is set to not less than
|
||||
* this value.
|
||||
* Zero selects the maximum rate supported.
|
||||
* @return OK if the value can be supported.
|
||||
*/
|
||||
int accel_set_samplerate(unsigned frequency);
|
||||
|
||||
/**
|
||||
* Set the LSM303D internal mag sampling frequency.
|
||||
*
|
||||
* @param frequency The internal mag sampling frequency is set to not less than
|
||||
* this value.
|
||||
* Zero selects the maximum rate supported.
|
||||
* @return OK if the value can be supported.
|
||||
*/
|
||||
int mag_set_samplerate(unsigned frequency);
|
||||
|
||||
|
||||
PX4Accelerometer _px4_accel;
|
||||
PX4Magnetometer _px4_mag;
|
||||
|
||||
unsigned _call_accel_interval{1000000 / LSM303D_ACCEL_DEFAULT_RATE};
|
||||
unsigned _call_mag_interval{1000000 / LSM303D_MAG_DEFAULT_RATE};
|
||||
|
||||
perf_counter_t _accel_sample_perf;
|
||||
perf_counter_t _accel_sample_interval_perf;
|
||||
perf_counter_t _mag_sample_perf;
|
||||
perf_counter_t _mag_sample_interval_perf;
|
||||
perf_counter_t _bad_registers;
|
||||
perf_counter_t _bad_values;
|
||||
perf_counter_t _accel_duplicates;
|
||||
|
||||
uint8_t _register_wait{0};
|
||||
|
||||
int16_t _last_accel[3] {};
|
||||
uint8_t _constant_accel_count{0};
|
||||
|
||||
hrt_abstime _mag_last_measure{0};
|
||||
|
||||
float _last_temperature{0.0f};
|
||||
|
||||
// this is used to support runtime checking of key
|
||||
// configuration registers to detect SPI bus errors and sensor
|
||||
// reset
|
||||
static constexpr int LSM303D_NUM_CHECKED_REGISTERS{8};
|
||||
uint8_t _checked_values[LSM303D_NUM_CHECKED_REGISTERS] {};
|
||||
uint8_t _checked_next{0};
|
||||
|
||||
};
|
||||
#include "LSM303D.hpp"
|
||||
|
||||
/*
|
||||
list of registers that will be checked in check_registers(). Note
|
||||
@@ -372,6 +68,7 @@ LSM303D::LSM303D(int bus, uint32_t device, enum Rotation rotation) :
|
||||
{
|
||||
_px4_accel.set_device_type(DRV_ACC_DEVTYPE_LSM303D);
|
||||
_px4_mag.set_device_type(DRV_MAG_DEVTYPE_LSM303D);
|
||||
_px4_mag.set_external(external());
|
||||
}
|
||||
|
||||
LSM303D::~LSM303D()
|
||||
@@ -914,144 +611,3 @@ LSM303D::print_info()
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Local functions in support of the shell command.
|
||||
*/
|
||||
namespace lsm303d
|
||||
{
|
||||
|
||||
LSM303D *g_dev;
|
||||
|
||||
void start(bool external_bus, enum Rotation rotation, unsigned range);
|
||||
void info();
|
||||
void usage();
|
||||
|
||||
/**
|
||||
* Start the driver.
|
||||
*
|
||||
* This function call only returns once the driver is
|
||||
* up and running or failed to detect the sensor.
|
||||
*/
|
||||
void
|
||||
start(bool external_bus, enum Rotation rotation, unsigned range)
|
||||
{
|
||||
if (g_dev != nullptr) {
|
||||
errx(0, "already started");
|
||||
}
|
||||
|
||||
/* create the driver */
|
||||
if (external_bus) {
|
||||
#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_ACCEL_MAG)
|
||||
g_dev = new LSM303D(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_ACCEL_MAG, rotation);
|
||||
#else
|
||||
errx(0, "External SPI not available");
|
||||
#endif
|
||||
|
||||
} else {
|
||||
g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_ACCEL_MAG, rotation);
|
||||
}
|
||||
|
||||
if (g_dev == nullptr) {
|
||||
PX4_ERR("failed instantiating LSM303D obj");
|
||||
goto fail;
|
||||
}
|
||||
|
||||
if (OK != g_dev->init()) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
exit(0);
|
||||
fail:
|
||||
|
||||
if (g_dev != nullptr) {
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
}
|
||||
|
||||
errx(1, "driver start failed");
|
||||
}
|
||||
|
||||
/**
|
||||
* Print a little info about the driver.
|
||||
*/
|
||||
void
|
||||
info()
|
||||
{
|
||||
if (g_dev == nullptr) {
|
||||
errx(1, "driver not running\n");
|
||||
}
|
||||
|
||||
printf("state @ %p\n", g_dev);
|
||||
g_dev->print_info();
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
void
|
||||
usage()
|
||||
{
|
||||
PX4_INFO("missing command: try 'start', 'info'");
|
||||
PX4_INFO("options:");
|
||||
PX4_INFO(" -X (external bus)");
|
||||
PX4_INFO(" -R rotation");
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int
|
||||
lsm303d_main(int argc, char *argv[])
|
||||
{
|
||||
bool external_bus = false;
|
||||
enum Rotation rotation = ROTATION_NONE;
|
||||
int accel_range = 8;
|
||||
|
||||
int myoptind = 1;
|
||||
int ch;
|
||||
const char *myoptarg = nullptr;
|
||||
|
||||
/* jump over start/off/etc and look at options first */
|
||||
while ((ch = px4_getopt(argc, argv, "XR:a:", &myoptind, &myoptarg)) != EOF) {
|
||||
switch (ch) {
|
||||
case 'X':
|
||||
external_bus = true;
|
||||
break;
|
||||
|
||||
case 'R':
|
||||
rotation = (enum Rotation)atoi(myoptarg);
|
||||
break;
|
||||
|
||||
case 'a':
|
||||
accel_range = atoi(myoptarg);
|
||||
break;
|
||||
|
||||
default:
|
||||
lsm303d::usage();
|
||||
exit(0);
|
||||
}
|
||||
}
|
||||
|
||||
if (myoptind >= argc) {
|
||||
lsm303d::usage();
|
||||
exit(0);
|
||||
}
|
||||
|
||||
const char *verb = argv[myoptind];
|
||||
|
||||
/*
|
||||
* Start/load the driver.
|
||||
|
||||
*/
|
||||
if (!strcmp(verb, "start")) {
|
||||
lsm303d::start(external_bus, rotation, accel_range);
|
||||
}
|
||||
|
||||
/*
|
||||
* Print driver information.
|
||||
*/
|
||||
if (!strcmp(verb, "info")) {
|
||||
lsm303d::info();
|
||||
}
|
||||
|
||||
errx(1, "unrecognized command, try 'start', info'");
|
||||
}
|
||||
@@ -0,0 +1,300 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2019 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 LSM303D.hpp
|
||||
* Driver for the ST LSM303D MEMS accelerometer / magnetometer connected via SPI.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <drivers/device/spi.h>
|
||||
#include <ecl/geo/geo.h>
|
||||
#include <perf/perf_counter.h>
|
||||
#include <px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
|
||||
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
|
||||
|
||||
/* SPI protocol address bits */
|
||||
#define DIR_READ (1<<7)
|
||||
#define DIR_WRITE (0<<7)
|
||||
#define ADDR_INCREMENT (1<<6)
|
||||
|
||||
/* register addresses: A: accel, M: mag, T: temp */
|
||||
#define ADDR_WHO_AM_I 0x0F
|
||||
#define WHO_I_AM 0x49
|
||||
|
||||
#define ADDR_OUT_TEMP_L 0x05
|
||||
|
||||
#define ADDR_STATUS_A 0x27
|
||||
|
||||
#define ADDR_CTRL_REG0 0x1F
|
||||
#define ADDR_CTRL_REG1 0x20
|
||||
#define ADDR_CTRL_REG2 0x21
|
||||
#define ADDR_CTRL_REG3 0x22
|
||||
#define ADDR_CTRL_REG4 0x23
|
||||
#define ADDR_CTRL_REG5 0x24
|
||||
#define ADDR_CTRL_REG6 0x25
|
||||
#define ADDR_CTRL_REG7 0x26
|
||||
|
||||
#define REG1_RATE_BITS_A ((1<<7) | (1<<6) | (1<<5) | (1<<4))
|
||||
#define REG1_POWERDOWN_A ((0<<7) | (0<<6) | (0<<5) | (0<<4))
|
||||
#define REG1_RATE_3_125HZ_A ((0<<7) | (0<<6) | (0<<5) | (1<<4))
|
||||
#define REG1_RATE_6_25HZ_A ((0<<7) | (0<<6) | (1<<5) | (0<<4))
|
||||
#define REG1_RATE_12_5HZ_A ((0<<7) | (0<<6) | (1<<5) | (1<<4))
|
||||
#define REG1_RATE_25HZ_A ((0<<7) | (1<<6) | (0<<5) | (0<<4))
|
||||
#define REG1_RATE_50HZ_A ((0<<7) | (1<<6) | (0<<5) | (1<<4))
|
||||
#define REG1_RATE_100HZ_A ((0<<7) | (1<<6) | (1<<5) | (0<<4))
|
||||
#define REG1_RATE_200HZ_A ((0<<7) | (1<<6) | (1<<5) | (1<<4))
|
||||
#define REG1_RATE_400HZ_A ((1<<7) | (0<<6) | (0<<5) | (0<<4))
|
||||
#define REG1_RATE_800HZ_A ((1<<7) | (0<<6) | (0<<5) | (1<<4))
|
||||
#define REG1_RATE_1600HZ_A ((1<<7) | (0<<6) | (1<<5) | (0<<4))
|
||||
|
||||
#define REG1_BDU_UPDATE (1<<3)
|
||||
#define REG1_Z_ENABLE_A (1<<2)
|
||||
#define REG1_Y_ENABLE_A (1<<1)
|
||||
#define REG1_X_ENABLE_A (1<<0)
|
||||
|
||||
#define REG2_ANTIALIAS_FILTER_BW_BITS_A ((1<<7) | (1<<6))
|
||||
#define REG2_AA_FILTER_BW_773HZ_A ((0<<7) | (0<<6))
|
||||
#define REG2_AA_FILTER_BW_194HZ_A ((0<<7) | (1<<6))
|
||||
#define REG2_AA_FILTER_BW_362HZ_A ((1<<7) | (0<<6))
|
||||
#define REG2_AA_FILTER_BW_50HZ_A ((1<<7) | (1<<6))
|
||||
|
||||
#define REG2_FULL_SCALE_BITS_A ((1<<5) | (1<<4) | (1<<3))
|
||||
#define REG2_FULL_SCALE_2G_A ((0<<5) | (0<<4) | (0<<3))
|
||||
#define REG2_FULL_SCALE_4G_A ((0<<5) | (0<<4) | (1<<3))
|
||||
#define REG2_FULL_SCALE_6G_A ((0<<5) | (1<<4) | (0<<3))
|
||||
#define REG2_FULL_SCALE_8G_A ((0<<5) | (1<<4) | (1<<3))
|
||||
#define REG2_FULL_SCALE_16G_A ((1<<5) | (0<<4) | (0<<3))
|
||||
|
||||
#define REG5_ENABLE_T (1<<7)
|
||||
|
||||
#define REG5_RES_HIGH_M ((1<<6) | (1<<5))
|
||||
#define REG5_RES_LOW_M ((0<<6) | (0<<5))
|
||||
|
||||
#define REG5_RATE_BITS_M ((1<<4) | (1<<3) | (1<<2))
|
||||
#define REG5_RATE_3_125HZ_M ((0<<4) | (0<<3) | (0<<2))
|
||||
#define REG5_RATE_6_25HZ_M ((0<<4) | (0<<3) | (1<<2))
|
||||
#define REG5_RATE_12_5HZ_M ((0<<4) | (1<<3) | (0<<2))
|
||||
#define REG5_RATE_25HZ_M ((0<<4) | (1<<3) | (1<<2))
|
||||
#define REG5_RATE_50HZ_M ((1<<4) | (0<<3) | (0<<2))
|
||||
#define REG5_RATE_100HZ_M ((1<<4) | (0<<3) | (1<<2))
|
||||
#define REG5_RATE_DO_NOT_USE_M ((1<<4) | (1<<3) | (0<<2))
|
||||
|
||||
#define REG6_FULL_SCALE_BITS_M ((1<<6) | (1<<5))
|
||||
#define REG6_FULL_SCALE_2GA_M ((0<<6) | (0<<5))
|
||||
#define REG6_FULL_SCALE_4GA_M ((0<<6) | (1<<5))
|
||||
#define REG6_FULL_SCALE_8GA_M ((1<<6) | (0<<5))
|
||||
#define REG6_FULL_SCALE_12GA_M ((1<<6) | (1<<5))
|
||||
|
||||
#define REG7_CONT_MODE_M ((0<<1) | (0<<0))
|
||||
|
||||
#define REG_STATUS_A_NEW_ZYXADA 0x08
|
||||
|
||||
/* default values for this device */
|
||||
#define LSM303D_ACCEL_DEFAULT_RANGE_G 16
|
||||
#define LSM303D_ACCEL_DEFAULT_RATE 800
|
||||
#define LSM303D_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50
|
||||
|
||||
#define LSM303D_MAG_DEFAULT_RANGE_GA 2
|
||||
#define LSM303D_MAG_DEFAULT_RATE 100
|
||||
|
||||
/*
|
||||
we set the timer interrupt to run a bit faster than the desired
|
||||
sample rate and then throw away duplicates using the data ready bit.
|
||||
This time reduction is enough to cope with worst case timing jitter
|
||||
due to other timers
|
||||
*/
|
||||
#define LSM303D_TIMER_REDUCTION 200
|
||||
|
||||
extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
|
||||
|
||||
class LSM303D : public device::SPI, public px4::ScheduledWorkItem
|
||||
{
|
||||
public:
|
||||
LSM303D(int bus, uint32_t device, enum Rotation rotation);
|
||||
virtual ~LSM303D();
|
||||
|
||||
int init() override;
|
||||
|
||||
/**
|
||||
* Diagnostics - print some basic information about the driver.
|
||||
*/
|
||||
void print_info();
|
||||
|
||||
protected:
|
||||
virtual int probe();
|
||||
|
||||
private:
|
||||
|
||||
void Run() override;
|
||||
|
||||
void start();
|
||||
void stop();
|
||||
void reset();
|
||||
|
||||
/**
|
||||
* disable I2C on the chip
|
||||
*/
|
||||
void disable_i2c();
|
||||
|
||||
/**
|
||||
* check key registers for correct values
|
||||
*/
|
||||
void check_registers(void);
|
||||
|
||||
/**
|
||||
* Fetch accel measurements from the sensor and update the report ring.
|
||||
*/
|
||||
void measureAccelerometer();
|
||||
|
||||
/**
|
||||
* Fetch mag measurements from the sensor and update the report ring.
|
||||
*/
|
||||
void measureMagnetometer();
|
||||
|
||||
/**
|
||||
* Read a register from the LSM303D
|
||||
*
|
||||
* @param The register to read.
|
||||
* @return The value that was read.
|
||||
*/
|
||||
uint8_t read_reg(unsigned reg);
|
||||
|
||||
/**
|
||||
* Write a register in the LSM303D
|
||||
*
|
||||
* @param reg The register to write.
|
||||
* @param value The new value to write.
|
||||
*/
|
||||
void write_reg(unsigned reg, uint8_t value);
|
||||
|
||||
/**
|
||||
* Modify a register in the LSM303D
|
||||
*
|
||||
* Bits are cleared before bits are set.
|
||||
*
|
||||
* @param reg The register to modify.
|
||||
* @param clearbits Bits in the register to clear.
|
||||
* @param setbits Bits in the register to set.
|
||||
*/
|
||||
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
|
||||
|
||||
/**
|
||||
* Write a register in the LSM303D, updating _checked_values
|
||||
*
|
||||
* @param reg The register to write.
|
||||
* @param value The new value to write.
|
||||
*/
|
||||
void write_checked_reg(unsigned reg, uint8_t value);
|
||||
|
||||
/**
|
||||
* Set the LSM303D accel measurement range.
|
||||
*
|
||||
* @param max_g The measurement range of the accel is in g (9.81m/s^2)
|
||||
* Zero selects the maximum supported range.
|
||||
* @return OK if the value can be supported, -ERANGE otherwise.
|
||||
*/
|
||||
int accel_set_range(unsigned max_g);
|
||||
|
||||
/**
|
||||
* Set the LSM303D mag measurement range.
|
||||
*
|
||||
* @param max_ga The measurement range of the mag is in Ga
|
||||
* Zero selects the maximum supported range.
|
||||
* @return OK if the value can be supported, -ERANGE otherwise.
|
||||
*/
|
||||
int mag_set_range(unsigned max_g);
|
||||
|
||||
/**
|
||||
* Set the LSM303D on-chip anti-alias filter bandwith.
|
||||
*
|
||||
* @param bandwidth The anti-alias filter bandwidth in Hz
|
||||
* Zero selects the highest bandwidth
|
||||
* @return OK if the value can be supported, -ERANGE otherwise.
|
||||
*/
|
||||
int accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth);
|
||||
|
||||
/**
|
||||
* Set the LSM303D internal accel sampling frequency.
|
||||
*
|
||||
* @param frequency The internal accel sampling frequency is set to not less than
|
||||
* this value.
|
||||
* Zero selects the maximum rate supported.
|
||||
* @return OK if the value can be supported.
|
||||
*/
|
||||
int accel_set_samplerate(unsigned frequency);
|
||||
|
||||
/**
|
||||
* Set the LSM303D internal mag sampling frequency.
|
||||
*
|
||||
* @param frequency The internal mag sampling frequency is set to not less than
|
||||
* this value.
|
||||
* Zero selects the maximum rate supported.
|
||||
* @return OK if the value can be supported.
|
||||
*/
|
||||
int mag_set_samplerate(unsigned frequency);
|
||||
|
||||
|
||||
PX4Accelerometer _px4_accel;
|
||||
PX4Magnetometer _px4_mag;
|
||||
|
||||
unsigned _call_accel_interval{1000000 / LSM303D_ACCEL_DEFAULT_RATE};
|
||||
unsigned _call_mag_interval{1000000 / LSM303D_MAG_DEFAULT_RATE};
|
||||
|
||||
perf_counter_t _accel_sample_perf;
|
||||
perf_counter_t _accel_sample_interval_perf;
|
||||
perf_counter_t _mag_sample_perf;
|
||||
perf_counter_t _mag_sample_interval_perf;
|
||||
perf_counter_t _bad_registers;
|
||||
perf_counter_t _bad_values;
|
||||
perf_counter_t _accel_duplicates;
|
||||
|
||||
uint8_t _register_wait{0};
|
||||
|
||||
int16_t _last_accel[3] {};
|
||||
uint8_t _constant_accel_count{0};
|
||||
|
||||
hrt_abstime _mag_last_measure{0};
|
||||
|
||||
float _last_temperature{0.0f};
|
||||
|
||||
// this is used to support runtime checking of key
|
||||
// configuration registers to detect SPI bus errors and sensor
|
||||
// reset
|
||||
static constexpr int LSM303D_NUM_CHECKED_REGISTERS{8};
|
||||
uint8_t _checked_values[LSM303D_NUM_CHECKED_REGISTERS] {};
|
||||
uint8_t _checked_next{0};
|
||||
|
||||
};
|
||||
@@ -0,0 +1,182 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2019 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 lsm303d_main.cpp
|
||||
* Driver for the ST LSM303D MEMS accelerometer / magnetometer connected via SPI.
|
||||
*/
|
||||
|
||||
#include "LSM303D.hpp"
|
||||
|
||||
#include <px4_getopt.h>
|
||||
|
||||
/**
|
||||
* Local functions in support of the shell command.
|
||||
*/
|
||||
namespace lsm303d
|
||||
{
|
||||
|
||||
LSM303D *g_dev;
|
||||
|
||||
void start(bool external_bus, enum Rotation rotation, unsigned range);
|
||||
void info();
|
||||
void usage();
|
||||
|
||||
/**
|
||||
* Start the driver.
|
||||
*
|
||||
* This function call only returns once the driver is
|
||||
* up and running or failed to detect the sensor.
|
||||
*/
|
||||
void
|
||||
start(bool external_bus, enum Rotation rotation, unsigned range)
|
||||
{
|
||||
if (g_dev != nullptr) {
|
||||
errx(0, "already started");
|
||||
}
|
||||
|
||||
/* create the driver */
|
||||
if (external_bus) {
|
||||
#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_ACCEL_MAG)
|
||||
g_dev = new LSM303D(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_ACCEL_MAG, rotation);
|
||||
#else
|
||||
errx(0, "External SPI not available");
|
||||
#endif
|
||||
|
||||
} else {
|
||||
g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_ACCEL_MAG, rotation);
|
||||
}
|
||||
|
||||
if (g_dev == nullptr) {
|
||||
PX4_ERR("failed instantiating LSM303D obj");
|
||||
goto fail;
|
||||
}
|
||||
|
||||
if (OK != g_dev->init()) {
|
||||
goto fail;
|
||||
}
|
||||
|
||||
exit(0);
|
||||
fail:
|
||||
|
||||
if (g_dev != nullptr) {
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
}
|
||||
|
||||
errx(1, "driver start failed");
|
||||
}
|
||||
|
||||
/**
|
||||
* Print a little info about the driver.
|
||||
*/
|
||||
void
|
||||
info()
|
||||
{
|
||||
if (g_dev == nullptr) {
|
||||
errx(1, "driver not running\n");
|
||||
}
|
||||
|
||||
printf("state @ %p\n", g_dev);
|
||||
g_dev->print_info();
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
void
|
||||
usage()
|
||||
{
|
||||
PX4_INFO("missing command: try 'start', 'info'");
|
||||
PX4_INFO("options:");
|
||||
PX4_INFO(" -X (external bus)");
|
||||
PX4_INFO(" -R rotation");
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int
|
||||
lsm303d_main(int argc, char *argv[])
|
||||
{
|
||||
bool external_bus = false;
|
||||
enum Rotation rotation = ROTATION_NONE;
|
||||
int accel_range = 8;
|
||||
|
||||
int myoptind = 1;
|
||||
int ch;
|
||||
const char *myoptarg = nullptr;
|
||||
|
||||
/* jump over start/off/etc and look at options first */
|
||||
while ((ch = px4_getopt(argc, argv, "XR:a:", &myoptind, &myoptarg)) != EOF) {
|
||||
switch (ch) {
|
||||
case 'X':
|
||||
external_bus = true;
|
||||
break;
|
||||
|
||||
case 'R':
|
||||
rotation = (enum Rotation)atoi(myoptarg);
|
||||
break;
|
||||
|
||||
case 'a':
|
||||
accel_range = atoi(myoptarg);
|
||||
break;
|
||||
|
||||
default:
|
||||
lsm303d::usage();
|
||||
exit(0);
|
||||
}
|
||||
}
|
||||
|
||||
if (myoptind >= argc) {
|
||||
lsm303d::usage();
|
||||
exit(0);
|
||||
}
|
||||
|
||||
const char *verb = argv[myoptind];
|
||||
|
||||
/*
|
||||
* Start/load the driver.
|
||||
|
||||
*/
|
||||
if (!strcmp(verb, "start")) {
|
||||
lsm303d::start(external_bus, rotation, accel_range);
|
||||
}
|
||||
|
||||
/*
|
||||
* Print driver information.
|
||||
*/
|
||||
if (!strcmp(verb, "info")) {
|
||||
lsm303d::info();
|
||||
}
|
||||
|
||||
errx(1, "unrecognized command, try 'start', info'");
|
||||
}
|
||||
Reference in New Issue
Block a user