mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-11 02:21:35 +08:00
287 lines
8.6 KiB
C++
287 lines
8.6 KiB
C++
/****************************************************************************
|
|
*
|
|
* Copyright (c) 2012-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 l3gd20.cpp
|
|
* Driver for the ST L3GD20 MEMS and L3GD20H mems gyros connected via SPI.
|
|
*
|
|
* Note: With the exception of the self-test feature, the ST L3G4200D is
|
|
* also supported by this driver.
|
|
*/
|
|
|
|
#include <drivers/device/spi.h>
|
|
#include <lib/conversion/rotation.h>
|
|
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
|
|
#include <perf/perf_counter.h>
|
|
#include <px4_platform_common/getopt.h>
|
|
#include <px4_platform_common/i2c_spi_buses.h>
|
|
|
|
/* Orientation on board */
|
|
#define SENSOR_BOARD_ROTATION_000_DEG 0
|
|
#define SENSOR_BOARD_ROTATION_090_DEG 1
|
|
#define SENSOR_BOARD_ROTATION_180_DEG 2
|
|
#define SENSOR_BOARD_ROTATION_270_DEG 3
|
|
|
|
/* SPI protocol address bits */
|
|
#define DIR_READ (1<<7)
|
|
#define DIR_WRITE (0<<7)
|
|
#define ADDR_INCREMENT (1<<6)
|
|
|
|
/* register addresses */
|
|
#define ADDR_WHO_AM_I 0x0F
|
|
#define WHO_I_AM_H 0xD7
|
|
#define WHO_I_AM 0xD4
|
|
#define WHO_I_AM_L3G4200D 0xD3 /* for L3G4200D */
|
|
|
|
#define ADDR_CTRL_REG1 0x20
|
|
#define REG1_RATE_LP_MASK 0xF0 /* Mask to guard partial register update */
|
|
|
|
/* keep lowpass low to avoid noise issues */
|
|
#define RATE_95HZ_LP_25HZ ((0<<7) | (0<<6) | (0<<5) | (1<<4))
|
|
#define RATE_190HZ_LP_25HZ ((0<<7) | (1<<6) | (0<<5) | (1<<4))
|
|
#define RATE_190HZ_LP_50HZ ((0<<7) | (1<<6) | (1<<5) | (0<<4))
|
|
#define RATE_190HZ_LP_70HZ ((0<<7) | (1<<6) | (1<<5) | (1<<4))
|
|
#define RATE_380HZ_LP_20HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4))
|
|
#define RATE_380HZ_LP_25HZ ((1<<7) | (0<<6) | (0<<5) | (1<<4))
|
|
#define RATE_380HZ_LP_50HZ ((1<<7) | (0<<6) | (1<<5) | (0<<4))
|
|
#define RATE_380HZ_LP_100HZ ((1<<7) | (0<<6) | (1<<5) | (1<<4))
|
|
#define RATE_760HZ_LP_30HZ ((1<<7) | (1<<6) | (0<<5) | (0<<4))
|
|
#define RATE_760HZ_LP_35HZ ((1<<7) | (1<<6) | (0<<5) | (1<<4))
|
|
#define RATE_760HZ_LP_50HZ ((1<<7) | (1<<6) | (1<<5) | (0<<4))
|
|
#define RATE_760HZ_LP_100HZ ((1<<7) | (1<<6) | (1<<5) | (1<<4))
|
|
|
|
#define ADDR_CTRL_REG2 0x21
|
|
#define ADDR_CTRL_REG3 0x22
|
|
#define ADDR_CTRL_REG4 0x23
|
|
#define REG4_RANGE_MASK 0x30 /* Mask to guard partial register update */
|
|
#define RANGE_250DPS (0<<4)
|
|
#define RANGE_500DPS (1<<4)
|
|
#define RANGE_2000DPS (3<<4)
|
|
|
|
#define ADDR_CTRL_REG5 0x24
|
|
#define ADDR_REFERENCE 0x25
|
|
#define ADDR_OUT_TEMP 0x26
|
|
#define ADDR_STATUS_REG 0x27
|
|
#define ADDR_OUT_X_L 0x28
|
|
#define ADDR_OUT_X_H 0x29
|
|
#define ADDR_OUT_Y_L 0x2A
|
|
#define ADDR_OUT_Y_H 0x2B
|
|
#define ADDR_OUT_Z_L 0x2C
|
|
#define ADDR_OUT_Z_H 0x2D
|
|
#define ADDR_FIFO_CTRL_REG 0x2E
|
|
#define ADDR_FIFO_SRC_REG 0x2F
|
|
#define ADDR_INT1_CFG 0x30
|
|
#define ADDR_INT1_SRC 0x31
|
|
#define ADDR_INT1_TSH_XH 0x32
|
|
#define ADDR_INT1_TSH_XL 0x33
|
|
#define ADDR_INT1_TSH_YH 0x34
|
|
#define ADDR_INT1_TSH_YL 0x35
|
|
#define ADDR_INT1_TSH_ZH 0x36
|
|
#define ADDR_INT1_TSH_ZL 0x37
|
|
#define ADDR_INT1_DURATION 0x38
|
|
#define ADDR_LOW_ODR 0x39
|
|
|
|
|
|
/* Internal configuration values */
|
|
#define REG1_POWER_NORMAL (1<<3)
|
|
#define REG1_Z_ENABLE (1<<2)
|
|
#define REG1_Y_ENABLE (1<<1)
|
|
#define REG1_X_ENABLE (1<<0)
|
|
|
|
#define REG4_BDU (1<<7)
|
|
#define REG4_BLE (1<<6)
|
|
//#define REG4_SPI_3WIRE (1<<0)
|
|
|
|
#define REG5_FIFO_ENABLE (1<<6)
|
|
#define REG5_REBOOT_MEMORY (1<<7)
|
|
|
|
#define STATUS_ZYXOR (1<<7)
|
|
#define STATUS_ZOR (1<<6)
|
|
#define STATUS_YOR (1<<5)
|
|
#define STATUS_XOR (1<<4)
|
|
#define STATUS_ZYXDA (1<<3)
|
|
#define STATUS_ZDA (1<<2)
|
|
#define STATUS_YDA (1<<1)
|
|
#define STATUS_XDA (1<<0)
|
|
|
|
#define FIFO_CTRL_BYPASS_MODE (0<<5)
|
|
#define FIFO_CTRL_FIFO_MODE (1<<5)
|
|
#define FIFO_CTRL_STREAM_MODE (1<<6)
|
|
#define FIFO_CTRL_STREAM_TO_FIFO_MODE (3<<5)
|
|
#define FIFO_CTRL_BYPASS_TO_STREAM_MODE (1<<7)
|
|
|
|
#define L3GD20_DEFAULT_RATE 760
|
|
#define L3GD20_DEFAULT_RANGE_DPS 2000
|
|
#define L3GD20_TEMP_OFFSET_CELSIUS 40
|
|
|
|
#define L3GD20_MAX_OFFSET 0.45f /**< max offset: 25 degrees/s */
|
|
|
|
#ifndef SENSOR_BOARD_ROTATION_DEFAULT
|
|
#define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG
|
|
#endif
|
|
|
|
/*
|
|
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 L3GD20_TIMER_REDUCTION 600
|
|
|
|
class L3GD20 : public device::SPI, public I2CSPIDriver<L3GD20>
|
|
{
|
|
public:
|
|
L3GD20(const I2CSPIDriverConfig &config);
|
|
~L3GD20() override;
|
|
|
|
static void print_usage();
|
|
|
|
void RunImpl();
|
|
|
|
int init() override;
|
|
|
|
void print_status() override;
|
|
|
|
// print register dump
|
|
void print_registers();
|
|
|
|
// trigger an error
|
|
void test_error();
|
|
|
|
protected:
|
|
void custom_method(const BusCLIArguments &cli) override;
|
|
int probe() override;
|
|
|
|
private:
|
|
|
|
PX4Gyroscope _px4_gyro;
|
|
|
|
unsigned _orientation{SENSOR_BOARD_ROTATION_DEFAULT};
|
|
|
|
unsigned _read{0};
|
|
|
|
perf_counter_t _sample_perf;
|
|
perf_counter_t _errors;
|
|
perf_counter_t _bad_registers;
|
|
perf_counter_t _duplicates;
|
|
|
|
uint8_t _register_wait{0};
|
|
|
|
/* true if an L3G4200D is detected */
|
|
bool _is_l3g4200d{false};
|
|
|
|
// this is used to support runtime checking of key
|
|
// configuration registers to detect SPI bus errors and sensor
|
|
// reset
|
|
static constexpr int L3GD20_NUM_CHECKED_REGISTERS{8};
|
|
static constexpr uint8_t _checked_registers[] = {
|
|
ADDR_WHO_AM_I,
|
|
ADDR_CTRL_REG1,
|
|
ADDR_CTRL_REG2,
|
|
ADDR_CTRL_REG3,
|
|
ADDR_CTRL_REG4,
|
|
ADDR_CTRL_REG5,
|
|
ADDR_FIFO_CTRL_REG,
|
|
ADDR_LOW_ODR
|
|
};
|
|
|
|
uint8_t _checked_values[L3GD20_NUM_CHECKED_REGISTERS] {};
|
|
uint8_t _checked_next{0};
|
|
|
|
void start();
|
|
void reset();
|
|
void disable_i2c();
|
|
|
|
/**
|
|
* check key registers for correct values
|
|
*/
|
|
void check_registers();
|
|
|
|
/**
|
|
* Read a register from the L3GD20
|
|
*
|
|
* @param The register to read.
|
|
* @return The value that was read.
|
|
*/
|
|
uint8_t read_reg(unsigned reg) override;
|
|
|
|
/**
|
|
* Write a register in the L3GD20
|
|
*
|
|
* @param reg The register to write.
|
|
* @param value The new value to write.
|
|
* @return OK on success, negative errno otherwise.
|
|
*/
|
|
int write_reg(unsigned reg, uint8_t value) override;
|
|
|
|
/**
|
|
* Modify a register in the L3GD20
|
|
*
|
|
* 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 L3GD20, 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 L3GD20 measurement range.
|
|
*
|
|
* @param max_dps The measurement range is set to permit reading at least
|
|
* this rate in degrees per second.
|
|
* Zero selects the maximum supported range.
|
|
* @return OK if the value can be supported, -ERANGE otherwise.
|
|
*/
|
|
int set_range(unsigned max_dps);
|
|
|
|
/**
|
|
* Set the L3GD20 internal sampling frequency.
|
|
*
|
|
* @param frequency The internal 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 set_samplerate(unsigned frequency);
|
|
|
|
};
|