diff --git a/src/drivers/imu/fxos8701cq/CMakeLists.txt b/src/drivers/imu/fxos8701cq/CMakeLists.txt index 34ffa9b602..7ec14b5f46 100644 --- a/src/drivers/imu/fxos8701cq/CMakeLists.txt +++ b/src/drivers/imu/fxos8701cq/CMakeLists.txt @@ -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 ) diff --git a/src/drivers/imu/fxos8701cq/fxos8701cq.cpp b/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp similarity index 77% rename from src/drivers/imu/fxos8701cq/fxos8701cq.cpp rename to src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp index e626f861f5..7aa1eb71e3 100644 --- a/src/drivers/imu/fxos8701cq/fxos8701cq.cpp +++ b/src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp @@ -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 -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) -# include -#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); diff --git a/src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp b/src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp new file mode 100644 index 0000000000..a30e4e05bf --- /dev/null +++ b/src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG) +# include +#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 diff --git a/src/drivers/imu/fxos8701cq/fxos8701cq_main.cpp b/src/drivers/imu/fxos8701cq/fxos8701cq_main.cpp new file mode 100644 index 0000000000..9344adb38a --- /dev/null +++ b/src/drivers/imu/fxos8701cq/fxos8701cq_main.cpp @@ -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'"); +}