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 */