mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-27 07:23:34 +08:00
hmc5883: build on linux and qurt
This commit is contained in:
Daniel Agar
@@ -21,6 +21,7 @@ px4_add_board(
|
|||||||
linux_pwm_out
|
linux_pwm_out
|
||||||
linux_sbus
|
linux_sbus
|
||||||
#magnetometer # all available magnetometer drivers
|
#magnetometer # all available magnetometer drivers
|
||||||
|
magnetometer/hmc5883
|
||||||
pwm_out_sim
|
pwm_out_sim
|
||||||
#telemetry # all available telemetry drivers
|
#telemetry # all available telemetry drivers
|
||||||
|
|
||||||
|
|||||||
@@ -48,6 +48,7 @@ px4_add_board(
|
|||||||
barometer/bmp280
|
barometer/bmp280
|
||||||
gps
|
gps
|
||||||
imu/mpu9250
|
imu/mpu9250
|
||||||
|
magnetometer/hmc5883
|
||||||
spektrum_rc
|
spektrum_rc
|
||||||
qshell/qurt
|
qshell/qurt
|
||||||
snapdragon_pwm_out
|
snapdragon_pwm_out
|
||||||
|
|||||||
@@ -19,6 +19,7 @@ px4_add_board(
|
|||||||
#imu # all available imu drivers
|
#imu # all available imu drivers
|
||||||
imu/mpu9250
|
imu/mpu9250
|
||||||
#magnetometer # all available magnetometer drivers
|
#magnetometer # all available magnetometer drivers
|
||||||
|
magnetometer/hmc5883
|
||||||
pwm_out_sim
|
pwm_out_sim
|
||||||
#telemetry # all available telemetry drivers
|
#telemetry # all available telemetry drivers
|
||||||
|
|
||||||
|
|||||||
@@ -17,6 +17,7 @@ px4_add_board(
|
|||||||
#imu # all available imu drivers
|
#imu # all available imu drivers
|
||||||
imu/mpu9250
|
imu/mpu9250
|
||||||
#magnetometer # all available magnetometer drivers
|
#magnetometer # all available magnetometer drivers
|
||||||
|
magnetometer/hmc5883
|
||||||
pwm_out_sim
|
pwm_out_sim
|
||||||
#telemetry # all available telemetry drivers
|
#telemetry # all available telemetry drivers
|
||||||
|
|
||||||
|
|||||||
@@ -36,9 +36,11 @@ px4_add_module(
|
|||||||
COMPILE_FLAGS
|
COMPILE_FLAGS
|
||||||
-Wno-cast-align # TODO: fix and enable
|
-Wno-cast-align # TODO: fix and enable
|
||||||
SRCS
|
SRCS
|
||||||
|
HMC5883.cpp
|
||||||
|
HMC5883.hpp
|
||||||
hmc5883_i2c.cpp
|
hmc5883_i2c.cpp
|
||||||
hmc5883_spi.cpp
|
hmc5883_spi.cpp
|
||||||
hmc5883.cpp
|
hmc5883_main.cpp
|
||||||
DEPENDS
|
DEPENDS
|
||||||
px4_work_queue
|
px4_work_queue
|
||||||
)
|
)
|
||||||
|
|||||||
+17
-797
@@ -31,298 +31,7 @@
|
|||||||
*
|
*
|
||||||
****************************************************************************/
|
****************************************************************************/
|
||||||
|
|
||||||
/**
|
#include "HMC5883.hpp"
|
||||||
* @file hmc5883.cpp
|
|
||||||
*
|
|
||||||
* Driver for the HMC5883 / HMC5983 magnetometer connected via I2C or SPI.
|
|
||||||
*/
|
|
||||||
|
|
||||||
#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 <nuttx/arch.h>
|
|
||||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
|
||||||
#include <nuttx/clock.h>
|
|
||||||
|
|
||||||
#include <board_config.h>
|
|
||||||
|
|
||||||
#include <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)
|
|
||||||
|
|
||||||
enum HMC5883_BUS {
|
|
||||||
HMC5883_BUS_ALL = 0,
|
|
||||||
HMC5883_BUS_I2C_INTERNAL,
|
|
||||||
HMC5883_BUS_I2C_EXTERNAL,
|
|
||||||
HMC5883_BUS_SPI
|
|
||||||
};
|
|
||||||
|
|
||||||
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(struct file *filp, char *buffer, size_t buflen);
|
|
||||||
virtual int ioctl(struct file *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 */
|
|
||||||
bool _calibrated; /**< the calibration is valid */
|
|
||||||
|
|
||||||
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(struct file *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();
|
|
||||||
|
|
||||||
/* this class has pointer data members, do not allow copying it */
|
|
||||||
HMC5883(const HMC5883 &);
|
|
||||||
HMC5883 operator=(const HMC5883 &);
|
|
||||||
};
|
|
||||||
|
|
||||||
/*
|
|
||||||
* Driver 'main' command.
|
|
||||||
*/
|
|
||||||
extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]);
|
|
||||||
|
|
||||||
|
|
||||||
HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rotation) :
|
HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rotation) :
|
||||||
CDev("HMC5883", path),
|
CDev("HMC5883", path),
|
||||||
@@ -336,12 +45,11 @@ HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rota
|
|||||||
_class_instance(-1),
|
_class_instance(-1),
|
||||||
_orb_class_instance(-1),
|
_orb_class_instance(-1),
|
||||||
_mag_topic(nullptr),
|
_mag_topic(nullptr),
|
||||||
_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
|
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
|
||||||
_comms_errors(perf_alloc(PC_COUNT, "hmc5883_com_err")),
|
_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err")),
|
||||||
_range_errors(perf_alloc(PC_COUNT, "hmc5883_rng_err")),
|
_range_errors(perf_alloc(PC_COUNT, MODULE_NAME": rng_err")),
|
||||||
_conf_errors(perf_alloc(PC_COUNT, "hmc5883_conf_err")),
|
_conf_errors(perf_alloc(PC_COUNT, MODULE_NAME": conf_err")),
|
||||||
_sensor_ok(false),
|
_sensor_ok(false),
|
||||||
_calibrated(false),
|
|
||||||
_rotation(rotation),
|
_rotation(rotation),
|
||||||
_range_bits(0),
|
_range_bits(0),
|
||||||
_conf_reg(0),
|
_conf_reg(0),
|
||||||
@@ -354,9 +62,6 @@ HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rota
|
|||||||
_device_id.devid_s.address = _interface->get_device_address();
|
_device_id.devid_s.address = _interface->get_device_address();
|
||||||
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
|
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
|
||||||
|
|
||||||
// enable debug() calls
|
|
||||||
_debug_enabled = false;
|
|
||||||
|
|
||||||
// default scaling
|
// default scaling
|
||||||
_scale.x_offset = 0;
|
_scale.x_offset = 0;
|
||||||
_scale.x_scale = 1.0f;
|
_scale.x_scale = 1.0f;
|
||||||
@@ -536,7 +241,7 @@ void HMC5883::check_conf(void)
|
|||||||
}
|
}
|
||||||
|
|
||||||
ssize_t
|
ssize_t
|
||||||
HMC5883::read(struct file *filp, char *buffer, size_t buflen)
|
HMC5883::read(cdev::file_t *filp, char *buffer, size_t buflen)
|
||||||
{
|
{
|
||||||
unsigned count = buflen / sizeof(struct mag_report);
|
unsigned count = buflen / sizeof(struct mag_report);
|
||||||
struct mag_report *mag_buf = reinterpret_cast<struct mag_report *>(buffer);
|
struct mag_report *mag_buf = reinterpret_cast<struct mag_report *>(buffer);
|
||||||
@@ -594,7 +299,7 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
|
|||||||
}
|
}
|
||||||
|
|
||||||
int
|
int
|
||||||
HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
|
HMC5883::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
|
||||||
{
|
{
|
||||||
unsigned dummy = arg;
|
unsigned dummy = arg;
|
||||||
|
|
||||||
@@ -971,9 +676,9 @@ out:
|
|||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
int HMC5883::calibrate(struct file *filp, unsigned enable)
|
int HMC5883::calibrate(cdev::file_t *filp, unsigned enable)
|
||||||
{
|
{
|
||||||
struct mag_report report;
|
sensor_mag_s report{};
|
||||||
ssize_t sz;
|
ssize_t sz;
|
||||||
int ret = 1;
|
int ret = 1;
|
||||||
uint8_t good_count = 0;
|
uint8_t good_count = 0;
|
||||||
@@ -1041,12 +746,12 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
|
|||||||
|
|
||||||
// discard 10 samples to let the sensor settle
|
// discard 10 samples to let the sensor settle
|
||||||
for (uint8_t i = 0; i < 10; i++) {
|
for (uint8_t i = 0; i < 10; i++) {
|
||||||
struct pollfd fds;
|
px4_pollfd_struct_t fds{};
|
||||||
|
|
||||||
/* wait for data to be ready */
|
/* wait for data to be ready */
|
||||||
fds.fd = fd;
|
fds.fd = fd;
|
||||||
fds.events = POLLIN;
|
fds.events = POLLIN;
|
||||||
ret = ::poll(&fds, 1, 2000);
|
ret = px4_poll(&fds, 1, 2000);
|
||||||
|
|
||||||
if (ret != 1) {
|
if (ret != 1) {
|
||||||
warn("ERROR: TIMEOUT 1");
|
warn("ERROR: TIMEOUT 1");
|
||||||
@@ -1054,7 +759,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
|
|||||||
}
|
}
|
||||||
|
|
||||||
/* now go get it */
|
/* now go get it */
|
||||||
sz = ::read(fd, &report, sizeof(report));
|
sz = px4_read(fd, &report, sizeof(report));
|
||||||
|
|
||||||
if (sz != sizeof(report)) {
|
if (sz != sizeof(report)) {
|
||||||
warn("ERROR: READ 1");
|
warn("ERROR: READ 1");
|
||||||
@@ -1065,12 +770,12 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
|
|||||||
|
|
||||||
/* read the sensor up to 150x, stopping when we have 50 good values */
|
/* read the sensor up to 150x, stopping when we have 50 good values */
|
||||||
for (uint8_t i = 0; i < 150 && good_count < 50; i++) {
|
for (uint8_t i = 0; i < 150 && good_count < 50; i++) {
|
||||||
struct pollfd fds;
|
px4_pollfd_struct_t fds{};
|
||||||
|
|
||||||
/* wait for data to be ready */
|
/* wait for data to be ready */
|
||||||
fds.fd = fd;
|
fds.fd = fd;
|
||||||
fds.events = POLLIN;
|
fds.events = POLLIN;
|
||||||
ret = ::poll(&fds, 1, 2000);
|
ret = px4_poll(&fds, 1, 2000);
|
||||||
|
|
||||||
if (ret != 1) {
|
if (ret != 1) {
|
||||||
warn("ERROR: TIMEOUT 2");
|
warn("ERROR: TIMEOUT 2");
|
||||||
@@ -1078,7 +783,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
|
|||||||
}
|
}
|
||||||
|
|
||||||
/* now go get it */
|
/* now go get it */
|
||||||
sz = ::read(fd, &report, sizeof(report));
|
sz = px4_read(fd, &report, sizeof(report));
|
||||||
|
|
||||||
if (sz != sizeof(report)) {
|
if (sz != sizeof(report)) {
|
||||||
warn("ERROR: READ 2");
|
warn("ERROR: READ 2");
|
||||||
@@ -1127,11 +832,11 @@ out:
|
|||||||
|
|
||||||
/* set back to normal mode */
|
/* set back to normal mode */
|
||||||
/* Set to 1.9 Gauss */
|
/* Set to 1.9 Gauss */
|
||||||
if (OK != ::ioctl(fd, MAGIOCSRANGE, 2)) {
|
if (OK != px4_ioctl(fd, MAGIOCSRANGE, 2)) {
|
||||||
warnx("FAILED: MAGIOCSRANGE 1.9 Ga");
|
warnx("FAILED: MAGIOCSRANGE 1.9 Ga");
|
||||||
}
|
}
|
||||||
|
|
||||||
if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
|
if (OK != px4_ioctl(fd, MAGIOCEXSTRAP, 0)) {
|
||||||
warnx("FAILED: MAGIOCEXSTRAP 0");
|
warnx("FAILED: MAGIOCEXSTRAP 0");
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1314,488 +1019,3 @@ HMC5883::print_info()
|
|||||||
print_message(_last_report);
|
print_message(_last_report);
|
||||||
_reports->print_info("report queue");
|
_reports->print_info("report queue");
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
|
||||||
* Local functions in support of the shell command.
|
|
||||||
*/
|
|
||||||
namespace hmc5883
|
|
||||||
{
|
|
||||||
|
|
||||||
/*
|
|
||||||
list of supported bus configurations
|
|
||||||
*/
|
|
||||||
struct hmc5883_bus_option {
|
|
||||||
enum HMC5883_BUS busid;
|
|
||||||
const char *devpath;
|
|
||||||
HMC5883_constructor interface_constructor;
|
|
||||||
uint8_t busnum;
|
|
||||||
HMC5883 *dev;
|
|
||||||
} bus_options[] = {
|
|
||||||
{ HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL },
|
|
||||||
#ifdef PX4_I2C_BUS_EXPANSION1
|
|
||||||
{ HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext1", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION1, NULL },
|
|
||||||
#endif
|
|
||||||
#ifdef PX4_I2C_BUS_EXPANSION2
|
|
||||||
{ HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext2", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION2, NULL },
|
|
||||||
#endif
|
|
||||||
#ifdef PX4_I2C_BUS_ONBOARD
|
|
||||||
{ HMC5883_BUS_I2C_INTERNAL, "/dev/hmc5883_int", &HMC5883_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL },
|
|
||||||
#endif
|
|
||||||
#ifdef PX4_SPIDEV_HMC
|
|
||||||
{ HMC5883_BUS_SPI, "/dev/hmc5883_spi", &HMC5883_SPI_interface, PX4_SPI_BUS_SENSORS, NULL },
|
|
||||||
#endif
|
|
||||||
};
|
|
||||||
#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
|
|
||||||
|
|
||||||
void start(enum HMC5883_BUS busid, enum Rotation rotation);
|
|
||||||
int stop();
|
|
||||||
bool start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation);
|
|
||||||
struct hmc5883_bus_option &find_bus(enum HMC5883_BUS busid);
|
|
||||||
void test(enum HMC5883_BUS busid);
|
|
||||||
void reset(enum HMC5883_BUS busid);
|
|
||||||
int info(enum HMC5883_BUS busid);
|
|
||||||
int calibrate(enum HMC5883_BUS busid);
|
|
||||||
int temp_enable(HMC5883_BUS busid, bool enable);
|
|
||||||
void usage();
|
|
||||||
|
|
||||||
/**
|
|
||||||
* start driver for a specific bus option
|
|
||||||
*/
|
|
||||||
bool
|
|
||||||
start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation)
|
|
||||||
{
|
|
||||||
if (bus.dev != nullptr) {
|
|
||||||
errx(1, "bus option already started");
|
|
||||||
}
|
|
||||||
|
|
||||||
device::Device *interface = bus.interface_constructor(bus.busnum);
|
|
||||||
|
|
||||||
if (interface->init() != OK) {
|
|
||||||
delete interface;
|
|
||||||
warnx("no device on bus %u (type: %u)", (unsigned)bus.busnum, (unsigned)bus.busid);
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
bus.dev = new HMC5883(interface, bus.devpath, rotation);
|
|
||||||
|
|
||||||
if (bus.dev != nullptr && OK != bus.dev->init()) {
|
|
||||||
delete bus.dev;
|
|
||||||
bus.dev = NULL;
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
int fd = open(bus.devpath, O_RDONLY);
|
|
||||||
|
|
||||||
if (fd < 0) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
|
|
||||||
close(fd);
|
|
||||||
errx(1, "Failed to setup poll rate");
|
|
||||||
}
|
|
||||||
|
|
||||||
close(fd);
|
|
||||||
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Start the driver.
|
|
||||||
*
|
|
||||||
* This function call only returns once the driver
|
|
||||||
* is either successfully up and running or failed to start.
|
|
||||||
*/
|
|
||||||
void
|
|
||||||
start(enum HMC5883_BUS busid, enum Rotation rotation)
|
|
||||||
{
|
|
||||||
bool started = false;
|
|
||||||
|
|
||||||
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
|
|
||||||
if (busid == HMC5883_BUS_ALL && bus_options[i].dev != NULL) {
|
|
||||||
// this device is already started
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (busid != HMC5883_BUS_ALL && bus_options[i].busid != busid) {
|
|
||||||
// not the one that is asked for
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
started |= start_bus(bus_options[i], rotation);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!started) {
|
|
||||||
exit(1);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
int
|
|
||||||
stop()
|
|
||||||
{
|
|
||||||
bool stopped = false;
|
|
||||||
|
|
||||||
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
|
|
||||||
if (bus_options[i].dev != nullptr) {
|
|
||||||
bus_options[i].dev->stop();
|
|
||||||
delete bus_options[i].dev;
|
|
||||||
bus_options[i].dev = nullptr;
|
|
||||||
stopped = true;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
return !stopped;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* find a bus structure for a busid
|
|
||||||
*/
|
|
||||||
struct hmc5883_bus_option &find_bus(enum HMC5883_BUS busid)
|
|
||||||
{
|
|
||||||
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
|
|
||||||
if ((busid == HMC5883_BUS_ALL ||
|
|
||||||
busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
|
|
||||||
return bus_options[i];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
errx(1, "bus %u not started", (unsigned)busid);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Perform some basic functional tests on the driver;
|
|
||||||
* make sure we can collect data from the sensor in polled
|
|
||||||
* and automatic modes.
|
|
||||||
*/
|
|
||||||
void
|
|
||||||
test(enum HMC5883_BUS busid)
|
|
||||||
{
|
|
||||||
struct hmc5883_bus_option &bus = find_bus(busid);
|
|
||||||
struct mag_report report;
|
|
||||||
ssize_t sz;
|
|
||||||
int ret;
|
|
||||||
const char *path = bus.devpath;
|
|
||||||
|
|
||||||
int fd = open(path, O_RDONLY);
|
|
||||||
|
|
||||||
if (fd < 0) {
|
|
||||||
err(1, "%s open failed (try 'hmc5883 start')", path);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* do a simple demand read */
|
|
||||||
sz = read(fd, &report, sizeof(report));
|
|
||||||
|
|
||||||
if (sz != sizeof(report)) {
|
|
||||||
err(1, "immediate read failed");
|
|
||||||
}
|
|
||||||
|
|
||||||
print_message(report);
|
|
||||||
|
|
||||||
/* check if mag is onboard or external */
|
|
||||||
if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0) {
|
|
||||||
errx(1, "failed to get if mag is onboard or external");
|
|
||||||
}
|
|
||||||
|
|
||||||
warnx("device active: %s", ret ? "external" : "onboard");
|
|
||||||
|
|
||||||
/* read the sensor 5x and report each value */
|
|
||||||
for (unsigned i = 0; i < 5; i++) {
|
|
||||||
struct pollfd fds;
|
|
||||||
|
|
||||||
/* wait for data to be ready */
|
|
||||||
fds.fd = fd;
|
|
||||||
fds.events = POLLIN;
|
|
||||||
ret = poll(&fds, 1, 2000);
|
|
||||||
|
|
||||||
if (ret != 1) {
|
|
||||||
errx(1, "timed out waiting for sensor data");
|
|
||||||
}
|
|
||||||
|
|
||||||
/* now go get it */
|
|
||||||
sz = read(fd, &report, sizeof(report));
|
|
||||||
|
|
||||||
if (sz != sizeof(report)) {
|
|
||||||
err(1, "periodic read failed");
|
|
||||||
}
|
|
||||||
|
|
||||||
print_message(report);
|
|
||||||
}
|
|
||||||
|
|
||||||
errx(0, "PASS");
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Automatic scale calibration.
|
|
||||||
*
|
|
||||||
* Basic idea:
|
|
||||||
*
|
|
||||||
* output = (ext field +- 1.1 Ga self-test) * scale factor
|
|
||||||
*
|
|
||||||
* and consequently:
|
|
||||||
*
|
|
||||||
* 1.1 Ga = (excited - normal) * scale factor
|
|
||||||
* scale factor = (excited - normal) / 1.1 Ga
|
|
||||||
*
|
|
||||||
* sxy = (excited - normal) / 766 | for conf reg. B set to 0x60 / Gain = 3
|
|
||||||
* sz = (excited - normal) / 713 | for conf reg. B set to 0x60 / Gain = 3
|
|
||||||
*
|
|
||||||
* By subtracting the non-excited measurement the pure 1.1 Ga reading
|
|
||||||
* can be extracted and the sensitivity of all axes can be matched.
|
|
||||||
*
|
|
||||||
* SELF TEST OPERATION
|
|
||||||
* To check the HMC5883L for proper operation, a self test feature in incorporated
|
|
||||||
* in which the sensor offset straps are excited to create a nominal field strength
|
|
||||||
* (bias field) to be measured. To implement self test, the least significant bits
|
|
||||||
* (MS1 and MS0) of configuration register A are changed from 00 to 01 (positive bias)
|
|
||||||
* or 10 (negetive bias), e.g. 0x11 or 0x12.
|
|
||||||
* Then, by placing the mode register into single-measurement mode (0x01),
|
|
||||||
* two data acquisition cycles will be made on each magnetic vector.
|
|
||||||
* The first acquisition will be a set pulse followed shortly by measurement
|
|
||||||
* data of the external field. The second acquisition will have the offset strap
|
|
||||||
* excited (about 10 mA) in the positive bias mode for X, Y, and Z axes to create
|
|
||||||
* about a ±1.1 gauss self test field plus the external field. The first acquisition
|
|
||||||
* values will be subtracted from the second acquisition, and the net measurement
|
|
||||||
* will be placed into the data output registers.
|
|
||||||
* Since self test adds ~1.1 Gauss additional field to the existing field strength,
|
|
||||||
* using a reduced gain setting prevents sensor from being saturated and data registers
|
|
||||||
* overflowed. For example, if the configuration register B is set to 0x60 (Gain=3),
|
|
||||||
* values around +766 LSB (1.16 Ga * 660 LSB/Ga) will be placed in the X and Y data
|
|
||||||
* output registers and around +713 (1.08 Ga * 660 LSB/Ga) will be placed in Z data
|
|
||||||
* output register. To leave the self test mode, change MS1 and MS0 bit of the
|
|
||||||
* configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10.
|
|
||||||
* Using the self test method described above, the user can scale sensor
|
|
||||||
*/
|
|
||||||
int calibrate(enum HMC5883_BUS busid)
|
|
||||||
{
|
|
||||||
int ret;
|
|
||||||
struct hmc5883_bus_option &bus = find_bus(busid);
|
|
||||||
const char *path = bus.devpath;
|
|
||||||
|
|
||||||
int fd = open(path, O_RDONLY);
|
|
||||||
|
|
||||||
if (fd < 0) {
|
|
||||||
err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) {
|
|
||||||
warnx("failed to enable sensor calibration mode");
|
|
||||||
}
|
|
||||||
|
|
||||||
close(fd);
|
|
||||||
|
|
||||||
return ret;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Reset the driver.
|
|
||||||
*/
|
|
||||||
void
|
|
||||||
reset(enum HMC5883_BUS busid)
|
|
||||||
{
|
|
||||||
struct hmc5883_bus_option &bus = find_bus(busid);
|
|
||||||
const char *path = bus.devpath;
|
|
||||||
|
|
||||||
int fd = open(path, O_RDONLY);
|
|
||||||
|
|
||||||
if (fd < 0) {
|
|
||||||
err(1, "failed ");
|
|
||||||
}
|
|
||||||
|
|
||||||
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
|
|
||||||
err(1, "driver reset failed");
|
|
||||||
}
|
|
||||||
|
|
||||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
|
|
||||||
err(1, "driver poll restart failed");
|
|
||||||
}
|
|
||||||
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
/**
|
|
||||||
* enable/disable temperature compensation
|
|
||||||
*/
|
|
||||||
int
|
|
||||||
temp_enable(enum HMC5883_BUS busid, bool enable)
|
|
||||||
{
|
|
||||||
struct hmc5883_bus_option &bus = find_bus(busid);
|
|
||||||
const char *path = bus.devpath;
|
|
||||||
|
|
||||||
int fd = open(path, O_RDONLY);
|
|
||||||
|
|
||||||
if (fd < 0) {
|
|
||||||
err(1, "failed ");
|
|
||||||
}
|
|
||||||
|
|
||||||
if (ioctl(fd, MAGIOCSTEMPCOMP, (unsigned)enable) < 0) {
|
|
||||||
err(1, "set temperature compensation failed");
|
|
||||||
}
|
|
||||||
|
|
||||||
close(fd);
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Print a little info about the driver.
|
|
||||||
*/
|
|
||||||
int
|
|
||||||
info(enum HMC5883_BUS busid)
|
|
||||||
{
|
|
||||||
struct hmc5883_bus_option &bus = find_bus(busid);
|
|
||||||
|
|
||||||
warnx("running on bus: %u (%s)\n", (unsigned)bus.busid, bus.devpath);
|
|
||||||
bus.dev->print_info();
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
|
||||||
usage()
|
|
||||||
{
|
|
||||||
warnx("missing command: try 'start', 'info', 'test', 'reset', 'info', 'calibrate'");
|
|
||||||
warnx("options:");
|
|
||||||
warnx(" -R rotation");
|
|
||||||
warnx(" -C calibrate on start");
|
|
||||||
warnx(" -X only external bus");
|
|
||||||
#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC)
|
|
||||||
warnx(" -I only internal bus");
|
|
||||||
#endif
|
|
||||||
}
|
|
||||||
|
|
||||||
} // namespace
|
|
||||||
|
|
||||||
int
|
|
||||||
hmc5883_main(int argc, char *argv[])
|
|
||||||
{
|
|
||||||
int myoptind = 1;
|
|
||||||
int ch;
|
|
||||||
const char *myoptarg = nullptr;
|
|
||||||
|
|
||||||
|
|
||||||
enum HMC5883_BUS busid = HMC5883_BUS_ALL;
|
|
||||||
enum Rotation rotation = ROTATION_NONE;
|
|
||||||
bool calibrate = false;
|
|
||||||
bool temp_compensation = false;
|
|
||||||
|
|
||||||
if (argc < 2) {
|
|
||||||
hmc5883::usage();
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
while ((ch = px4_getopt(argc, argv, "XISR:CT", &myoptind, &myoptarg)) != EOF) {
|
|
||||||
switch (ch) {
|
|
||||||
case 'R':
|
|
||||||
rotation = (enum Rotation)atoi(myoptarg);
|
|
||||||
break;
|
|
||||||
#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC)
|
|
||||||
|
|
||||||
case 'I':
|
|
||||||
busid = HMC5883_BUS_I2C_INTERNAL;
|
|
||||||
break;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
case 'X':
|
|
||||||
busid = HMC5883_BUS_I2C_EXTERNAL;
|
|
||||||
break;
|
|
||||||
|
|
||||||
case 'S':
|
|
||||||
busid = HMC5883_BUS_SPI;
|
|
||||||
break;
|
|
||||||
|
|
||||||
case 'C':
|
|
||||||
calibrate = true;
|
|
||||||
break;
|
|
||||||
|
|
||||||
case 'T':
|
|
||||||
temp_compensation = true;
|
|
||||||
break;
|
|
||||||
|
|
||||||
default:
|
|
||||||
hmc5883::usage();
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if (myoptind >= argc) {
|
|
||||||
hmc5883::usage();
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
const char *verb = argv[myoptind];
|
|
||||||
|
|
||||||
|
|
||||||
/*
|
|
||||||
* Start/load the driver.
|
|
||||||
*/
|
|
||||||
if (!strcmp(verb, "start")) {
|
|
||||||
hmc5883::start(busid, rotation);
|
|
||||||
|
|
||||||
if (calibrate && hmc5883::calibrate(busid) != 0) {
|
|
||||||
errx(1, "calibration failed");
|
|
||||||
}
|
|
||||||
|
|
||||||
if (temp_compensation) {
|
|
||||||
// we consider failing to setup temperature
|
|
||||||
// compensation as non-fatal
|
|
||||||
hmc5883::temp_enable(busid, true);
|
|
||||||
}
|
|
||||||
|
|
||||||
exit(0);
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
|
||||||
* Stop the driver
|
|
||||||
*/
|
|
||||||
if (!strcmp(verb, "stop")) {
|
|
||||||
return hmc5883::stop();
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
|
||||||
* Test the driver/device.
|
|
||||||
*/
|
|
||||||
if (!strcmp(verb, "test")) {
|
|
||||||
hmc5883::test(busid);
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
|
||||||
* Reset the driver.
|
|
||||||
*/
|
|
||||||
if (!strcmp(verb, "reset")) {
|
|
||||||
hmc5883::reset(busid);
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
|
||||||
* enable/disable temperature compensation
|
|
||||||
*/
|
|
||||||
if (!strcmp(verb, "tempoff")) {
|
|
||||||
hmc5883::temp_enable(busid, false);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (!strcmp(verb, "tempon")) {
|
|
||||||
hmc5883::temp_enable(busid, true);
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
|
||||||
* Print driver information.
|
|
||||||
*/
|
|
||||||
if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
|
|
||||||
hmc5883::info(busid);
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
|
||||||
* Autocalibrate the scaling
|
|
||||||
*/
|
|
||||||
if (!strcmp(verb, "calibrate")) {
|
|
||||||
if (hmc5883::calibrate(busid) == 0) {
|
|
||||||
errx(0, "calibration successful");
|
|
||||||
|
|
||||||
} else {
|
|
||||||
errx(1, "calibration failed");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
errx(1, "unrecognized command, try 'start', 'test', 'reset' 'calibrate', 'tempoff', 'tempon' or 'info'");
|
|
||||||
}
|
|
||||||
@@ -0,0 +1,299 @@
|
|||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* 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();
|
||||||
|
};
|
||||||
@@ -39,6 +39,8 @@
|
|||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include <drivers/device/Device.hpp>
|
||||||
|
|
||||||
#define ADDR_ID_A 0x0a
|
#define ADDR_ID_A 0x0a
|
||||||
#define ADDR_ID_B 0x0b
|
#define ADDR_ID_B 0x0b
|
||||||
#define ADDR_ID_C 0x0c
|
#define ADDR_ID_C 0x0c
|
||||||
|
|||||||
@@ -37,28 +37,13 @@
|
|||||||
* I2C interface for HMC5883 / HMC 5983
|
* I2C interface for HMC5883 / HMC 5983
|
||||||
*/
|
*/
|
||||||
|
|
||||||
/* XXX trim includes */
|
|
||||||
#include <px4_platform_common/px4_config.h>
|
#include <px4_platform_common/px4_config.h>
|
||||||
|
|
||||||
#include <sys/types.h>
|
|
||||||
#include <stdint.h>
|
|
||||||
#include <stdbool.h>
|
|
||||||
#include <string.h>
|
|
||||||
#include <assert.h>
|
|
||||||
#include <debug.h>
|
|
||||||
#include <errno.h>
|
|
||||||
#include <unistd.h>
|
|
||||||
|
|
||||||
#include <arch/board/board.h>
|
|
||||||
|
|
||||||
#include <drivers/device/i2c.h>
|
#include <drivers/device/i2c.h>
|
||||||
#include <drivers/drv_mag.h>
|
#include <drivers/drv_mag.h>
|
||||||
#include <drivers/drv_device.h>
|
#include <drivers/drv_device.h>
|
||||||
|
|
||||||
#include "hmc5883.h"
|
#include "hmc5883.h"
|
||||||
|
|
||||||
#include "board_config.h"
|
|
||||||
|
|
||||||
#define HMC5883L_ADDRESS 0x1E
|
#define HMC5883L_ADDRESS 0x1E
|
||||||
|
|
||||||
device::Device *HMC5883_I2C_interface(int bus);
|
device::Device *HMC5883_I2C_interface(int bus);
|
||||||
|
|||||||
@@ -0,0 +1,448 @@
|
|||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2012-2015 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 hmc5883.cpp
|
||||||
|
*
|
||||||
|
* Driver for the HMC5883 / HMC5983 magnetometer connected via I2C or SPI.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <px4_platform_common/getopt.h>
|
||||||
|
|
||||||
|
#include "HMC5883.hpp"
|
||||||
|
#include "hmc5883.h"
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Driver 'main' command.
|
||||||
|
*/
|
||||||
|
extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]);
|
||||||
|
|
||||||
|
enum HMC5883_BUS {
|
||||||
|
HMC5883_BUS_ALL = 0,
|
||||||
|
HMC5883_BUS_I2C_INTERNAL,
|
||||||
|
HMC5883_BUS_I2C_EXTERNAL,
|
||||||
|
HMC5883_BUS_SPI
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Local functions in support of the shell command.
|
||||||
|
*/
|
||||||
|
namespace hmc5883
|
||||||
|
{
|
||||||
|
|
||||||
|
/*
|
||||||
|
list of supported bus configurations
|
||||||
|
*/
|
||||||
|
struct hmc5883_bus_option {
|
||||||
|
enum HMC5883_BUS busid;
|
||||||
|
const char *devpath;
|
||||||
|
HMC5883_constructor interface_constructor;
|
||||||
|
uint8_t busnum;
|
||||||
|
HMC5883 *dev;
|
||||||
|
} bus_options[] = {
|
||||||
|
{ HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION, NULL },
|
||||||
|
#ifdef PX4_I2C_BUS_EXPANSION1
|
||||||
|
{ HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext1", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION1, NULL },
|
||||||
|
#endif
|
||||||
|
#ifdef PX4_I2C_BUS_EXPANSION2
|
||||||
|
{ HMC5883_BUS_I2C_EXTERNAL, "/dev/hmc5883_ext2", &HMC5883_I2C_interface, PX4_I2C_BUS_EXPANSION2, NULL },
|
||||||
|
#endif
|
||||||
|
#ifdef PX4_I2C_BUS_ONBOARD
|
||||||
|
{ HMC5883_BUS_I2C_INTERNAL, "/dev/hmc5883_int", &HMC5883_I2C_interface, PX4_I2C_BUS_ONBOARD, NULL },
|
||||||
|
#endif
|
||||||
|
#ifdef PX4_SPIDEV_HMC
|
||||||
|
{ HMC5883_BUS_SPI, "/dev/hmc5883_spi", &HMC5883_SPI_interface, PX4_SPI_BUS_SENSORS, NULL },
|
||||||
|
#endif
|
||||||
|
};
|
||||||
|
#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
|
||||||
|
|
||||||
|
/**
|
||||||
|
* start driver for a specific bus option
|
||||||
|
*/
|
||||||
|
static bool
|
||||||
|
start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation)
|
||||||
|
{
|
||||||
|
if (bus.dev != nullptr) {
|
||||||
|
PX4_ERR("bus option already started");
|
||||||
|
}
|
||||||
|
|
||||||
|
device::Device *interface = bus.interface_constructor(bus.busnum);
|
||||||
|
|
||||||
|
if (interface->init() != OK) {
|
||||||
|
delete interface;
|
||||||
|
PX4_WARN("no device on bus %u (type: %u)", (unsigned)bus.busnum, (unsigned)bus.busid);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
bus.dev = new HMC5883(interface, bus.devpath, rotation);
|
||||||
|
|
||||||
|
if (bus.dev != nullptr && OK != bus.dev->init()) {
|
||||||
|
delete bus.dev;
|
||||||
|
bus.dev = NULL;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
int fd = px4_open(bus.devpath, O_RDONLY);
|
||||||
|
|
||||||
|
if (fd < 0) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
|
||||||
|
px4_close(fd);
|
||||||
|
PX4_ERR("Failed to setup poll rate");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
px4_close(fd);
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Start the driver.
|
||||||
|
*
|
||||||
|
* This function call only returns once the driver
|
||||||
|
* is either successfully up and running or failed to start.
|
||||||
|
*/
|
||||||
|
static int
|
||||||
|
start(enum HMC5883_BUS busid, enum Rotation rotation)
|
||||||
|
{
|
||||||
|
bool started = false;
|
||||||
|
|
||||||
|
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
|
||||||
|
if (busid == HMC5883_BUS_ALL && bus_options[i].dev != NULL) {
|
||||||
|
// this device is already started
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (busid != HMC5883_BUS_ALL && bus_options[i].busid != busid) {
|
||||||
|
// not the one that is asked for
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
started |= start_bus(bus_options[i], rotation);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!started) {
|
||||||
|
return PX4_ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
return PX4_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
static int
|
||||||
|
stop()
|
||||||
|
{
|
||||||
|
bool stopped = false;
|
||||||
|
|
||||||
|
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
|
||||||
|
if (bus_options[i].dev != nullptr) {
|
||||||
|
bus_options[i].dev->stop();
|
||||||
|
delete bus_options[i].dev;
|
||||||
|
bus_options[i].dev = nullptr;
|
||||||
|
stopped = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return !stopped;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* find a bus structure for a busid
|
||||||
|
*/
|
||||||
|
static struct hmc5883_bus_option *
|
||||||
|
find_bus(enum HMC5883_BUS busid)
|
||||||
|
{
|
||||||
|
for (unsigned i = 0; i < NUM_BUS_OPTIONS; i++) {
|
||||||
|
if ((busid == HMC5883_BUS_ALL ||
|
||||||
|
busid == bus_options[i].busid) && bus_options[i].dev != NULL) {
|
||||||
|
return &bus_options[i];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
PX4_ERR("bus %u not started", (unsigned)busid);
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Automatic scale calibration.
|
||||||
|
*
|
||||||
|
* Basic idea:
|
||||||
|
*
|
||||||
|
* output = (ext field +- 1.1 Ga self-test) * scale factor
|
||||||
|
*
|
||||||
|
* and consequently:
|
||||||
|
*
|
||||||
|
* 1.1 Ga = (excited - normal) * scale factor
|
||||||
|
* scale factor = (excited - normal) / 1.1 Ga
|
||||||
|
*
|
||||||
|
* sxy = (excited - normal) / 766 | for conf reg. B set to 0x60 / Gain = 3
|
||||||
|
* sz = (excited - normal) / 713 | for conf reg. B set to 0x60 / Gain = 3
|
||||||
|
*
|
||||||
|
* By subtracting the non-excited measurement the pure 1.1 Ga reading
|
||||||
|
* can be extracted and the sensitivity of all axes can be matched.
|
||||||
|
*
|
||||||
|
* SELF TEST OPERATION
|
||||||
|
* To check the HMC5883L for proper operation, a self test feature in incorporated
|
||||||
|
* in which the sensor offset straps are excited to create a nominal field strength
|
||||||
|
* (bias field) to be measured. To implement self test, the least significant bits
|
||||||
|
* (MS1 and MS0) of configuration register A are changed from 00 to 01 (positive bias)
|
||||||
|
* or 10 (negetive bias), e.g. 0x11 or 0x12.
|
||||||
|
* Then, by placing the mode register into single-measurement mode (0x01),
|
||||||
|
* two data acquisition cycles will be made on each magnetic vector.
|
||||||
|
* The first acquisition will be a set pulse followed shortly by measurement
|
||||||
|
* data of the external field. The second acquisition will have the offset strap
|
||||||
|
* excited (about 10 mA) in the positive bias mode for X, Y, and Z axes to create
|
||||||
|
* about a ±1.1 gauss self test field plus the external field. The first acquisition
|
||||||
|
* values will be subtracted from the second acquisition, and the net measurement
|
||||||
|
* will be placed into the data output registers.
|
||||||
|
* Since self test adds ~1.1 Gauss additional field to the existing field strength,
|
||||||
|
* using a reduced gain setting prevents sensor from being saturated and data registers
|
||||||
|
* overflowed. For example, if the configuration register B is set to 0x60 (Gain=3),
|
||||||
|
* values around +766 LSB (1.16 Ga * 660 LSB/Ga) will be placed in the X and Y data
|
||||||
|
* output registers and around +713 (1.08 Ga * 660 LSB/Ga) will be placed in Z data
|
||||||
|
* output register. To leave the self test mode, change MS1 and MS0 bit of the
|
||||||
|
* configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10.
|
||||||
|
* Using the self test method described above, the user can scale sensor
|
||||||
|
*/
|
||||||
|
static int calibrate(enum HMC5883_BUS busid)
|
||||||
|
{
|
||||||
|
struct hmc5883_bus_option *bus = find_bus(busid);
|
||||||
|
|
||||||
|
if (bus == nullptr) {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
const char *path = bus->devpath;
|
||||||
|
|
||||||
|
int fd = px4_open(path, O_RDONLY);
|
||||||
|
|
||||||
|
if (fd < 0) {
|
||||||
|
PX4_ERR("%s open failed (try 'hmc5883 start' if the driver is not running", path);
|
||||||
|
return PX4_ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
int ret = 0;
|
||||||
|
|
||||||
|
if (OK != (ret = px4_ioctl(fd, MAGIOCCALIBRATE, fd))) {
|
||||||
|
PX4_ERR("failed to enable sensor calibration mode");
|
||||||
|
}
|
||||||
|
|
||||||
|
px4_close(fd);
|
||||||
|
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* enable/disable temperature compensation
|
||||||
|
*/
|
||||||
|
static int
|
||||||
|
temp_enable(enum HMC5883_BUS busid, bool enable)
|
||||||
|
{
|
||||||
|
struct hmc5883_bus_option *bus = find_bus(busid);
|
||||||
|
|
||||||
|
if (bus == nullptr) {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
const char *path = bus->devpath;
|
||||||
|
|
||||||
|
int fd = px4_open(path, O_RDONLY);
|
||||||
|
|
||||||
|
if (fd < 0) {
|
||||||
|
PX4_ERR("failed ");
|
||||||
|
return PX4_ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (px4_ioctl(fd, MAGIOCSTEMPCOMP, (unsigned)enable) < 0) {
|
||||||
|
PX4_ERR("set temperature compensation failed");
|
||||||
|
return PX4_ERROR;
|
||||||
|
}
|
||||||
|
|
||||||
|
px4_close(fd);
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Print a little info about the driver.
|
||||||
|
*/
|
||||||
|
static int
|
||||||
|
info(enum HMC5883_BUS busid)
|
||||||
|
{
|
||||||
|
struct hmc5883_bus_option *bus = find_bus(busid);
|
||||||
|
|
||||||
|
if (bus == nullptr) {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (bus != nullptr) {
|
||||||
|
PX4_INFO("running on bus: %u (%s)\n", (unsigned)bus->busid, bus->devpath);
|
||||||
|
bus->dev->print_info();
|
||||||
|
}
|
||||||
|
|
||||||
|
return PX4_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
static int
|
||||||
|
usage()
|
||||||
|
{
|
||||||
|
warnx("missing command: try 'start', 'info', 'calibrate'");
|
||||||
|
warnx("options:");
|
||||||
|
warnx(" -R rotation");
|
||||||
|
warnx(" -C calibrate on start");
|
||||||
|
warnx(" -X only external bus");
|
||||||
|
#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC)
|
||||||
|
warnx(" -I only internal bus");
|
||||||
|
#endif
|
||||||
|
return PX4_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
int
|
||||||
|
hmc5883_main(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
int myoptind = 1;
|
||||||
|
int ch;
|
||||||
|
const char *myoptarg = nullptr;
|
||||||
|
|
||||||
|
enum HMC5883_BUS busid = HMC5883_BUS_ALL;
|
||||||
|
enum Rotation rotation = ROTATION_NONE;
|
||||||
|
bool calibrate = false;
|
||||||
|
bool temp_compensation = false;
|
||||||
|
|
||||||
|
if (argc < 2) {
|
||||||
|
return hmc5883::usage();
|
||||||
|
}
|
||||||
|
|
||||||
|
while ((ch = px4_getopt(argc, argv, "XISR:CT", &myoptind, &myoptarg)) != EOF) {
|
||||||
|
switch (ch) {
|
||||||
|
case 'R':
|
||||||
|
rotation = (enum Rotation)atoi(myoptarg);
|
||||||
|
break;
|
||||||
|
#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC)
|
||||||
|
|
||||||
|
case 'I':
|
||||||
|
busid = HMC5883_BUS_I2C_INTERNAL;
|
||||||
|
break;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
case 'X':
|
||||||
|
busid = HMC5883_BUS_I2C_EXTERNAL;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 'S':
|
||||||
|
busid = HMC5883_BUS_SPI;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 'C':
|
||||||
|
calibrate = true;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 'T':
|
||||||
|
temp_compensation = true;
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
return hmc5883::usage();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (myoptind >= argc) {
|
||||||
|
return hmc5883::usage();
|
||||||
|
}
|
||||||
|
|
||||||
|
const char *verb = argv[myoptind];
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Start/load the driver.
|
||||||
|
*/
|
||||||
|
if (!strcmp(verb, "start")) {
|
||||||
|
hmc5883::start(busid, rotation);
|
||||||
|
|
||||||
|
if (calibrate && hmc5883::calibrate(busid) != 0) {
|
||||||
|
PX4_ERR("calibration failed");
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (temp_compensation) {
|
||||||
|
// we consider failing to setup temperature
|
||||||
|
// compensation as non-fatal
|
||||||
|
hmc5883::temp_enable(busid, true);
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Stop the driver
|
||||||
|
*/
|
||||||
|
if (!strcmp(verb, "stop")) {
|
||||||
|
return hmc5883::stop();
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* enable/disable temperature compensation
|
||||||
|
*/
|
||||||
|
if (!strcmp(verb, "tempoff")) {
|
||||||
|
return hmc5883::temp_enable(busid, false);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!strcmp(verb, "tempon")) {
|
||||||
|
return hmc5883::temp_enable(busid, true);
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Print driver information.
|
||||||
|
*/
|
||||||
|
if (!strcmp(verb, "info") || !strcmp(verb, "status")) {
|
||||||
|
return hmc5883::info(busid);
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Autocalibrate the scaling
|
||||||
|
*/
|
||||||
|
if (!strcmp(verb, "calibrate")) {
|
||||||
|
if (hmc5883::calibrate(busid) == 0) {
|
||||||
|
PX4_INFO("calibration successful");
|
||||||
|
return 0;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
PX4_ERR("calibration failed");
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return hmc5883::usage();
|
||||||
|
}
|
||||||
@@ -37,26 +37,12 @@
|
|||||||
* SPI interface for HMC5983
|
* SPI interface for HMC5983
|
||||||
*/
|
*/
|
||||||
|
|
||||||
/* XXX trim includes */
|
|
||||||
#include <px4_platform_common/px4_config.h>
|
#include <px4_platform_common/px4_config.h>
|
||||||
|
|
||||||
#include <sys/types.h>
|
|
||||||
#include <stdint.h>
|
|
||||||
#include <stdbool.h>
|
|
||||||
#include <string.h>
|
|
||||||
#include <assert.h>
|
|
||||||
#include <debug.h>
|
|
||||||
#include <errno.h>
|
|
||||||
#include <unistd.h>
|
|
||||||
|
|
||||||
#include <arch/board/board.h>
|
|
||||||
|
|
||||||
#include <drivers/device/spi.h>
|
#include <drivers/device/spi.h>
|
||||||
#include <drivers/drv_mag.h>
|
#include <drivers/drv_mag.h>
|
||||||
#include <drivers/drv_device.h>
|
#include <drivers/drv_device.h>
|
||||||
|
|
||||||
#include "hmc5883.h"
|
#include "hmc5883.h"
|
||||||
#include <board_config.h>
|
|
||||||
|
|
||||||
#ifdef PX4_SPIDEV_HMC
|
#ifdef PX4_SPIDEV_HMC
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user