fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

fxos8701cq split out header and main

Browse Source
This commit is contained in:
Daniel Agar 2019-08-28 22:16:02 -04:00
parent f0ee0b5d49
commit ee9f65b38b
4 changed files with 852 additions and 374 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/drivers/imu/fxos8701cq/CMakeLists.txt
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2017 PX4 Development Team. All rights reserved.
# Copyright (c) 2017-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
@ -31,10 +31,12 @@
#
############################################################################
px4_add_module(
MODULE drivers__fxos8701cq
MODULE drivers__imu__fxos8701cq
MAIN fxos8701cq
COMPILE_FLAGS
-Wno-cast-align # TODO: fix and enable
SRCS
fxos8701cq.cpp
FXOS8701CQ.cpp
FXOS8701CQ.hpp
fxos8701cq_main.cpp
)

373
src/drivers/imu/fxos8701cq/fxos8701cq.cpp → src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2017 PX4 Development Team. All rights reserved.
* Copyright (c) 2017-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
@ -37,341 +37,7 @@
* magnetometer connected via SPI.
*/
#include <drivers/device/integrator.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/device/spi.h>
#include <drivers/drv_accel.h>
#include <ecl/geo/geo.h>
#include <lib/conversion/rotation.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <perf/perf_counter.h>
#include <px4_getopt.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <systemlib/err.h>
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
# include <drivers/drv_mag.h>
#endif
/* SPI protocol address bits */
#define DIR_READ(a) ((a) & 0x7f)
#define DIR_WRITE(a) ((a) | (1 << 7))
#define ADDR_7(a) ((a) & (1 << 7))
#define swap16(w) __builtin_bswap16((w))
#define swap16RightJustify14(w) (((int16_t)swap16(w)) >> 2)
#define FXOS8701C_DEVICE_PATH_ACCEL "/dev/fxos8701cq_accel"
#define FXOS8701C_DEVICE_PATH_ACCEL_EXT "/dev/fxos8701cq_accel_ext"
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
# define FXOS8701C_DEVICE_PATH_MAG "/dev/fxos8701cq_mag"
#endif
#define FXOS8701CQ_DR_STATUS 0x00
# define DR_STATUS_ZYXDR (1 << 3)
#define FXOS8701CQ_OUT_X_MSB 0x01
#define FXOS8701CQ_XYZ_DATA_CFG 0x0e
# define XYZ_DATA_CFG_FS_SHIFTS 0
# define XYZ_DATA_CFG_FS_MASK (3 << XYZ_DATA_CFG_FS_SHIFTS)
# define XYZ_DATA_CFG_FS_2G (0 << XYZ_DATA_CFG_FS_SHIFTS)
# define XYZ_DATA_CFG_FS_4G (1 << XYZ_DATA_CFG_FS_SHIFTS)
# define XYZ_DATA_CFG_FS_8G (2 << XYZ_DATA_CFG_FS_SHIFTS)
#define FXOS8701CQ_WHOAMI 0x0d
# define FXOS8700CQ_WHOAMI_VAL 0xC7
# define FXOS8701CQ_WHOAMI_VAL 0xCA
#define FXOS8701CQ_CTRL_REG1 0x2a
# define CTRL_REG1_ACTIVE (1 << 0)
# define CTRL_REG1_DR_SHIFTS 3
# define CTRL_REG1_DR_MASK (7 << CTRL_REG1_DR_SHIFTS)
# define CTRL_REG1_DR(n) (((n) & 7) << CTRL_REG1_DR_SHIFTS)
#define FXOS8701CQ_CTRL_REG2 0x2b
# define CTRL_REG2_AUTO_INC (1 << 5)
#define FXOS8701CQ_M_DR_STATUS 0x32
# define M_DR_STATUS_ZYXDR (1 << 3)
#define FXOS8701CQ_M_OUT_X_MSB 0x33
#define FXOS8701CQ_TEMP 0x51
#define FXOS8701CQ_M_CTRL_REG1 0x5b
# define M_CTRL_REG1_HMS_SHIFTS 0
# define M_CTRL_REG1_HMS_MASK (3 << M_CTRL_REG1_HMS_SHIFTS)
# define M_CTRL_REG1_HMS_A (0 << M_CTRL_REG1_HMS_SHIFTS)
# define M_CTRL_REG1_HMS_M (1 << M_CTRL_REG1_HMS_SHIFTS)
# define M_CTRL_REG1_HMS_AM (3 << M_CTRL_REG1_HMS_SHIFTS)
# define M_CTRL_REG1_OS_SHIFTS 2
# define M_CTRL_REG1_OS_MASK (7 << M_CTRL_REG1_HMS_SHIFTS)
# define M_CTRL_REG1_OS(n) (((n) & 7) << M_CTRL_REG1_OS_SHIFTS)
#define FXOS8701CQ_M_CTRL_REG2 0x5c
#define FXOS8701CQ_M_CTRL_REG3 0x5d
#define DEF_REG(r) {r, #r}
/* default values for this device */
#define FXOS8701C_ACCEL_DEFAULT_RANGE_G 8
#define FXOS8701C_ACCEL_DEFAULT_RATE 400 /* ODR is 400 in Hybird mode (accel + mag) */
#define FXOS8701C_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50
#define FXOS8701C_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
#define FXOS8701C_ACCEL_MAX_OUTPUT_RATE 280
#define FXOS8701C_MAG_DEFAULT_RANGE_GA 12 /* It is fixed at 12 G */
#define FXOS8701C_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 FXOS8701C_TIMER_REDUCTION 240
extern "C" { __EXPORT int fxos8701cq_main(int argc, char *argv[]); }
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
class FXOS8701CQ_mag;
#endif
class FXOS8701CQ : public device::SPI, public px4::ScheduledWorkItem
{
public:
FXOS8701CQ(int bus, const char *path, uint32_t device, enum Rotation rotation);
virtual ~FXOS8701CQ();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
/**
* dump register values
*/
void print_registers();
/**
* deliberately trigger an error
*/
void test_error();
protected:
virtual int probe();
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
friend class FXOS8701CQ_mag;
virtual ssize_t mag_read(struct file *filp, char *buffer, size_t buflen);
virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg);
#endif
private:
void Run() override;
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
FXOS8701CQ_mag *_mag;
unsigned _call_mag_interval;
ringbuffer::RingBuffer *_mag_reports;
struct mag_calibration_s _mag_scale;
unsigned _mag_range_ga;
float _mag_range_scale;
unsigned _mag_samplerate;
unsigned _mag_read;
perf_counter_t _mag_sample_perf;
int16_t _last_raw_mag_x;
int16_t _last_raw_mag_y;
int16_t _last_raw_mag_z;
hrt_abstime _mag_last_measure{0};
#endif
unsigned _call_accel_interval;
ringbuffer::RingBuffer *_accel_reports;
struct accel_calibration_s _accel_scale;
unsigned _accel_range_m_s2;
float _accel_range_scale;
unsigned _accel_samplerate;
unsigned _accel_onchip_filter_bandwith;
orb_advert_t _accel_topic;
int _accel_orb_class_instance;
int _accel_class_instance;
unsigned _accel_read;
perf_counter_t _accel_sample_perf;
perf_counter_t _bad_registers;
perf_counter_t _bad_values;
perf_counter_t _accel_duplicates;
uint8_t _register_wait;
math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y;
math::LowPassFilter2p _accel_filter_z;
Integrator _accel_int;
enum Rotation _rotation;
// values used to
float _last_accel[3];
uint8_t _constant_accel_count;
// last temperature value
float _last_temperature;
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
// reset
#define FXOS8701C_NUM_CHECKED_REGISTERS 5
static const uint8_t _checked_registers[FXOS8701C_NUM_CHECKED_REGISTERS];
uint8_t _checked_values[FXOS8701C_NUM_CHECKED_REGISTERS];
uint8_t _checked_next;
/**
* Start automatic measurement.
*/
void start();
/**
* Stop automatic measurement.
*/
void stop();
/**
* Reset chip.
*
* Resets the chip and measurements ranges, but not scale and offset.
*/
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 measure();
/**
* Fetch mag measurements from the sensor and update the report ring.
*/
void mag_measure();
/**
* Read a register from the FXOS8701C
*
* @param The register to read.
* @return The value that was read.
*/
uint8_t read_reg(unsigned reg);
/**
* Write a register in the FXOS8701C
*
* @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 FXOS8701C
*
* 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 FXOS8701C, 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 FXOS8701C 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 FXOS8701C 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 FXOS8701C 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 driver lowpass filter bandwidth.
*
* @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_driver_lowpass_filter(float samplerate, float bandwidth);
/**
* Set the FXOS8701C internal accel and mag sampling frequency.
*
* @param frequency The internal accel and 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 accel_set_samplerate(unsigned frequency);
/**
* Set the FXOS8701CQ internal mag sampling frequency.
*
* @param frequency The mag reporting frequency is set to not less than
* this value. (sampling is all way the same as accel
* Zero selects the maximum rate supported.
* @return OK if the value can be supported.
*/
int mag_set_samplerate(unsigned frequency);
/* this class cannot be copied */
FXOS8701CQ(const FXOS8701CQ &);
FXOS8701CQ operator=(const FXOS8701CQ &);
};
#include "FXOS8701CQ.hpp"
/*
list of registers that will be checked in check_registers(). Note
@ -385,40 +51,6 @@ const uint8_t FXOS8701CQ::_checked_registers[FXOS8701C_NUM_CHECKED_REGISTERS] =
FXOS8701CQ_M_CTRL_REG2,
};
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
/**
* Helper class implementing the mag driver node.
*/
class FXOS8701CQ_mag : public device::CDev
{
public:
FXOS8701CQ_mag(FXOS8701CQ *parent);
~FXOS8701CQ_mag();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int init();
protected:
friend class FXOS8701CQ;
void parent_poll_notify();
private:
FXOS8701CQ *_parent;
orb_advert_t _mag_topic;
int _mag_orb_class_instance;
int _mag_class_instance;
void measure();
/* this class does not allow copying due to ptr data members */
FXOS8701CQ_mag(const FXOS8701CQ_mag &);
FXOS8701CQ_mag operator=(const FXOS8701CQ_mag &);
};
#endif
FXOS8701CQ::FXOS8701CQ(int bus, const char *path, uint32_t device, enum Rotation rotation) :
SPI("FXOS8701CQ", path, bus, device, SPIDEV_MODE0,
1 * 1000 * 1000),
@ -619,7 +251,6 @@ FXOS8701CQ::reset()
{
/* enable accel set it To Standby */
write_checked_reg(FXOS8701CQ_CTRL_REG1, 0);
write_checked_reg(FXOS8701CQ_XYZ_DATA_CFG, 0);

409
src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp Normal file
View File

@ -0,0 +1,409 @@
/****************************************************************************
*
* Copyright (c) 2017-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 fxos8701cq.cpp
* Driver for the NXP FXOS8701CQ 6-axis sensor with integrated linear accelerometer and
* magnetometer connected via SPI.
*/
#include <drivers/device/integrator.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/device/spi.h>
#include <drivers/drv_accel.h>
#include <ecl/geo/geo.h>
#include <lib/conversion/rotation.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <perf/perf_counter.h>
#include <px4_getopt.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <systemlib/err.h>
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
# include <drivers/drv_mag.h>
#endif
/* SPI protocol address bits */
#define DIR_READ(a) ((a) & 0x7f)
#define DIR_WRITE(a) ((a) | (1 << 7))
#define ADDR_7(a) ((a) & (1 << 7))
#define swap16(w) __builtin_bswap16((w))
#define swap16RightJustify14(w) (((int16_t)swap16(w)) >> 2)
#define FXOS8701C_DEVICE_PATH_ACCEL "/dev/fxos8701cq_accel"
#define FXOS8701C_DEVICE_PATH_ACCEL_EXT "/dev/fxos8701cq_accel_ext"
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
# define FXOS8701C_DEVICE_PATH_MAG "/dev/fxos8701cq_mag"
#endif
#define FXOS8701CQ_DR_STATUS 0x00
# define DR_STATUS_ZYXDR (1 << 3)
#define FXOS8701CQ_OUT_X_MSB 0x01
#define FXOS8701CQ_XYZ_DATA_CFG 0x0e
# define XYZ_DATA_CFG_FS_SHIFTS 0
# define XYZ_DATA_CFG_FS_MASK (3 << XYZ_DATA_CFG_FS_SHIFTS)
# define XYZ_DATA_CFG_FS_2G (0 << XYZ_DATA_CFG_FS_SHIFTS)
# define XYZ_DATA_CFG_FS_4G (1 << XYZ_DATA_CFG_FS_SHIFTS)
# define XYZ_DATA_CFG_FS_8G (2 << XYZ_DATA_CFG_FS_SHIFTS)
#define FXOS8701CQ_WHOAMI 0x0d
# define FXOS8700CQ_WHOAMI_VAL 0xC7
# define FXOS8701CQ_WHOAMI_VAL 0xCA
#define FXOS8701CQ_CTRL_REG1 0x2a
# define CTRL_REG1_ACTIVE (1 << 0)
# define CTRL_REG1_DR_SHIFTS 3
# define CTRL_REG1_DR_MASK (7 << CTRL_REG1_DR_SHIFTS)
# define CTRL_REG1_DR(n) (((n) & 7) << CTRL_REG1_DR_SHIFTS)
#define FXOS8701CQ_CTRL_REG2 0x2b
# define CTRL_REG2_AUTO_INC (1 << 5)
#define FXOS8701CQ_M_DR_STATUS 0x32
# define M_DR_STATUS_ZYXDR (1 << 3)
#define FXOS8701CQ_M_OUT_X_MSB 0x33
#define FXOS8701CQ_TEMP 0x51
#define FXOS8701CQ_M_CTRL_REG1 0x5b
# define M_CTRL_REG1_HMS_SHIFTS 0
# define M_CTRL_REG1_HMS_MASK (3 << M_CTRL_REG1_HMS_SHIFTS)
# define M_CTRL_REG1_HMS_A (0 << M_CTRL_REG1_HMS_SHIFTS)
# define M_CTRL_REG1_HMS_M (1 << M_CTRL_REG1_HMS_SHIFTS)
# define M_CTRL_REG1_HMS_AM (3 << M_CTRL_REG1_HMS_SHIFTS)
# define M_CTRL_REG1_OS_SHIFTS 2
# define M_CTRL_REG1_OS_MASK (7 << M_CTRL_REG1_HMS_SHIFTS)
# define M_CTRL_REG1_OS(n) (((n) & 7) << M_CTRL_REG1_OS_SHIFTS)
#define FXOS8701CQ_M_CTRL_REG2 0x5c
#define FXOS8701CQ_M_CTRL_REG3 0x5d
#define DEF_REG(r) {r, #r}
/* default values for this device */
#define FXOS8701C_ACCEL_DEFAULT_RANGE_G 8
#define FXOS8701C_ACCEL_DEFAULT_RATE 400 /* ODR is 400 in Hybird mode (accel + mag) */
#define FXOS8701C_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50
#define FXOS8701C_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
#define FXOS8701C_ACCEL_MAX_OUTPUT_RATE 280
#define FXOS8701C_MAG_DEFAULT_RANGE_GA 12 /* It is fixed at 12 G */
#define FXOS8701C_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 FXOS8701C_TIMER_REDUCTION 240
extern "C" { __EXPORT int fxos8701cq_main(int argc, char *argv[]); }
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
class FXOS8701CQ_mag;
#endif
class FXOS8701CQ : public device::SPI, public px4::ScheduledWorkItem
{
public:
FXOS8701CQ(int bus, const char *path, uint32_t device, enum Rotation rotation);
virtual ~FXOS8701CQ();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
/**
* dump register values
*/
void print_registers();
/**
* deliberately trigger an error
*/
void test_error();
protected:
virtual int probe();
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
friend class FXOS8701CQ_mag;
virtual ssize_t mag_read(struct file *filp, char *buffer, size_t buflen);
virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg);
#endif
private:
void Run() override;
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
FXOS8701CQ_mag *_mag;
unsigned _call_mag_interval;
ringbuffer::RingBuffer *_mag_reports;
struct mag_calibration_s _mag_scale;
unsigned _mag_range_ga;
float _mag_range_scale;
unsigned _mag_samplerate;
unsigned _mag_read;
perf_counter_t _mag_sample_perf;
int16_t _last_raw_mag_x;
int16_t _last_raw_mag_y;
int16_t _last_raw_mag_z;
hrt_abstime _mag_last_measure{0};
#endif
unsigned _call_accel_interval;
ringbuffer::RingBuffer *_accel_reports;
struct accel_calibration_s _accel_scale;
unsigned _accel_range_m_s2;
float _accel_range_scale;
unsigned _accel_samplerate;
unsigned _accel_onchip_filter_bandwith;
orb_advert_t _accel_topic;
int _accel_orb_class_instance;
int _accel_class_instance;
unsigned _accel_read;
perf_counter_t _accel_sample_perf;
perf_counter_t _bad_registers;
perf_counter_t _bad_values;
perf_counter_t _accel_duplicates;
uint8_t _register_wait;
math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y;
math::LowPassFilter2p _accel_filter_z;
Integrator _accel_int;
enum Rotation _rotation;
// values used to
float _last_accel[3];
uint8_t _constant_accel_count;
// last temperature value
float _last_temperature;
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
// reset
#define FXOS8701C_NUM_CHECKED_REGISTERS 5
static const uint8_t _checked_registers[FXOS8701C_NUM_CHECKED_REGISTERS];
uint8_t _checked_values[FXOS8701C_NUM_CHECKED_REGISTERS];
uint8_t _checked_next;
/**
* Start automatic measurement.
*/
void start();
/**
* Stop automatic measurement.
*/
void stop();
/**
* Reset chip.
*
* Resets the chip and measurements ranges, but not scale and offset.
*/
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 measure();
/**
* Fetch mag measurements from the sensor and update the report ring.
*/
void mag_measure();
/**
* Read a register from the FXOS8701C
*
* @param The register to read.
* @return The value that was read.
*/
uint8_t read_reg(unsigned reg);
/**
* Write a register in the FXOS8701C
*
* @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 FXOS8701C
*
* 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 FXOS8701C, 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 FXOS8701C 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 FXOS8701C 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 FXOS8701C 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 driver lowpass filter bandwidth.
*
* @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_driver_lowpass_filter(float samplerate, float bandwidth);
/**
* Set the FXOS8701C internal accel and mag sampling frequency.
*
* @param frequency The internal accel and 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 accel_set_samplerate(unsigned frequency);
/**
* Set the FXOS8701CQ internal mag sampling frequency.
*
* @param frequency The mag reporting frequency is set to not less than
* this value. (sampling is all way the same as accel
* Zero selects the maximum rate supported.
* @return OK if the value can be supported.
*/
int mag_set_samplerate(unsigned frequency);
/* this class cannot be copied */
FXOS8701CQ(const FXOS8701CQ &);
FXOS8701CQ operator=(const FXOS8701CQ &);
};
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
/**
* Helper class implementing the mag driver node.
*/
class FXOS8701CQ_mag : public device::CDev
{
public:
FXOS8701CQ_mag(FXOS8701CQ *parent);
~FXOS8701CQ_mag();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int init();
protected:
friend class FXOS8701CQ;
void parent_poll_notify();
private:
FXOS8701CQ *_parent;
orb_advert_t _mag_topic;
int _mag_orb_class_instance;
int _mag_class_instance;
void measure();
/* this class does not allow copying due to ptr data members */
FXOS8701CQ_mag(const FXOS8701CQ_mag &);
FXOS8701CQ_mag operator=(const FXOS8701CQ_mag &);
};
#endif

