From 8ebde51648f0afcc123736ec2cb12537ecabe124 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 13 Mar 2020 09:05:36 +0100 Subject: [PATCH] refactor hmc5883: use driver base class Removes the calibration on startup, as these values were overwritten by the system calibration values anyway. So the only difference is that if all calibration scales were equal to 1, the driver startup would have failed. --- .../airmind/mindpx-v2/init/rc.board_sensors | 4 +- boards/av/x-v1/init/rc.board_sensors | 2 +- .../holybro/durandal-v1/init/rc.board_sensors | 2 +- boards/holybro/kakutef7/init/rc.board_sensors | 2 +- boards/modalai/fc-v1/init/rc.board_sensors | 2 +- boards/mro/ctrl-zero-f7/init/rc.board_sensors | 2 +- boards/mro/x21-777/init/rc.board_sensors | 4 +- boards/mro/x21/init/rc.board_sensors | 2 +- boards/nxp/fmuk66-v3/init/rc.board_sensors | 2 +- boards/nxp/fmurt1062-v1/init/rc.board_sensors | 2 +- boards/px4/fmu-v2/init/rc.board_sensors | 4 +- boards/px4/fmu-v3/init/rc.board_sensors | 4 +- boards/px4/fmu-v4/init/rc.board_sensors | 4 +- boards/px4/fmu-v4pro/init/rc.board_sensors | 2 +- boards/px4/fmu-v5/init/rc.board_sensors | 2 +- boards/px4/fmu-v5x/init/rc.board_sensors | 2 +- posix-configs/ocpoc/px4.config | 2 +- src/drivers/magnetometer/hmc5883/HMC5883.cpp | 95 ++-- src/drivers/magnetometer/hmc5883/HMC5883.hpp | 44 +- src/drivers/magnetometer/hmc5883/hmc5883.h | 6 +- .../magnetometer/hmc5883/hmc5883_i2c.cpp | 12 +- .../magnetometer/hmc5883/hmc5883_main.cpp | 410 +++--------------- .../magnetometer/hmc5883/hmc5883_spi.cpp | 16 +- 23 files changed, 167 insertions(+), 460 deletions(-) diff --git a/boards/airmind/mindpx-v2/init/rc.board_sensors b/boards/airmind/mindpx-v2/init/rc.board_sensors index 73c25931a2..877f330333 100644 --- a/boards/airmind/mindpx-v2/init/rc.board_sensors +++ b/boards/airmind/mindpx-v2/init/rc.board_sensors @@ -6,11 +6,11 @@ adc start # External I2C bus -hmc5883 -C -T -X start +hmc5883 -T -X start qmc5883 -X start # Internal I2C bus -hmc5883 -C -T -I -R 12 start +hmc5883 -T -I -R 12 start mpu6000 -s -R 8 start mpu9250 -s -R 8 start diff --git a/boards/av/x-v1/init/rc.board_sensors b/boards/av/x-v1/init/rc.board_sensors index 1675c87db1..6aa46b14ea 100644 --- a/boards/av/x-v1/init/rc.board_sensors +++ b/boards/av/x-v1/init/rc.board_sensors @@ -22,5 +22,5 @@ fi # Possible external compasses ist8310 -X start -hmc5883 -C -T -X start +hmc5883 -T -X start qmc5883 -X start diff --git a/boards/holybro/durandal-v1/init/rc.board_sensors b/boards/holybro/durandal-v1/init/rc.board_sensors index d8c8b76306..668ccb02e8 100644 --- a/boards/holybro/durandal-v1/init/rc.board_sensors +++ b/boards/holybro/durandal-v1/init/rc.board_sensors @@ -15,7 +15,7 @@ bmi088 -G -R 10 start # Possible external compasses ist8310 -X start -hmc5883 -C -T -X start +hmc5883 -T -X start qmc5883 -X start lis3mdl -X start diff --git a/boards/holybro/kakutef7/init/rc.board_sensors b/boards/holybro/kakutef7/init/rc.board_sensors index a1328d90db..0838ccb0bb 100644 --- a/boards/holybro/kakutef7/init/rc.board_sensors +++ b/boards/holybro/kakutef7/init/rc.board_sensors @@ -16,5 +16,5 @@ bmp280 -X start # Possible external compasses ist8310 -X start -hmc5883 -C -T -X start +hmc5883 -T -X start qmc5883 -X start diff --git a/boards/modalai/fc-v1/init/rc.board_sensors b/boards/modalai/fc-v1/init/rc.board_sensors index 2bc2646063..b2a59a1680 100644 --- a/boards/modalai/fc-v1/init/rc.board_sensors +++ b/boards/modalai/fc-v1/init/rc.board_sensors @@ -23,7 +23,7 @@ bmi088 -G -R 4 start # Possible external compasses ist8310 -X start -hmc5883 -C -T -X start +hmc5883 -T -X start qmc5883 -X start # Internal I2C Baro diff --git a/boards/mro/ctrl-zero-f7/init/rc.board_sensors b/boards/mro/ctrl-zero-f7/init/rc.board_sensors index 3a7e294210..f13e8f887a 100644 --- a/boards/mro/ctrl-zero-f7/init/rc.board_sensors +++ b/boards/mro/ctrl-zero-f7/init/rc.board_sensors @@ -22,6 +22,6 @@ dps310 -s start # Possible external compasses ist8310 -X start -hmc5883 -C -T -X start +hmc5883 -T -X start qmc5883 -X start lis3mdl -X start diff --git a/boards/mro/x21-777/init/rc.board_sensors b/boards/mro/x21-777/init/rc.board_sensors index d097a99040..85f6735ed7 100644 --- a/boards/mro/x21-777/init/rc.board_sensors +++ b/boards/mro/x21-777/init/rc.board_sensors @@ -6,7 +6,7 @@ adc start # External I2C bus -hmc5883 -C -T -X start +hmc5883 -T -X start lis3mdl -X start # Internal SPI bus ICM-20608-G is rotated 90 deg yaw @@ -20,6 +20,6 @@ mpu9250 -R 2 start # Possible external compasses ist8310 -X start -hmc5883 -C -T -X start +hmc5883 -T -X start qmc5883 -X start lis3mdl -X start diff --git a/boards/mro/x21/init/rc.board_sensors b/boards/mro/x21/init/rc.board_sensors index fcae7ba099..23c091eca3 100644 --- a/boards/mro/x21/init/rc.board_sensors +++ b/boards/mro/x21/init/rc.board_sensors @@ -6,7 +6,7 @@ adc start # External I2C bus -hmc5883 -C -T -X start +hmc5883 -T -X start lis3mdl -X start ist8310 -X start qmc5883 -X start diff --git a/boards/nxp/fmuk66-v3/init/rc.board_sensors b/boards/nxp/fmuk66-v3/init/rc.board_sensors index e5c7b4eef0..6b98b2ae52 100644 --- a/boards/nxp/fmuk66-v3/init/rc.board_sensors +++ b/boards/nxp/fmuk66-v3/init/rc.board_sensors @@ -6,7 +6,7 @@ adc start # Possible external compasses -hmc5883 -C -X start +hmc5883 -X start lis3mdl -X start ist8310 -X start qmc5883 -X start diff --git a/boards/nxp/fmurt1062-v1/init/rc.board_sensors b/boards/nxp/fmurt1062-v1/init/rc.board_sensors index 5c54d39c45..3cf7ce88d9 100644 --- a/boards/nxp/fmurt1062-v1/init/rc.board_sensors +++ b/boards/nxp/fmurt1062-v1/init/rc.board_sensors @@ -31,7 +31,7 @@ bmi055 -G -R 10 -s start # Possible external compasses ist8310 -X start -hmc5883 -C -T -X start +hmc5883 -T -X start qmc5883 -X start lis3mdl -X start diff --git a/boards/px4/fmu-v2/init/rc.board_sensors b/boards/px4/fmu-v2/init/rc.board_sensors index b9cb6158f8..0b5317f3a2 100644 --- a/boards/px4/fmu-v2/init/rc.board_sensors +++ b/boards/px4/fmu-v2/init/rc.board_sensors @@ -8,12 +8,12 @@ rgbled start -I adc start # External I2C bus -hmc5883 -C -T -X start +hmc5883 -T -X start lis3mdl -X start ist8310 -X start # Internal I2C bus -hmc5883 -C -T -I -R 4 start +hmc5883 -T -I -R 4 start # Internal SPI bus ICM-20608-G mpu6000 -s -T 20608 start diff --git a/boards/px4/fmu-v3/init/rc.board_sensors b/boards/px4/fmu-v3/init/rc.board_sensors index 207043b0ee..99ca3f4b59 100644 --- a/boards/px4/fmu-v3/init/rc.board_sensors +++ b/boards/px4/fmu-v3/init/rc.board_sensors @@ -8,13 +8,13 @@ rgbled start -I adc start # External I2C bus -hmc5883 -C -T -X start +hmc5883 -T -X start lis3mdl -X start ist8310 -X start qmc5883 -X start # Internal I2C bus -hmc5883 -C -T -I -R 4 start +hmc5883 -T -I -R 4 start # Internal SPI bus ICM-20608-G mpu6000 -s -T 20608 start diff --git a/boards/px4/fmu-v4/init/rc.board_sensors b/boards/px4/fmu-v4/init/rc.board_sensors index 4ad07cd44f..cc1ddfc865 100644 --- a/boards/px4/fmu-v4/init/rc.board_sensors +++ b/boards/px4/fmu-v4/init/rc.board_sensors @@ -15,7 +15,7 @@ fmu sensor_reset 50 # External I2C bus -hmc5883 -C -T -X start +hmc5883 -T -X start lis3mdl -X start ist8310 -X start qmc5883 -X start @@ -32,7 +32,7 @@ then fi # hmc5883 internal SPI bus is rotated 90 deg yaw -if ! hmc5883 -C -T -S -R 2 start +if ! hmc5883 -T -S -R 2 start then # lis3mdl internal SPI bus is rotated 90 deg yaw lis3mdl start diff --git a/boards/px4/fmu-v4pro/init/rc.board_sensors b/boards/px4/fmu-v4pro/init/rc.board_sensors index f3c13deb7f..a208c8ad76 100644 --- a/boards/px4/fmu-v4pro/init/rc.board_sensors +++ b/boards/px4/fmu-v4pro/init/rc.board_sensors @@ -20,7 +20,7 @@ mpu9250 -R 2 start lis3mdl -R 0 start # Possible external compasses -hmc5883 -C -T -X start +hmc5883 -T -X start qmc5883 -X start # RM3100 diff --git a/boards/px4/fmu-v5/init/rc.board_sensors b/boards/px4/fmu-v5/init/rc.board_sensors index a02d31b17c..5576bd107a 100644 --- a/boards/px4/fmu-v5/init/rc.board_sensors +++ b/boards/px4/fmu-v5/init/rc.board_sensors @@ -25,7 +25,7 @@ bmi055 -G -R 10 -s start # Possible external compasses ist8310 -X start -hmc5883 -C -T -X start +hmc5883 -T -X start qmc5883 -X start lis3mdl -X start diff --git a/boards/px4/fmu-v5x/init/rc.board_sensors b/boards/px4/fmu-v5x/init/rc.board_sensors index 8518b91e79..bb03d57e8f 100644 --- a/boards/px4/fmu-v5x/init/rc.board_sensors +++ b/boards/px4/fmu-v5x/init/rc.board_sensors @@ -23,7 +23,7 @@ bmi088 -G -R 12 start # Possible external compasses ist8310 -X start -hmc5883 -C -T -X start +hmc5883 -T -X start qmc5883 -X start # Possible internal compass diff --git a/posix-configs/ocpoc/px4.config b/posix-configs/ocpoc/px4.config index e28150fa80..5c91ca04f5 100644 --- a/posix-configs/ocpoc/px4.config +++ b/posix-configs/ocpoc/px4.config @@ -16,7 +16,7 @@ param set MAV_TYPE 2 param set MAV_SYS_ID 1 mpu9250 -R 10 start -hmc5883 start +hmc5883 -I start ms5611 -s start rgbled start -b 1 diff --git a/src/drivers/magnetometer/hmc5883/HMC5883.cpp b/src/drivers/magnetometer/hmc5883/HMC5883.cpp index 91c37868a9..4acdf6a744 100644 --- a/src/drivers/magnetometer/hmc5883/HMC5883.cpp +++ b/src/drivers/magnetometer/hmc5883/HMC5883.cpp @@ -33,9 +33,9 @@ #include "HMC5883.hpp" -HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rotation) : - CDev("HMC5883", path), - ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())), +HMC5883::HMC5883(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus) : + CDev("HMC5883", nullptr), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus), _interface(interface), _reports(nullptr), _scale{}, @@ -73,9 +73,6 @@ HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rota HMC5883::~HMC5883() { - /* make sure we are truly inactive */ - stop(); - if (_reports != nullptr) { delete _reports; } @@ -118,6 +115,10 @@ HMC5883::init() ret = OK; /* sensor is ok, but not calibrated */ _sensor_ok = true; + + _measure_interval = HMC5883_CONVERSION_INTERVAL; + start(); + out: return ret; } @@ -399,16 +400,6 @@ HMC5883::start() ScheduleNow(); } -void -HMC5883::stop() -{ - if (_measure_interval > 0) { - /* ensure no new items are queued while we cancel this one */ - _measure_interval = 0; - ScheduleClear(); - } -} - int HMC5883::reset() { @@ -417,7 +408,7 @@ HMC5883::reset() } void -HMC5883::Run() +HMC5883::RunImpl() { if (_measure_interval == 0) { return; @@ -676,6 +667,47 @@ out: return ret; } +/** + * 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 HMC5883::calibrate(cdev::file_t *filp, unsigned enable) { sensor_mag_s report{}; @@ -713,7 +745,7 @@ int HMC5883::calibrate(cdev::file_t *filp, unsigned enable) /* start the sensor polling at 50 Hz */ if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) { - warn("FAILED: SENSORIOCSPOLLRATE 50Hz"); + PX4_ERR("FAILED: SENSORIOCSPOLLRATE 50Hz"); ret = 1; goto out; } @@ -721,25 +753,25 @@ int HMC5883::calibrate(cdev::file_t *filp, unsigned enable) /* Set to 2.5 Gauss. We ask for 3 to get the right part of * the chained if statement above. */ if (OK != ioctl(filp, MAGIOCSRANGE, 3)) { - warnx("FAILED: MAGIOCSRANGE 2.5 Ga"); + PX4_ERR("FAILED: MAGIOCSRANGE 2.5 Ga"); ret = 1; goto out; } if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) { - warnx("FAILED: MAGIOCEXSTRAP 1"); + PX4_ERR("FAILED: MAGIOCEXSTRAP 1"); ret = 1; goto out; } if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) { - warn("FAILED: MAGIOCGSCALE 1"); + PX4_ERR("FAILED: MAGIOCGSCALE 1"); ret = 1; goto out; } if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { - warn("FAILED: MAGIOCSSCALE 1"); + PX4_ERR("FAILED: MAGIOCSSCALE 1"); ret = 1; goto out; } @@ -754,7 +786,7 @@ int HMC5883::calibrate(cdev::file_t *filp, unsigned enable) ret = px4_poll(&fds, 1, 2000); if (ret != 1) { - warn("ERROR: TIMEOUT 1"); + PX4_ERR("ERROR: TIMEOUT 1"); goto out; } @@ -762,7 +794,7 @@ int HMC5883::calibrate(cdev::file_t *filp, unsigned enable) sz = px4_read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { - warn("ERROR: READ 1"); + PX4_ERR("ERROR: READ 1"); ret = -EIO; goto out; } @@ -778,7 +810,7 @@ int HMC5883::calibrate(cdev::file_t *filp, unsigned enable) ret = px4_poll(&fds, 1, 2000); if (ret != 1) { - warn("ERROR: TIMEOUT 2"); + PX4_ERR("ERROR: TIMEOUT 2"); goto out; } @@ -786,7 +818,7 @@ int HMC5883::calibrate(cdev::file_t *filp, unsigned enable) sz = px4_read(fd, &report, sizeof(report)); if (sz != sizeof(report)) { - warn("ERROR: READ 2"); + PX4_ERR("ERROR: READ 2"); ret = -EIO; goto out; } @@ -827,23 +859,23 @@ int HMC5883::calibrate(cdev::file_t *filp, unsigned enable) out: if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) { - warn("FAILED: MAGIOCSSCALE 2"); + PX4_ERR("FAILED: MAGIOCSSCALE 2"); } /* set back to normal mode */ /* Set to 1.9 Gauss */ if (OK != px4_ioctl(fd, MAGIOCSRANGE, 2)) { - warnx("FAILED: MAGIOCSRANGE 1.9 Ga"); + PX4_ERR("FAILED: MAGIOCSRANGE 1.9 Ga"); } if (OK != px4_ioctl(fd, MAGIOCEXSTRAP, 0)) { - warnx("FAILED: MAGIOCEXSTRAP 0"); + PX4_ERR("FAILED: MAGIOCEXSTRAP 0"); } if (ret == OK) { if (check_scale()) { /* failed */ - warnx("FAILED: SCALE"); + PX4_ERR("FAILED: SCALE"); ret = PX4_ERROR; } @@ -1011,8 +1043,9 @@ HMC5883::meas_to_float(uint8_t in[2]) } void -HMC5883::print_info() +HMC5883::print_status() { + I2CSPIDriverBase::print_status(); perf_print_counter(_sample_perf); perf_print_counter(_comms_errors); printf("interval: %u us\n", _measure_interval); diff --git a/src/drivers/magnetometer/hmc5883/HMC5883.hpp b/src/drivers/magnetometer/hmc5883/HMC5883.hpp index df86a5fa3d..da4348cb71 100644 --- a/src/drivers/magnetometer/hmc5883/HMC5883.hpp +++ b/src/drivers/magnetometer/hmc5883/HMC5883.hpp @@ -52,10 +52,9 @@ #include #include -#include +#include #include -#include #include #include @@ -65,7 +64,6 @@ #include #include -#include #include #include "hmc5883.h" @@ -110,32 +108,29 @@ #define HMC5983_TEMP_SENSOR_ENABLE (1 << 7) -class HMC5883 : public device::CDev, public px4::ScheduledWorkItem +class HMC5883 : public device::CDev, public I2CSPIDriver { public: - HMC5883(device::Device *interface, const char *path, enum Rotation rotation); + HMC5883(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus); virtual ~HMC5883(); - virtual int init(); + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); - 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); + void RunImpl(); - /** - * Stop the automatic measurement state machine. - */ - void stop(); + int init() override; - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); + ssize_t read(cdev::file_t *filp, char *buffer, size_t buflen) override; + int ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override; protected: - Device *_interface; + void print_status() override; private: + Device *_interface; unsigned _measure_interval{0}; ringbuffer::RingBuffer *_reports; @@ -230,21 +225,6 @@ private: */ 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. * diff --git a/src/drivers/magnetometer/hmc5883/hmc5883.h b/src/drivers/magnetometer/hmc5883/hmc5883.h index ec4e6a4eb6..2b46e753a1 100644 --- a/src/drivers/magnetometer/hmc5883/hmc5883.h +++ b/src/drivers/magnetometer/hmc5883/hmc5883.h @@ -40,6 +40,7 @@ #pragma once #include +#include #define ADDR_ID_A 0x0a #define ADDR_ID_B 0x0b @@ -50,6 +51,5 @@ #define ID_C_WHO_AM_I '3' /* interface factories */ -extern device::Device *HMC5883_SPI_interface(int bus); -extern device::Device *HMC5883_I2C_interface(int bus); -typedef device::Device *(*HMC5883_constructor)(int); +extern device::Device *HMC5883_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode); +extern device::Device *HMC5883_I2C_interface(int bus, int bus_frequency); diff --git a/src/drivers/magnetometer/hmc5883/hmc5883_i2c.cpp b/src/drivers/magnetometer/hmc5883/hmc5883_i2c.cpp index ecb99eb0d4..a431c42aca 100644 --- a/src/drivers/magnetometer/hmc5883/hmc5883_i2c.cpp +++ b/src/drivers/magnetometer/hmc5883/hmc5883_i2c.cpp @@ -46,12 +46,12 @@ #define HMC5883L_ADDRESS 0x1E -device::Device *HMC5883_I2C_interface(int bus); +device::Device *HMC5883_I2C_interface(int bus, int bus_frequency); class HMC5883_I2C : public device::I2C { public: - HMC5883_I2C(int bus); + HMC5883_I2C(int bus, int bus_frequency); virtual ~HMC5883_I2C() = default; virtual int read(unsigned address, void *data, unsigned count); @@ -65,13 +65,13 @@ protected: }; device::Device * -HMC5883_I2C_interface(int bus) +HMC5883_I2C_interface(int bus, int bus_frequency) { - return new HMC5883_I2C(bus); + return new HMC5883_I2C(bus, bus_frequency); } -HMC5883_I2C::HMC5883_I2C(int bus) : - I2C("HMC5883_I2C", nullptr, bus, HMC5883L_ADDRESS, 400000) +HMC5883_I2C::HMC5883_I2C(int bus, int bus_frequency) : + I2C("HMC5883_I2C", nullptr, bus, HMC5883L_ADDRESS, bus_frequency) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; } diff --git a/src/drivers/magnetometer/hmc5883/hmc5883_main.cpp b/src/drivers/magnetometer/hmc5883/hmc5883_main.cpp index b67046b8a1..52b30a523e 100644 --- a/src/drivers/magnetometer/hmc5883/hmc5883_main.cpp +++ b/src/drivers/magnetometer/hmc5883/hmc5883_main.cpp @@ -38,411 +38,109 @@ */ #include +#include #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 +I2CSPIDriverBase *HMC5883::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { + device::Device *interface = nullptr; -/* - 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])) + if (iterator.busType() == BOARD_I2C_BUS) { + interface = HMC5883_I2C_interface(iterator.bus(), cli.bus_frequency); -/** - * 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"); + } else if (iterator.busType() == BOARD_SPI_BUS) { + interface = HMC5883_SPI_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode); } - device::Device *interface = bus.interface_constructor(bus.busnum); + if (interface == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; + } if (interface->init() != OK) { delete interface; - PX4_WARN("no device on bus %u (type: %u)", (unsigned)bus.busnum, (unsigned)bus.busid); - return false; + PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + return nullptr; } - bus.dev = new HMC5883(interface, bus.devpath, rotation); + HMC5883 *dev = new HMC5883(interface, cli.rotation, iterator.configuredBusOption(), iterator.bus()); - if (bus.dev != nullptr && OK != bus.dev->init()) { - delete bus.dev; - bus.dev = NULL; - return false; + if (dev == nullptr) { + delete interface; + return nullptr; } - int fd = px4_open(bus.devpath, O_RDONLY); - - if (fd < 0) { - return false; + if (OK != dev->init()) { + delete dev; + return nullptr; } - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - px4_close(fd); - PX4_ERR("Failed to setup poll rate"); - return false; + bool enable_temp_compensation = cli.custom1 == 1; + + if (enable_temp_compensation) { + dev->set_temperature_compensation(1); } - px4_close(fd); - - return true; + return dev; } -/** - * 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) +void HMC5883::print_usage() { - 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; + PRINT_MODULE_USAGE_NAME("hmc5883", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true); + PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); + PRINT_MODULE_USAGE_PARAM_FLAG('T', "Enable temperature compensation", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -static int -stop() +extern "C" int hmc5883_main(int argc, char *argv[]) { - 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; + using ThisDriver = HMC5883; int ch; - const char *myoptarg = nullptr; + BusCLIArguments cli{true, true}; + cli.default_i2c_frequency = 400000; + cli.default_spi_frequency = 11 * 1000 * 1000; - 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) { + while ((ch = cli.getopt(argc, argv, "R:T")) != 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; + cli.rotation = (enum Rotation)atoi(cli.optarg()); break; case 'T': - temp_compensation = true; + cli.custom1 = 1; break; - - default: - return hmc5883::usage(); } } - if (myoptind >= argc) { - return hmc5883::usage(); + const char *verb = cli.optarg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; } - const char *verb = argv[myoptind]; + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_HMC5883); - /* - * 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; + return ThisDriver::module_start(cli, iterator); } - /* - * Stop the driver - */ if (!strcmp(verb, "stop")) { - return hmc5883::stop(); + return ThisDriver::module_stop(iterator); } - /* - * enable/disable temperature compensation - */ - if (!strcmp(verb, "tempoff")) { - return hmc5883::temp_enable(busid, false); + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); } - 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(); + ThisDriver::print_usage(); + return -1; } diff --git a/src/drivers/magnetometer/hmc5883/hmc5883_spi.cpp b/src/drivers/magnetometer/hmc5883/hmc5883_spi.cpp index b18ff02012..b632d8263a 100644 --- a/src/drivers/magnetometer/hmc5883/hmc5883_spi.cpp +++ b/src/drivers/magnetometer/hmc5883/hmc5883_spi.cpp @@ -44,8 +44,6 @@ #include "hmc5883.h" -#ifdef PX4_SPIDEV_HMC - /* SPI protocol address bits */ #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) @@ -54,12 +52,12 @@ #define HMC_MAX_SEND_LEN 4 #define HMC_MAX_RCV_LEN 8 -device::Device *HMC5883_SPI_interface(int bus); +device::Device *HMC5883_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode); class HMC5883_SPI : public device::SPI { public: - HMC5883_SPI(int bus, uint32_t device); + HMC5883_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode); virtual ~HMC5883_SPI() = default; virtual int init(); @@ -71,13 +69,13 @@ public: }; device::Device * -HMC5883_SPI_interface(int bus) +HMC5883_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode) { - return new HMC5883_SPI(bus, PX4_SPIDEV_HMC); + return new HMC5883_SPI(bus, devid, bus_frequency, spi_mode); } -HMC5883_SPI::HMC5883_SPI(int bus, uint32_t device) : - SPI("HMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000 /* will be rounded to 10.4 MHz */) +HMC5883_SPI::HMC5883_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) : + SPI("HMC5883_SPI", nullptr, bus, device, spi_mode, bus_frequency) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; } @@ -169,5 +167,3 @@ HMC5883_SPI::read(unsigned address, void *data, unsigned count) memcpy(data, &buf[1], count); return ret; } - -#endif /* PX4_SPIDEV_HMC */