HMC5883.hpp

Go to the documentation of this file.

/****************************************************************************
 *
 *   Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

#pragma once

#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/time.h>

#include <drivers/device/i2c.h>

#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>

#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>

#include <lib/perf/perf_counter.h>
#include <systemlib/err.h>

#include <drivers/drv_mag.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/drv_device.h>

#include <uORB/uORB.h>

#include <float.h>
#include <px4_platform_common/getopt.h>
#include <lib/conversion/rotation.h>

#include "hmc5883.h"

/*
 * HMC5883 internal constants and data structures.
 */

/* Max measurement rate is 160Hz, however with 160 it will be set to 166 Hz, therefore workaround using 150 */
#define HMC5883_CONVERSION_INTERVAL (1000000 / 150) /* microseconds */

#define ADDR_CONF_A         0x00
#define ADDR_CONF_B         0x01
#define ADDR_MODE           0x02
#define ADDR_DATA_OUT_X_MSB     0x03
#define ADDR_DATA_OUT_X_LSB     0x04
#define ADDR_DATA_OUT_Z_MSB     0x05
#define ADDR_DATA_OUT_Z_LSB     0x06
#define ADDR_DATA_OUT_Y_MSB     0x07
#define ADDR_DATA_OUT_Y_LSB     0x08
#define ADDR_STATUS         0x09

/* temperature on hmc5983 only */
#define ADDR_TEMP_OUT_MSB       0x31
#define ADDR_TEMP_OUT_LSB       0x32

/* modes not changeable outside of driver */
#define HMC5883L_MODE_NORMAL        (0 << 0)  /* default */
#define HMC5883L_MODE_POSITIVE_BIAS (1 << 0)  /* positive bias */
#define HMC5883L_MODE_NEGATIVE_BIAS (1 << 1)  /* negative bias */

#define HMC5883L_AVERAGING_1        (0 << 5) /* conf a register */
#define HMC5883L_AVERAGING_2        (1 << 5)
#define HMC5883L_AVERAGING_4        (2 << 5)
#define HMC5883L_AVERAGING_8        (3 << 5)

#define MODE_REG_CONTINOUS_MODE     (0 << 0)
#define MODE_REG_SINGLE_MODE        (1 << 0) /* default */

#define STATUS_REG_DATA_OUT_LOCK    (1 << 1) /* page 16: set if data is only partially read, read device to reset */
#define STATUS_REG_DATA_READY       (1 << 0) /* page 16: set if all axes have valid measurements */

#define HMC5983_TEMP_SENSOR_ENABLE  (1 << 7)

class HMC5883 : public device::CDev, public px4::ScheduledWorkItem
{
public:
    HMC5883(device::Device *interface, const char *path, enum Rotation rotation);
    virtual ~HMC5883();

    virtual int     init();

    virtual ssize_t     read(cdev::file_t *filp, char *buffer, size_t buflen);
    virtual int     ioctl(cdev::file_t *filp, int cmd, unsigned long arg);

    /**
     * Stop the automatic measurement state machine.
     */
    void            stop();

    /**
     * Diagnostics - print some basic information about the driver.
     */
    void            print_info();

protected:
    Device          *_interface;

private:

    unsigned        _measure_interval{0};

    ringbuffer::RingBuffer  *_reports;
    struct mag_calibration_s    _scale;
    float           _range_scale;
    float           _range_ga;
    bool            _collect_phase;
    int         _class_instance;
    int         _orb_class_instance;

    orb_advert_t        _mag_topic;

    perf_counter_t      _sample_perf;
    perf_counter_t      _comms_errors;
    perf_counter_t      _range_errors;
    perf_counter_t      _conf_errors;

    /* status reporting */
    bool            _sensor_ok;     /**< sensor was found and reports ok */

    enum Rotation       _rotation;

    struct mag_report   _last_report {};         /**< used for info() */

    uint8_t         _range_bits;
    uint8_t         _conf_reg;
    uint8_t         _temperature_counter;
    uint8_t         _temperature_error_count;

    /**
     * Initialise the automatic measurement state machine and start it.
     *
     * @note This function is called at open and error time.  It might make sense
     *       to make it more aggressive about resetting the bus in case of errors.
     */
    void            start();

    /**
     * Reset the device
     */
    int         reset();

    /**
     * Perform the on-sensor scale calibration routine.
     *
     * @note The sensor will continue to provide measurements, these
     *   will however reflect the uncalibrated sensor state until
     *   the calibration routine has been completed.
     *
     * @param enable set to 1 to enable self-test strap, 0 to disable
     */
    int         calibrate(cdev::file_t *filp, unsigned enable);

    /**
     * Perform the on-sensor scale calibration routine.
     *
     * @note The sensor will continue to provide measurements, these
     *   will however reflect the uncalibrated sensor state until
     *   the calibration routine has been completed.
     *
     * @param enable set to 1 to enable self-test positive strap, -1 to enable
     *        negative strap, 0 to set to normal mode
     */
    int         set_excitement(unsigned enable);

    /**
     * enable hmc5983 temperature compensation
     */
    int         set_temperature_compensation(unsigned enable);

    /**
     * Set the sensor range.
     *
     * Sets the internal range to handle at least the argument in Gauss.
     */
    int             set_range(unsigned range);

    /**
     * check the sensor range.
     *
     * checks that the range of the sensor is correctly set, to
     * cope with communication errors causing the range to change
     */
    void            check_range(void);

    /**
     * check the sensor configuration.
     *
     * checks that the config of the sensor is correctly set, to
     * cope with communication errors causing the configuration to
     * change
     */
    void            check_conf(void);

    /**
     * Perform a poll cycle; collect from the previous measurement
     * and start a new one.
     *
     * This is the heart of the measurement state machine.  This function
     * alternately starts a measurement, or collects the data from the
     * previous measurement.
     *
     * When the interval between measurements is greater than the minimum
     * measurement interval, a gap is inserted between collection
     * and measurement to provide the most recent measurement possible
     * at the next interval.
     */
    void            Run() override;

    /**
     * Write a register.
     *
     * @param reg       The register to write.
     * @param val       The value to write.
     * @return      OK on write success.
     */
    int         write_reg(uint8_t reg, uint8_t val);

    /**
     * Read a register.
     *
     * @param reg       The register to read.
     * @param val       The value read.
     * @return      OK on read success.
     */
    int         read_reg(uint8_t reg, uint8_t &val);

    /**
     * Issue a measurement command.
     *
     * @return      OK if the measurement command was successful.
     */
    int         measure();

    /**
     * Collect the result of the most recent measurement.
     */
    int         collect();

    /**
     * Convert a big-endian signed 16-bit value to a float.
     *
     * @param in        A signed 16-bit big-endian value.
     * @return      The floating-point representation of the value.
     */
    float           meas_to_float(uint8_t in[2]);

    /**
    * Check the current scale calibration
    *
    * @return 0 if scale calibration is ok, 1 else
    */
    int             check_scale();

    /**
    * Check the current offset calibration
    *
    * @return 0 if offset calibration is ok, 1 else
    */
    int             check_offset();
};