436
src/drivers/imu/fxos8701cq/fxos8701cq_main.cpp Normal file
View File

@ -0,0 +1,436 @@
/****************************************************************************
*
* Copyright (c) 2017-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 fxos8701cq.cpp
* Driver for the NXP FXOS8701CQ 6-axis sensor with integrated linear accelerometer and
* magnetometer connected via SPI.
*/
#include "FXOS8701CQ.hpp"
/**
* Local functions in support of the shell command.
*/
namespace fxos8701cq
{
FXOS8701CQ *g_dev;
void start(bool external_bus, enum Rotation rotation);
void test();
void reset();
void info();
void stop();
void regdump();
void usage();
void test_error();
/**
* 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)
{
int fd;
if (g_dev != nullptr) {
PX4_INFO("already started");
exit(0);
}
/* create the driver */
if (external_bus) {
#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_ACCEL_MAG)
g_dev = new FXOS8701CQ(PX4_SPI_BUS_EXT, FXOS8701C_DEVICE_PATH_ACCEL, PX4_SPIDEV_EXT_ACCEL_MAG, rotation);
#else
PX4_ERR("External SPI not available");
exit(0);
#endif
} else {
g_dev = new FXOS8701CQ(PX4_SPI_BUS_SENSORS, FXOS8701C_DEVICE_PATH_ACCEL, PX4_SPIDEV_ACCEL_MAG, rotation);
}
if (g_dev == nullptr) {
PX4_ERR("failed instantiating FXOS8701C obj");
goto fail;
}
if (OK != g_dev->init()) {
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = open(FXOS8701C_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd < 0) {
goto fail;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail;
}
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
int fd_mag;
fd_mag = open(FXOS8701C_DEVICE_PATH_MAG, O_RDONLY);
/* don't fail if open cannot be opened */
if (0 <= fd_mag) {
if (ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail;
}
}
close(fd_mag);
#endif
close(fd);
exit(0);
fail:
if (g_dev != nullptr) {
delete g_dev;
g_dev = nullptr;
}
errx(1, "driver start failed");
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test()
{
int rv = 1;
int fd_accel = -1;
sensor_accel_s accel_report;
ssize_t sz;
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
int fd_mag = -1;
int ret;
struct mag_report m_report;
#endif
/* get the driver */
fd_accel = open(FXOS8701C_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd_accel < 0) {
PX4_ERR("%s open failed", FXOS8701C_DEVICE_PATH_ACCEL);
goto exit_none;
}
/* do a simple demand read */
sz = read(fd_accel, &accel_report, sizeof(sensor_accel_s));
if (sz != sizeof(sensor_accel_s)) {
PX4_ERR("immediate read failed");
goto exit_with_accel;
}
PX4_INFO("accel x: \t% 9.5f\tm/s^2", (double)accel_report.x);
PX4_INFO("accel y: \t% 9.5f\tm/s^2", (double)accel_report.y);
PX4_INFO("accel z: \t% 9.5f\tm/s^2", (double)accel_report.z);
PX4_INFO("accel x: \t%d\traw", (int)accel_report.x_raw);
PX4_INFO("accel y: \t%d\traw", (int)accel_report.y_raw);
PX4_INFO("accel z: \t%d\traw", (int)accel_report.z_raw);
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
/* get the driver */
fd_mag = open(FXOS8701C_DEVICE_PATH_MAG, O_RDONLY);
if (fd_mag < 0) {
PX4_ERR("%s open failed", FXOS8701C_DEVICE_PATH_MAG);
goto exit_with_accel;
}
/* check if mag is onboard or external */
if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0) {
PX4_ERR("failed to get if mag is onboard or external");
goto exit_with_mag_accel;
}
PX4_INFO("mag device active: %s", ret ? "external" : "onboard");
/* do a simple demand read */
sz = read(fd_mag, &m_report, sizeof(m_report));
if (sz != sizeof(m_report)) {
PX4_ERR("immediate read failed");
goto exit_with_mag_accel;
}
PX4_INFO("mag x: \t% 9.5f\tga", (double)m_report.x);
PX4_INFO("mag y: \t% 9.5f\tga", (double)m_report.y);
PX4_INFO("mag z: \t% 9.5f\tga", (double)m_report.z);
PX4_INFO("mag x: \t%d\traw", (int)m_report.x_raw);
PX4_INFO("mag y: \t%d\traw", (int)m_report.y_raw);
PX4_INFO("mag z: \t%d\traw", (int)m_report.z_raw);
#endif
/* reset to default polling */
if (ioctl(fd_accel, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
PX4_ERR("reset to default polling");
} else {
rv = 0;
}
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
exit_with_mag_accel:
close(fd_mag);
#endif
exit_with_accel:
close(fd_accel);
reset();
if (rv == 0) {
PX4_INFO("PASS");
}
exit_none:
exit(rv);
}
/**
* Reset the driver.
*/
void
reset()
{
int fd = open(FXOS8701C_DEVICE_PATH_ACCEL, O_RDONLY);
int rv = 1;
if (fd < 0) {
PX4_ERR("Open failed\n");
exit(1);
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
PX4_ERR("driver reset failed");
exit(1);
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
PX4_ERR("accel pollrate reset failed");
exit(1);
}
close(fd);
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
fd = open(FXOS8701C_DEVICE_PATH_MAG, O_RDONLY);
if (fd < 0) {
PX4_ERR("mag could not be opened, external mag might be used");
} else {
/* no need to reset the mag as well, the reset() is the same */
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
PX4_ERR("mag pollrate reset failed");
} else {
rv = 0;
}
}
close(fd);
#endif
exit(rv);
}
/**
* Print a little info about the driver.
*/
void
info()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running\n");
exit(1);
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
exit(0);
}
void
stop()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running\n");
exit(1);
}
delete g_dev;
g_dev = nullptr;
exit(0);
}
/**
* dump registers from device
*/
void
regdump()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running\n");
exit(1);
}
printf("regdump @ %p\n", g_dev);
g_dev->print_registers();
exit(0);
}
/**
* trigger an error
*/
void
test_error()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running\n");
exit(1);
}
g_dev->test_error();
exit(0);
}
void
usage()
{
PX4_INFO("missing command: try 'start', 'info', 'stop', 'test', 'reset', 'testerror' or 'regdump'");
PX4_INFO("options:");
PX4_INFO(" -X (external bus)");
PX4_INFO(" -R rotation");
}
} // namespace
int
fxos8701cq_main(int argc, char *argv[])
{
bool external_bus = false;
int ch;
enum Rotation rotation = ROTATION_NONE;
int myoptind = 1;
const char *myoptarg = NULL;
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;
default:
fxos8701cq::usage();
exit(0);
}
}
const char *verb = argv[myoptind];
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
fxos8701cq::start(external_bus, rotation);
}
/*
* Test the driver/device.
*/
if (!strcmp(verb, "test")) {
fxos8701cq::test();
}
if (!strcmp(verb, "stop")) {
fxos8701cq::stop();
}
/*
* Reset the driver.
*/
if (!strcmp(verb, "reset")) {
fxos8701cq::reset();
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info")) {
fxos8701cq::info();
}
/*
* dump device registers
*/
if (!strcmp(verb, "regdump")) {
fxos8701cq::regdump();
}
/*
* trigger an error
*/
if (!strcmp(verb, "testerror")) {
fxos8701cq::test_error();
}
errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset', 'info', 'testerror' or 'regdump'");
}