diff --git a/cmake/configs/nuttx_nxphlite-v3_default.cmake b/cmake/configs/nuttx_nxphlite-v3_default.cmake index 1a25319447..06c93ac8cb 100644 --- a/cmake/configs/nuttx_nxphlite-v3_default.cmake +++ b/cmake/configs/nuttx_nxphlite-v3_default.cmake @@ -18,6 +18,7 @@ set(config_module_list drivers/px4fmu drivers/boards/nxphlite-v3 drivers/rgbled + drivers/fxos8700cq drivers/mpu6000 drivers/mpu9250 drivers/hmc5883 diff --git a/src/drivers/fxos8700cq/CMakeLists.txt b/src/drivers/fxos8700cq/CMakeLists.txt new file mode 100644 index 0000000000..885a4a3a3b --- /dev/null +++ b/src/drivers/fxos8700cq/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2017 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__fxos8700cq + MAIN fxos8700cq + STACK_MAIN 1200 + COMPILE_FLAGS + SRCS + fxos8700cq.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : \ No newline at end of file diff --git a/src/drivers/fxos8700cq/fxos8700cq.cpp b/src/drivers/fxos8700cq/fxos8700cq.cpp new file mode 100644 index 0000000000..a4bf9d0f7f --- /dev/null +++ b/src/drivers/fxos8700cq/fxos8700cq.cpp @@ -0,0 +1,2104 @@ +/**************************************************************************** + * + * Copyright (c) 2017 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 fxos8700cq.cpp + * Driver for the ST FXOS8700CQ 3-axis, linear accelerometer and and 3-axis, + * magnetometer connected via SPI. + */ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +/* 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 FXOS8700C_DEVICE_PATH_ACCEL "/dev/fxos8700cq_accel" +#define FXOS8700C_DEVICE_PATH_ACCEL_EXT "/dev/fxos8700cq_accel_ext" +#define FXOS8700C_DEVICE_PATH_MAG "/dev/fxos8700cq_mag" + +#define FXOS8700CQ_DR_STATUS 0x00 +# define DR_STATUS_ZYXDR (1 << 3) + +#define FXOS8700CQ_OUT_X_MSB 0x01 + +#define FXOS8700CQ_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 FXOS8700CQ_WHOAMI 0x0d +# define FXOS8700CQ_WHOAMI_VAL 0xC7 + +#define FXOS8700CQ_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 FXOS8700CQ_CTRL_REG2 0x2b +# define CTRL_REG2_AUTO_INC (1 << 5) + +#define FXOS8700CQ_M_DR_STATUS 0x32 +# define M_DR_STATUS_ZYXDR (1 << 3) +#define FXOS8700CQ_M_OUT_X_MSB 0x33 +#define FXOS8700CQ_TEMP 0x51 +#define FXOS8700CQ_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 FXOS8700CQ_M_CTRL_REG2 0x5c +#define FXOS8700CQ_M_CTRL_REG3 0x5d + +#define DEF_REG(r) {r, #r} + +/* default values for this device */ +#define FXOS8700C_ACCEL_DEFAULT_RANGE_G 8 +#define FXOS8700C_ACCEL_DEFAULT_RATE 400 /* ODR is 400 in Hybird mode (accel + mag) */ +#define FXOS8700C_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50 +#define FXOS8700C_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 +#define FXOS8700C_ACCEL_MAX_OUTPUT_RATE 280 + +#define FXOS8700C_MAG_DEFAULT_RANGE_GA 12 /* It is fixed at 12 G */ +#define FXOS8700C_MAG_DEFAULT_RATE 100 +#define FXOS8700C_ONE_G 9.80665f + +#ifdef PX4_SPI_BUS_EXT +#define EXTERNAL_BUS PX4_SPI_BUS_EXT +#else +#define EXTERNAL_BUS 0 +#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 FXOS8700C_TIMER_REDUCTION 240 + +extern "C" { __EXPORT int fxos8700cq_main(int argc, char *argv[]); } + + +class FXOS8700CQ_mag; + +class FXOS8700CQ : public device::SPI +{ +public: + FXOS8700CQ(int bus, const char *path, uint32_t device, enum Rotation rotation); + virtual ~FXOS8700CQ(); + + 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(); + + friend class FXOS8700CQ_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); + +private: + + FXOS8700CQ_mag *_mag; + + struct hrt_call _accel_call; + struct hrt_call _mag_call; + + unsigned _call_accel_interval; + unsigned _call_mag_interval; + + ringbuffer::RingBuffer *_accel_reports; + ringbuffer::RingBuffer *_mag_reports; + + struct accel_calibration_s _accel_scale; + unsigned _accel_range_m_s2; + float _accel_range_scale; + unsigned _accel_samplerate; + unsigned _accel_onchip_filter_bandwith; + + struct mag_calibration_s _mag_scale; + unsigned _mag_range_ga; + float _mag_range_scale; + unsigned _mag_samplerate; + + orb_advert_t _accel_topic; + int _accel_orb_class_instance; + int _accel_class_instance; + + unsigned _accel_read; + unsigned _mag_read; + + perf_counter_t _accel_sample_perf; + perf_counter_t _mag_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; + int16_t _last_raw_mag_x; + int16_t _last_raw_mag_y; + int16_t _last_raw_mag_z; + + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset +#define FXOS8700C_NUM_CHECKED_REGISTERS 5 + static const uint8_t _checked_registers[FXOS8700C_NUM_CHECKED_REGISTERS]; + uint8_t _checked_values[FXOS8700C_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(); + + /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_bus == EXTERNAL_BUS); } + + /** + * Static trampoline from the hrt_call context; because we don't have a + * generic hrt wrapper yet. + * + * Called by the HRT in interrupt context at the specified rate if + * automatic polling is enabled. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void measure_trampoline(void *arg); + + /** + * Static trampoline for the mag because it runs at a lower rate + * + * @param arg Instance pointer for the driver that is polling. + */ + static void mag_measure_trampoline(void *arg); + + /** + * 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(); + + /** + * Accel self test + * + * @return 0 on success, 1 on failure + */ + int accel_self_test(); + + /** + * Mag self test + * + * @return 0 on success, 1 on failure + */ + int mag_self_test(); + + /** + * Read a register from the FXOS8700C + * + * @param The register to read. + * @return The value that was read. + */ + uint8_t read_reg(unsigned reg); + + /** + * Write a register in the FXOS8700C + * + * @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 FXOS8700C + * + * 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 FXOS8700C, 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 FXOS8700C 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 FXOS8700C 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 FXOS8700C 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 FXOS8700C 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 FXOS8700CQ 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 */ + FXOS8700CQ(const FXOS8700CQ &); + FXOS8700CQ operator=(const FXOS8700CQ &); +}; + +/* + list of registers that will be checked in check_registers(). Note + that ADDR_WHO_AM_I must be first in the list. + */ +const uint8_t FXOS8700CQ::_checked_registers[FXOS8700C_NUM_CHECKED_REGISTERS] = { + FXOS8700CQ_WHOAMI, + FXOS8700CQ_XYZ_DATA_CFG, + FXOS8700CQ_CTRL_REG1, + FXOS8700CQ_M_CTRL_REG1, + FXOS8700CQ_M_CTRL_REG2, +}; + +/** + * Helper class implementing the mag driver node. + */ +class FXOS8700CQ_mag : public device::CDev +{ +public: + FXOS8700CQ_mag(FXOS8700CQ *parent); + ~FXOS8700CQ_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 FXOS8700CQ; + + void parent_poll_notify(); +private: + FXOS8700CQ *_parent; + + orb_advert_t _mag_topic; + int _mag_orb_class_instance; + int _mag_class_instance; + + void measure(); + + void measure_trampoline(void *arg); + + /* this class does not allow copying due to ptr data members */ + FXOS8700CQ_mag(const FXOS8700CQ_mag &); + FXOS8700CQ_mag operator=(const FXOS8700CQ_mag &); +}; + + +FXOS8700CQ::FXOS8700CQ(int bus, const char *path, uint32_t device, enum Rotation rotation) : + SPI("FXOS8700CQ", path, bus, device, SPIDEV_MODE0, + 11 * 1000 * 1000 /* will be rounded to 10.4 MHz, within safety margins for FXOS8700C */), + _mag(new FXOS8700CQ_mag(this)), + _accel_call{}, + _mag_call{}, + _call_accel_interval(0), + _call_mag_interval(0), + _accel_reports(nullptr), + _mag_reports(nullptr), + _accel_scale{}, + _accel_range_m_s2(0.0f), + _accel_range_scale(0.0f), + _accel_samplerate(0), + _accel_onchip_filter_bandwith(0), + _mag_scale{}, + _mag_range_ga(0.0f), + _mag_range_scale(0.0f), + _mag_samplerate(0), + _accel_topic(nullptr), + _accel_orb_class_instance(-1), + _accel_class_instance(-1), + _accel_read(0), + _mag_read(0), + _accel_sample_perf(perf_alloc(PC_ELAPSED, "fxos8700cq_acc_read")), + _mag_sample_perf(perf_alloc(PC_ELAPSED, "fxos8700cq_mag_read")), + _bad_registers(perf_alloc(PC_COUNT, "fxos8700cq_bad_reg")), + _bad_values(perf_alloc(PC_COUNT, "fxos8700cq_bad_val")), + _accel_duplicates(perf_alloc(PC_COUNT, "fxos8700cq_acc_dupe")), + _register_wait(0), + _accel_filter_x(FXOS8700C_ACCEL_DEFAULT_RATE, FXOS8700C_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_y(FXOS8700C_ACCEL_DEFAULT_RATE, FXOS8700C_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_filter_z(FXOS8700C_ACCEL_DEFAULT_RATE, FXOS8700C_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), + _accel_int(1000000 / FXOS8700C_ACCEL_MAX_OUTPUT_RATE, true), + _rotation(rotation), + _constant_accel_count(0), + _last_temperature(0), + _last_raw_mag_x(0), + _last_raw_mag_y(0), + _last_raw_mag_z(0), + _checked_next(0) +{ + + + // enable debug() calls + _debug_enabled = true; + + _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_FXOS8700C; + + /* Prime _mag with parents devid. */ + _mag->_device_id.devid = _device_id.devid; + _mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_FXOS8700C; + + + // default scale factors + _accel_scale.x_offset = 0.0f; + _accel_scale.x_scale = 1.0f; + _accel_scale.y_offset = 0.0f; + _accel_scale.y_scale = 1.0f; + _accel_scale.z_offset = 0.0f; + _accel_scale.z_scale = 1.0f; + + _mag_scale.x_offset = 0.0f; + _mag_scale.x_scale = 1.0f; + _mag_scale.y_offset = 0.0f; + _mag_scale.y_scale = 1.0f; + _mag_scale.z_offset = 0.0f; + _mag_scale.z_scale = 1.0f; +} + +FXOS8700CQ::~FXOS8700CQ() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_accel_reports != nullptr) { + delete _accel_reports; + } + + if (_mag_reports != nullptr) { + delete _mag_reports; + } + + if (_accel_class_instance != -1) { + unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); + } + + delete _mag; + + /* delete the perf counter */ + perf_free(_accel_sample_perf); + perf_free(_mag_sample_perf); + perf_free(_bad_registers); + perf_free(_bad_values); + perf_free(_accel_duplicates); +} + +int +FXOS8700CQ::init() +{ + int ret = PX4_ERROR; + + /* do SPI init (and probe) first */ + if (SPI::init() != OK) { + PX4_ERR("SPI init failed"); + goto out; + } + + /* allocate basic report buffers */ + _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + + if (_accel_reports == nullptr) { + goto out; + } + + _mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); + + if (_mag_reports == nullptr) { + goto out; + } + + reset(); + + /* do CDev init for the mag device node */ + ret = _mag->init(); + + if (ret != OK) { + PX4_ERR("MAG init failed"); + goto out; + } + + /* fill report structures */ + measure(); + + /* advertise sensor topic, measure manually to initialize valid report */ + struct mag_report mrp; + _mag_reports->get(&mrp); + + /* measurement will have generated a report, publish */ + _mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, + &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); + + if (_mag->_mag_topic == nullptr) { + PX4_ERR("ADVERT ERR"); + } + + + _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); + + /* advertise sensor topic, measure manually to initialize valid report */ + struct accel_report arp; + _accel_reports->get(&arp); + + /* measurement will have generated a report, publish */ + _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, + &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); + + if (_accel_topic == nullptr) { + PX4_ERR("ADVERT ERR"); + } + +out: + return ret; +} + + +void +FXOS8700CQ::reset() +{ + + /* enable accel set it To Standby */ + + write_checked_reg(FXOS8700CQ_CTRL_REG1, 0); + write_checked_reg(FXOS8700CQ_XYZ_DATA_CFG, 0); + + /* Use hybird mode to read Accel and Mag */ + + write_checked_reg(FXOS8700CQ_M_CTRL_REG1, M_CTRL_REG1_HMS_AM | M_CTRL_REG1_OS(7)); + + /* Use the hybird auto increment mode to read all the data at the same time */ + + write_checked_reg(FXOS8700CQ_M_CTRL_REG2, CTRL_REG2_AUTO_INC); + + accel_set_range(FXOS8700C_ACCEL_DEFAULT_RANGE_G); + accel_set_samplerate(FXOS8700C_ACCEL_DEFAULT_RATE); + accel_set_driver_lowpass_filter((float)FXOS8700C_ACCEL_DEFAULT_RATE, (float)FXOS8700C_ACCEL_DEFAULT_DRIVER_FILTER_FREQ); + + // we setup the anti-alias on-chip filter as 50Hz. We believe + // this operates in the analog domain, and is critical for + // anti-aliasing. The 2 pole software filter is designed to + // operate in conjunction with this on-chip filter + accel_set_onchip_lowpass_filter_bandwidth(FXOS8700C_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); + + mag_set_range(FXOS8700C_MAG_DEFAULT_RANGE_GA); + + /* enable set it To Standby mode at 800 Hz which becomes 400 Hz due to hybird mode */ + + write_checked_reg(FXOS8700CQ_CTRL_REG1, CTRL_REG1_DR(0) | CTRL_REG1_ACTIVE); + + _accel_read = 0; + _mag_read = 0; +} + +int +FXOS8700CQ::probe() +{ + /* verify that the device is attached and functioning */ + bool success = (read_reg(FXOS8700CQ_WHOAMI) == FXOS8700CQ_WHOAMI_VAL); + + if (success) { + _checked_values[0] = FXOS8700CQ_WHOAMI_VAL; + return OK; + } + + return -EIO; +} + +ssize_t +FXOS8700CQ::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct accel_report); + accel_report *arb = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } + + /* if automatic measurement is enabled */ + if (_call_accel_interval > 0) { + /* + * While there is space in the caller's buffer, and reports, copy them. + */ + while (count--) { + if (_accel_reports->get(arb)) { + ret += sizeof(*arb); + arb++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement */ + measure(); + + /* measurement will have generated a report, copy it out */ + if (_accel_reports->get(arb)) { + ret = sizeof(*arb); + } + + return ret; +} + +ssize_t +FXOS8700CQ::mag_read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct mag_report); + mag_report *mrb = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } + + /* if automatic measurement is enabled */ + if (_call_mag_interval > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + */ + while (count--) { + if (_mag_reports->get(mrb)) { + ret += sizeof(*mrb); + mrb++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement */ + _mag_reports->flush(); + _mag->measure(); + + /* measurement will have generated a report, copy it out */ + if (_mag_reports->get(mrb)) { + ret = sizeof(*mrb); + } + + return ret; +} + +int +FXOS8700CQ::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_accel_interval = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + return ioctl(filp, SENSORIOCSPOLLRATE, 1600); + + case SENSOR_POLLRATE_DEFAULT: + return ioctl(filp, SENSORIOCSPOLLRATE, FXOS8700C_ACCEL_DEFAULT_RATE); + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_accel_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum sane rate */ + if (ticks < 500) { + return -EINVAL; + } + + /* adjust filters */ + accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq()); + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _call_accel_interval = ticks; + + _accel_call.period = _call_accel_interval - FXOS8700C_TIMER_REDUCTION; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_accel_interval == 0) { + return SENSOR_POLLRATE_MANUAL; + } + + return 1000000 / _call_accel_interval; + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = px4_enter_critical_section(); + + if (!_accel_reports->resize(arg)) { + px4_leave_critical_section(flags); + return -ENOMEM; + } + + px4_leave_critical_section(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _accel_reports->size(); + + case SENSORIOCRESET: + reset(); + return OK; + + case ACCELIOCSSAMPLERATE: + return accel_set_samplerate(arg); + + case ACCELIOCGSAMPLERATE: + return _accel_samplerate; + + case ACCELIOCSLOWPASS: { + return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg); + } + + case ACCELIOCGLOWPASS: + return static_cast(_accel_filter_x.get_cutoff_freq()); + + case ACCELIOCSSCALE: { + /* copy scale, but only if off by a few percent */ + struct accel_calibration_s *s = (struct accel_calibration_s *) arg; + float sum = s->x_scale + s->y_scale + s->z_scale; + + if (sum > 2.0f && sum < 4.0f) { + memcpy(&_accel_scale, s, sizeof(_accel_scale)); + return OK; + + } else { + return -EINVAL; + } + } + + case ACCELIOCSRANGE: + /* arg needs to be in G */ + return accel_set_range(arg); + + case ACCELIOCGRANGE: + /* convert to m/s^2 and return rounded in G */ + return (unsigned long)((_accel_range_m_s2) / FXOS8700C_ONE_G + 0.5f); + + case ACCELIOCGSCALE: + /* copy scale out */ + memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale)); + return OK; + + case ACCELIOCSELFTEST: + return accel_self_test(); + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); + } +} + +int +FXOS8700CQ::mag_ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _call_mag_interval = 0; + return OK; + + /* external signalling not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: + /* 100 Hz is max for mag */ + return mag_ioctl(filp, SENSORIOCSPOLLRATE, 100); + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_call_mag_interval == 0); + + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; + + /* check against maximum sane rate */ + if (ticks < 1000) { + return -EINVAL; + } + + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _mag_call.period = _call_mag_interval = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_call_mag_interval == 0) { + return SENSOR_POLLRATE_MANUAL; + } + + return 1000000 / _call_mag_interval; + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = px4_enter_critical_section(); + + if (!_mag_reports->resize(arg)) { + px4_leave_critical_section(flags); + return -ENOMEM; + } + + px4_leave_critical_section(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _mag_reports->size(); + + case SENSORIOCRESET: + reset(); + return OK; + + case MAGIOCSSAMPLERATE: + return mag_set_samplerate(arg); + + case MAGIOCGSAMPLERATE: + return _mag_samplerate; + + case MAGIOCSLOWPASS: + case MAGIOCGLOWPASS: + /* not supported, no internal filtering */ + return -EINVAL; + + case MAGIOCSSCALE: + /* copy scale in */ + memcpy(&_mag_scale, (struct mag_calibration_s *) arg, sizeof(_mag_scale)); + return OK; + + case MAGIOCGSCALE: + /* copy scale out */ + memcpy((struct mag_calibration_s *) arg, &_mag_scale, sizeof(_mag_scale)); + return OK; + + case MAGIOCSRANGE: + return mag_set_range(arg); + + case MAGIOCGRANGE: + return _mag_range_ga; + + case MAGIOCSELFTEST: + return mag_self_test(); + + case MAGIOCGEXTERNAL: + /* Even if this sensor is on the "external" SPI bus + * it is still fixed to the autopilot assembly, + * so always return 0. + */ + return 0; + + default: + /* give it to the superclass */ + return SPI::ioctl(filp, cmd, arg); + } +} + +int +FXOS8700CQ::accel_self_test() +{ + /*todo:Implement + * set to 2 Jmode Save current samples + * Light bit and look for the offsets. + * ±2 g mode, X-axis +192 + * ±2 g mode, Y-axis +270 + * ±2 g mode, Z-axis +1275 + */ + + + if (_accel_read == 0) { + return 1; + } + + return 0; +} + +int +FXOS8700CQ::mag_self_test() +{ + if (_mag_read == 0) { + return 1; + } + + /** + * inspect mag offsets + * don't check mag scale because it seems this is calibrated on chip + */ + if (fabsf(_mag_scale.x_offset) < 0.000001f) { + return 1; + } + + if (fabsf(_mag_scale.y_offset) < 0.000001f) { + return 1; + } + + if (fabsf(_mag_scale.z_offset) < 0.000001f) { + return 1; + } + + return 0; +} + +uint8_t +FXOS8700CQ::read_reg(unsigned reg) +{ + uint8_t cmd[3]; + + cmd[0] = DIR_READ(reg); + cmd[1] = ADDR_7(reg); + cmd[2] = 0; + + transfer(cmd, cmd, sizeof(cmd)); + + return cmd[2]; +} + +void +FXOS8700CQ::write_reg(unsigned reg, uint8_t value) +{ + uint8_t cmd[3]; + + cmd[0] = DIR_WRITE(reg); + cmd[1] = ADDR_7(reg); + cmd[2] = value; + + transfer(cmd, nullptr, sizeof(cmd)); +} + +void +FXOS8700CQ::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + + for (uint8_t i = 0; i < FXOS8700C_NUM_CHECKED_REGISTERS; i++) { + if (reg == _checked_registers[i]) { + _checked_values[i] = value; + } + } +} + +void +FXOS8700CQ::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) +{ + uint8_t val; + + val = read_reg(reg); + val &= ~clearbits; + val |= setbits; + write_checked_reg(reg, val); +} + +int +FXOS8700CQ::accel_set_range(unsigned max_g) +{ + uint8_t setbits = 0; + float lsb_per_g; + float max_accel_g; + + if (max_g == 0 || max_g > 8) { + max_g = 8; + } + + if (max_g > 4) { // 8g + setbits = XYZ_DATA_CFG_FS_8G; + lsb_per_g = 1024; + max_accel_g = 8; + + } else if (max_g > 2) { // 4g + setbits = XYZ_DATA_CFG_FS_4G; + lsb_per_g = 2048; + max_accel_g = 4; + + } else { // 2g + setbits = XYZ_DATA_CFG_FS_2G; + lsb_per_g = 4096; + max_accel_g = 2; + } + + _accel_range_scale = (FXOS8700C_ONE_G / lsb_per_g); + _accel_range_m_s2 = max_accel_g * FXOS8700C_ONE_G; + + + modify_reg(FXOS8700CQ_XYZ_DATA_CFG, XYZ_DATA_CFG_FS_MASK, setbits); + + return OK; +} + +int +FXOS8700CQ::mag_set_range(unsigned max_ga) +{ + _mag_range_ga = 12; + _mag_range_scale = 0.001f; + + return OK; +} + +int +FXOS8700CQ::accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth) +{ + return OK; +} + +int +FXOS8700CQ::accel_set_driver_lowpass_filter(float samplerate, float bandwidth) +{ + _accel_filter_x.set_cutoff_frequency(samplerate, bandwidth); + _accel_filter_y.set_cutoff_frequency(samplerate, bandwidth); + _accel_filter_z.set_cutoff_frequency(samplerate, bandwidth); + + return OK; +} + +int +FXOS8700CQ::accel_set_samplerate(unsigned frequency) +{ + uint8_t setbits = 0; + + /* The selected ODR is reduced by a factor of two when the device is operated in hybrid mode.*/ + + uint8_t active = read_reg(FXOS8700CQ_CTRL_REG1) & CTRL_REG1_ACTIVE; + + if (frequency == 0 || frequency == ACCEL_SAMPLERATE_DEFAULT) { + frequency = FXOS8700C_ACCEL_DEFAULT_RATE; + } + + if (frequency <= 25) { + setbits = CTRL_REG1_DR(4); // Use 50 as it is 50 / 2 + _accel_samplerate = 25; + + } else if (frequency <= 50) { + setbits = CTRL_REG1_DR(3); // Use 100 as it is 100 / 2 + _accel_samplerate = 50; + + } else if (frequency <= 100) { + setbits = CTRL_REG1_DR(2); // Use 200 as it is 200 / 2 + _accel_samplerate = 100; + + } else if (frequency <= 200) { + setbits = CTRL_REG1_DR(1); // Use 400 as it is 400 / 2; + _accel_samplerate = 200; + + } else if (frequency <= 400) { + setbits = CTRL_REG1_DR(0); // Use 800 as it is 800 / 2; + _accel_samplerate = 400; + + } else { + return -EINVAL; + } + + modify_reg(FXOS8700CQ_CTRL_REG1, CTRL_REG1_ACTIVE, 0); + modify_reg(FXOS8700CQ_CTRL_REG1, CTRL_REG1_DR_MASK, setbits); + modify_reg(FXOS8700CQ_CTRL_REG1, 0, active); + return OK; +} + +int +FXOS8700CQ::mag_set_samplerate(unsigned frequency) +{ + if (frequency == 0) { + frequency = 100; + } + + if (frequency <= 25) { + _mag_samplerate = 25; + + } else if (frequency <= 50) { + _mag_samplerate = 50; + + } else if (frequency <= 100) { + _mag_samplerate = 100; + + } else if (frequency <= 200) { + _mag_samplerate = 200; + + } else if (frequency <= 400) { + _mag_samplerate = 400; + + } else { + return -EINVAL; + } + + return OK; +} + + +void +FXOS8700CQ::start() +{ + /* make sure we are stopped first */ + stop(); + + /* reset the report ring */ + _accel_reports->flush(); + _mag_reports->flush(); + + /* start polling at the specified rate */ + hrt_call_every(&_accel_call, + 1000, + _call_accel_interval - FXOS8700C_TIMER_REDUCTION, + (hrt_callout)&FXOS8700CQ::measure_trampoline, this); + hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&FXOS8700CQ::mag_measure_trampoline, this); +} + +void +FXOS8700CQ::stop() +{ + hrt_cancel(&_accel_call); + hrt_cancel(&_mag_call); + + /* reset internal states */ + memset(_last_accel, 0, sizeof(_last_accel)); + + /* discard unread data in the buffers */ + _accel_reports->flush(); + _mag_reports->flush(); +} + +void +FXOS8700CQ::measure_trampoline(void *arg) +{ + FXOS8700CQ *dev = (FXOS8700CQ *)arg; + + /* make another measurement */ + dev->measure(); +} + +void +FXOS8700CQ::mag_measure_trampoline(void *arg) +{ + FXOS8700CQ *dev = (FXOS8700CQ *)arg; + + /* make another measurement */ + dev->mag_measure(); +} + +void +FXOS8700CQ::check_registers(void) +{ + + uint8_t v; + + if ((v = read_reg(_checked_registers[_checked_next])) != _checked_values[_checked_next]) { + /* + if we get the wrong value then we know the SPI bus + or sensor is very sick. We set _register_wait to 20 + and wait until we have seen 20 good values in a row + before we consider the sensor to be OK again. + */ + perf_count(_bad_registers); + + /* + try to fix the bad register value. We only try to + fix one per loop to prevent a bad sensor hogging the + bus. We skip zero as that is the WHO_AM_I, which + is not writeable + */ + if (_checked_next != 0) { + write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); + } + + _register_wait = 20; + } + + _checked_next = (_checked_next + 1) % FXOS8700C_NUM_CHECKED_REGISTERS; +} + +void +FXOS8700CQ::measure() +{ + /* status register and data as read back from the device */ + +#pragma pack(push, 1) + struct { + uint8_t cmd[2]; + uint8_t status; + int16_t x; + int16_t y; + int16_t z; + int16_t mx; + int16_t my; + int16_t mz; + } raw_accel_mag_report; +#pragma pack(pop) + + accel_report accel_report; + + /* start the performance counter */ + perf_begin(_accel_sample_perf); + + check_registers(); + + if (_register_wait != 0) { + // we are waiting for some good transfers before using + // the sensor again. + _register_wait--; + perf_end(_accel_sample_perf); + return; + } + + /* fetch data from the sensor */ + memset(&raw_accel_mag_report, 0, sizeof(raw_accel_mag_report)); + raw_accel_mag_report.cmd[0] = DIR_READ(FXOS8700CQ_DR_STATUS); + raw_accel_mag_report.cmd[1] = ADDR_7(FXOS8700CQ_DR_STATUS); + transfer((uint8_t *)&raw_accel_mag_report, (uint8_t *)&raw_accel_mag_report, sizeof(raw_accel_mag_report)); + + if (!(raw_accel_mag_report.status & DR_STATUS_ZYXDR)) { + perf_end(_accel_sample_perf); + perf_count(_accel_duplicates); + return; + } + + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ + + accel_report.timestamp = hrt_absolute_time(); + + /* + * Eight-bit 2’s complement sensor temperature value with 0.96 °C/LSB sensitivity. + * Temperature data is only valid between –40 °C and 125 °C. The temperature sensor + * output is only valid when M_CTRL_REG1[m_hms] > 0b00. Please note that the + * temperature sensor is uncalibrated and its output for a given temperature will vary from + * one device to the next + */ + + write_checked_reg(FXOS8700CQ_M_CTRL_REG1, M_CTRL_REG1_HMS_A | M_CTRL_REG1_OS(7)); + _last_temperature = (read_reg(FXOS8700CQ_TEMP)) * 0.96f; + write_checked_reg(FXOS8700CQ_M_CTRL_REG1, M_CTRL_REG1_HMS_AM | M_CTRL_REG1_OS(7)); + + accel_report.temperature = _last_temperature; + + // report the error count as the sum of the number of bad + // register reads and bad values. This allows the higher level + // code to decide if it should use this sensor based on + // whether it has had failures + accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); + + accel_report.x_raw = swap16RightJustify14(raw_accel_mag_report.x); + accel_report.y_raw = swap16RightJustify14(raw_accel_mag_report.y); + accel_report.z_raw = swap16RightJustify14(raw_accel_mag_report.z); + + /* Save off the Mag readings todo: revist integrating theses */ + + _last_raw_mag_x = swap16(raw_accel_mag_report.mx); + _last_raw_mag_y = swap16(raw_accel_mag_report.my); + _last_raw_mag_z = swap16(raw_accel_mag_report.mz); + + float xraw_f = accel_report.x_raw; + float yraw_f = accel_report.y_raw; + float zraw_f = accel_report.z_raw; + + // apply user specified rotation + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; + float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; + float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + + /* + we have logs where the accelerometers get stuck at a fixed + large value. We want to detect this and mark the sensor as + being faulty + */ + if (fabsf(_last_accel[0] - x_in_new) < 0.001f && + fabsf(_last_accel[1] - y_in_new) < 0.001f && + fabsf(_last_accel[2] - z_in_new) < 0.001f && + fabsf(x_in_new) > 20 && + fabsf(y_in_new) > 20 && + fabsf(z_in_new) > 20) { + _constant_accel_count += 1; + + } else { + _constant_accel_count = 0; + } + + if (_constant_accel_count > 100) { + // we've had 100 constant accel readings with large + // values. The sensor is almost certainly dead. We + // will raise the error_count so that the top level + // flight code will know to avoid this sensor, but + // we'll still give the data so that it can be logged + // and viewed + perf_count(_bad_values); + _constant_accel_count = 0; + } + + _last_accel[0] = x_in_new; + _last_accel[1] = y_in_new; + _last_accel[2] = z_in_new; + + accel_report.x = _accel_filter_x.apply(x_in_new); + accel_report.y = _accel_filter_y.apply(y_in_new); + accel_report.z = _accel_filter_z.apply(z_in_new); + + math::Vector<3> aval(x_in_new, y_in_new, z_in_new); + math::Vector<3> aval_integrated; + + bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt); + accel_report.x_integral = aval_integrated(0); + accel_report.y_integral = aval_integrated(1); + accel_report.z_integral = aval_integrated(2); + + accel_report.scaling = _accel_range_scale; + accel_report.range_m_s2 = _accel_range_m_s2; + + /* return device ID */ + accel_report.device_id = _device_id.devid; + + _accel_reports->force(&accel_report); + + /* notify anyone waiting for data */ + if (accel_notify) { + poll_notify(POLLIN); + + if (!(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + } + } + + _accel_read++; + + /* stop the perf counter */ + perf_end(_accel_sample_perf); +} + +void +FXOS8700CQ::mag_measure() +{ + /* status register and data as read back from the device */ + + mag_report mag_report {}; + + /* start the performance counter */ + perf_begin(_mag_sample_perf); + + /* + * 1) Scale raw value to SI units using scaling from datasheet. + * 2) Subtract static offset (in SI units) + * 3) Scale the statically calibrated values with a linear + * dynamically obtained factor + * + * Note: the static sensor offset is the number the sensor outputs + * at a nominally 'zero' input. Therefore the offset has to + * be subtracted. + * + * Example: A gyro outputs a value of 74 at zero angular rate + * the offset is 74 from the origin and subtracting + * 74 from all measurements centers them around zero. + */ + + mag_report.timestamp = hrt_absolute_time(); + mag_report.is_external = is_external(); + + mag_report.x_raw = _last_raw_mag_x; + mag_report.y_raw = _last_raw_mag_y; + mag_report.z_raw = _last_raw_mag_z; + + float xraw_f = mag_report.x_raw; + float yraw_f = mag_report.y_raw; + float zraw_f = mag_report.z_raw; + + /* apply user specified rotation */ + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; + mag_report.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; + mag_report.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; + mag_report.scaling = _mag_range_scale; + mag_report.range_ga = (float)_mag_range_ga; + mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); + + mag_report.temperature = _last_temperature; + mag_report.device_id = _mag->_device_id.devid; + + _mag_reports->force(&mag_report); + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + if (!(_pub_blocked)) { + /* publish it */ + orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); + } + + _mag_read++; + + /* stop the perf counter */ + perf_end(_mag_sample_perf); +} + +void +FXOS8700CQ::print_info() +{ + printf("accel reads: %u\n", _accel_read); + printf("mag reads: %u\n", _mag_read); + perf_print_counter(_accel_sample_perf); + perf_print_counter(_mag_sample_perf); + perf_print_counter(_bad_registers); + perf_print_counter(_bad_values); + perf_print_counter(_accel_duplicates); + _accel_reports->print_info("accel reports"); + _mag_reports->print_info("mag reports"); + ::printf("checked_next: %u\n", _checked_next); + + for (uint8_t i = 0; i < FXOS8700C_NUM_CHECKED_REGISTERS; i++) { + uint8_t v = read_reg(_checked_registers[i]); + + if (v != _checked_values[i]) { + ::printf("reg %02x:%02x should be %02x\n", + (unsigned)_checked_registers[i], + (unsigned)v, + (unsigned)_checked_values[i]); + } + } + + ::printf("temperature: %.2f\n", (double)_last_temperature); +} + +void +FXOS8700CQ::print_registers() +{ + const struct { + uint8_t reg; + const char *name; + } regmap[] = { + DEF_REG(FXOS8700CQ_DR_STATUS), + DEF_REG(FXOS8700CQ_OUT_X_MSB), + DEF_REG(FXOS8700CQ_XYZ_DATA_CFG), + DEF_REG(FXOS8700CQ_WHOAMI), + DEF_REG(FXOS8700CQ_CTRL_REG1), + DEF_REG(FXOS8700CQ_CTRL_REG2), + DEF_REG(FXOS8700CQ_M_DR_STATUS), + DEF_REG(FXOS8700CQ_M_OUT_X_MSB), + DEF_REG(FXOS8700CQ_M_CTRL_REG1), + DEF_REG(FXOS8700CQ_M_CTRL_REG2), + DEF_REG(FXOS8700CQ_M_CTRL_REG3), + }; + + for (uint8_t i = 0; i < sizeof(regmap) / sizeof(regmap[0]); i++) { + printf("0x%02x %s\n", read_reg(regmap[i].reg), regmap[i].name); + } +} + +void +FXOS8700CQ::test_error() +{ + // trigger an error + write_reg(FXOS8700CQ_CTRL_REG1, 0); +} + +FXOS8700CQ_mag::FXOS8700CQ_mag(FXOS8700CQ *parent) : + CDev("FXOS8700C_mag", FXOS8700C_DEVICE_PATH_MAG), + _parent(parent), + _mag_topic(nullptr), + _mag_orb_class_instance(-1), + _mag_class_instance(-1) +{ +} + +FXOS8700CQ_mag::~FXOS8700CQ_mag() +{ + if (_mag_class_instance != -1) { + unregister_class_devname(MAG_BASE_DEVICE_PATH, _mag_class_instance); + } +} + +int +FXOS8700CQ_mag::init() +{ + int ret; + + ret = CDev::init(); + + if (ret == OK) { + _mag_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH); + } + + return ret; +} + +void +FXOS8700CQ_mag::parent_poll_notify() +{ + poll_notify(POLLIN); +} + +ssize_t +FXOS8700CQ_mag::read(struct file *filp, char *buffer, size_t buflen) +{ + return _parent->mag_read(filp, buffer, buflen); +} + +int +FXOS8700CQ_mag::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + case DEVIOCGDEVICEID: + return (int)CDev::ioctl(filp, cmd, arg); + break; + + default: + return _parent->mag_ioctl(filp, cmd, arg); + } +} + +void +FXOS8700CQ_mag::measure() +{ + _parent->mag_measure(); +} + +void +FXOS8700CQ_mag::measure_trampoline(void *arg) +{ + _parent->mag_measure_trampoline(arg); +} + +/** + * Local functions in support of the shell command. + */ +namespace fxos8700cq +{ + +FXOS8700CQ *g_dev; + +void start(bool external_bus, enum Rotation rotation, unsigned range); +void test(); +void reset(); +void info(); +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, unsigned range) +{ + int fd, fd_mag; + + 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 FXOS8700CQ(PX4_SPI_BUS_EXT, FXOS8700C_DEVICE_PATH_ACCEL, PX4_SPIDEV_EXT_ACCEL_MAG, rotation); +#else + PX4_ERR("External SPI not available"); + exit(0); +#endif + + } else { + g_dev = new FXOS8700CQ(PX4_SPI_BUS_SENSORS, FXOS8700C_DEVICE_PATH_ACCEL, PX4_SPIDEV_ACCEL_MAG, rotation); + } + + if (g_dev == nullptr) { + PX4_ERR("failed instantiating FXOS8700C obj"); + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } + + /* set the poll rate to default, starts automatic data collection */ + fd = open(FXOS8700C_DEVICE_PATH_ACCEL, O_RDONLY); + + if (fd < 0) { + goto fail; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + + if (ioctl(fd, ACCELIOCSRANGE, range) < 0) { + goto fail; + } + + fd_mag = open(FXOS8700C_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); + close(fd_mag); + + 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; + struct accel_report accel_report; + ssize_t sz; + int ret; + int fd_mag = -1; + struct mag_report m_report; + + + /* get the driver */ + fd_accel = open(FXOS8700C_DEVICE_PATH_ACCEL, O_RDONLY); + + if (fd_accel < 0) { + PX4_ERR("%s open failed", FXOS8700C_DEVICE_PATH_ACCEL); + goto exit_none; + } + + /* do a simple demand read */ + sz = read(fd_accel, &accel_report, sizeof(accel_report)); + + if (sz != sizeof(accel_report)) { + 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); + + PX4_INFO("accel range: %8.4f m/s^2", (double)accel_report.range_m_s2); + + if (PX4_ERROR == (ret = ioctl(fd_accel, ACCELIOCGLOWPASS, 0))) { + PX4_ERR("accel antialias filter bandwidth: fail"); + + } else { + PX4_INFO("accel antialias filter bandwidth: %d Hz", ret); + } + + /* get the driver */ + fd_mag = open(FXOS8700C_DEVICE_PATH_MAG, O_RDONLY); + + if (fd_mag < 0) { + PX4_ERR("%s open failed", FXOS8700C_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); + PX4_INFO("mag range: %8.4f ga", (double)m_report.range_ga); + + /* reset to default polling */ + if (ioctl(fd_accel, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + PX4_ERR("reset to default polling"); + + } else { + rv = 0; + } + +exit_with_mag_accel: + close(fd_mag); + +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(FXOS8700C_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); + + fd = open(FXOS8700C_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); + + 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); +} + +/** + * 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', 'test', 'reset', 'testerror' or 'regdump'"); + PX4_INFO("options:"); + PX4_INFO(" -X (external bus)"); + PX4_INFO(" -R rotation"); + PX4_INFO(" -a range in ga 2,4,8"); +} + +} // namespace + +int +fxos8700cq_main(int argc, char *argv[]) +{ + bool external_bus = false; + int ch; + enum Rotation rotation = ROTATION_NONE; + int accel_range = 8; + + 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; + + case 'a': + accel_range = atoi(myoptarg); + break; + + default: + fxos8700cq::usage(); + exit(0); + } + } + + const char *verb = argv[myoptind]; + + /* + * Start/load the driver. + + */ + if (!strcmp(verb, "start")) { + fxos8700cq::start(external_bus, rotation, accel_range); + } + + /* + * Test the driver/device. + */ + if (!strcmp(verb, "test")) { + fxos8700cq::test(); + } + + /* + * Reset the driver. + */ + if (!strcmp(verb, "reset")) { + fxos8700cq::reset(); + } + + /* + * Print driver information. + */ + if (!strcmp(verb, "info")) { + fxos8700cq::info(); + } + + /* + * dump device registers + */ + if (!strcmp(verb, "regdump")) { + fxos8700cq::regdump(); + } + + /* + * trigger an error + */ + if (!strcmp(verb, "testerror")) { + fxos8700cq::test_error(); + } + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'"); +}