diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index a2e9dcb3d7..6ba876c465 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -60,6 +60,7 @@ #define DRV_MAG_DEVTYPE_IST8310 0x06 #define DRV_MAG_DEVTYPE_RM3100 0x07 #define DRV_MAG_DEVTYPE_QMC5883 0x08 +#define DRV_MAG_DEVTYPE_AK09916 0x09 #define DRV_ACC_DEVTYPE_LSM303D 0x11 #define DRV_ACC_DEVTYPE_BMA180 0x12 #define DRV_ACC_DEVTYPE_MPU6000 0x13 diff --git a/src/drivers/magnetometer/ak09916/CMakeLists.txt b/src/drivers/magnetometer/ak09916/CMakeLists.txt index 51b89e3621..6bedb1c54e 100644 --- a/src/drivers/magnetometer/ak09916/CMakeLists.txt +++ b/src/drivers/magnetometer/ak09916/CMakeLists.txt @@ -39,5 +39,7 @@ px4_add_module( SRCS ak09916.cpp DEPENDS + drivers_magnetometer + px4_work_queue ) diff --git a/src/drivers/magnetometer/ak09916/ak09916.cpp b/src/drivers/magnetometer/ak09916/ak09916.cpp index 955f6c2858..ed4ca4a102 100644 --- a/src/drivers/magnetometer/ak09916/ak09916.cpp +++ b/src/drivers/magnetometer/ak09916/ak09916.cpp @@ -45,22 +45,11 @@ #include #include #include -#include -#include -#include -#include -#include -#include -#include #include - - -#include -#include -#include +#include #include "ak09916.hpp" -#include + /** driver 'main' command */ extern "C" { __EXPORT int ak09916_main(int argc, char *argv[]); } @@ -74,12 +63,9 @@ AK09916 *g_dev_ext; AK09916 *g_dev_int; void start(bool, enum Rotation); -void test(bool); -void reset(bool); void info(bool); void usage(); - /** * Start the driver. * @@ -88,7 +74,6 @@ void usage(); */ void start(bool external_bus, enum Rotation rotation) { - int fd; AK09916 **g_dev_ptr = (external_bus ? &g_dev_ext : &g_dev_int); const char *path = (external_bus ? AK09916_DEVICE_PATH_MAG_EXT : AK09916_DEVICE_PATH_MAG); @@ -122,20 +107,6 @@ void start(bool external_bus, enum Rotation rotation) goto fail; } - /* set the poll rate to default, starts automatic data collection */ - fd = open(path, O_RDONLY); - - if (fd < 0) { - goto fail; - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - close(fd); - PX4_ERR("Failed to setup poll rate"); - } - - close(fd); - exit(0); fail: @@ -146,72 +117,21 @@ fail: PX4_ERR("driver start failed"); exit(1); - } - -void test(bool external_bus) -{ - int fd = -1; - const char *path = (external_bus ? AK09916_DEVICE_PATH_MAG_EXT : AK09916_DEVICE_PATH_MAG); - struct mag_report m_report; - ssize_t sz; - - - /* get the driver */ - fd = open(path, O_RDONLY); - - if (fd < 0) { - PX4_ERR("%s open failed (try 'AK09916 start' if the driver is not running)", path); - exit(1); - } - - /* do a simple demand read */ - sz = read(fd, &m_report, sizeof(m_report)); - - if (sz != sizeof(m_report)) { - PX4_ERR("immediate mag read failed"); - exit(1); - } - - PX4_WARN("single read"); - PX4_WARN("time: %lld", m_report.timestamp); - PX4_WARN("mag x: \t%8.4f\t", (double)m_report.x); - PX4_WARN("mag y: \t%8.4f\t", (double)m_report.y); - PX4_WARN("mag z: \t%8.4f\t", (double)m_report.z); - PX4_WARN("mag x: \t%d\traw 0x%0x", (short)m_report.x_raw, (unsigned short)m_report.x_raw); - PX4_WARN("mag y: \t%d\traw 0x%0x", (short)m_report.y_raw, (unsigned short)m_report.y_raw); - PX4_WARN("mag z: \t%d\traw 0x%0x", (short)m_report.z_raw, (unsigned short)m_report.z_raw); - - PX4_ERR("PASS"); - exit(0); - -} - - void -reset(bool external_bus) +stop(bool external_bus) { - const char *path = external_bus ? AK09916_DEVICE_PATH_MAG_EXT : AK09916_DEVICE_PATH_MAG; - int fd = open(path, O_RDONLY); - - if (fd < 0) { - PX4_ERR("failed"); - exit(1); - } + AK09916 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; - if (ioctl(fd, SENSORIOCRESET, 0) < 0) { - PX4_ERR("driver reset failed"); + if (*g_dev_ptr == nullptr) { + PX4_ERR("driver not running"); exit(1); } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - PX4_ERR("driver poll restart failed"); - exit(1); - } + (*g_dev_ptr)->stop(); exit(0); - } void @@ -228,13 +148,12 @@ info(bool external_bus) (*g_dev_ptr)->print_info(); exit(0); - } void usage() { - PX4_WARN("missing command: try 'start', 'info', 'test', 'stop',\n'reset'"); + PX4_WARN("missing command: try 'start', 'info', stop'"); PX4_WARN("options:"); PX4_WARN(" -X (external bus)"); @@ -246,52 +165,20 @@ usage() // Otherwise, it will passthrough the parent AK09916 AK09916::AK09916(int bus, const char *path, enum Rotation rotation) : I2C("AK09916", path, bus, AK09916_I2C_ADDR, 400000), - _measure_ticks(0), - _rotation(rotation), - _mag_topic(nullptr), - _mag_orb_class_instance(-1), - _mag_class_instance(-1), - _mag_reading_data(false), - _mag_reports(nullptr), - _mag_scale{}, - _mag_range_scale(), + ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())), + _px4_mag(get_device_id(), ORB_PRIO_MAX, rotation), _mag_reads(perf_alloc(PC_COUNT, "ak09916_mag_reads")), _mag_errors(perf_alloc(PC_COUNT, "ak09916_mag_errors")), _mag_overruns(perf_alloc(PC_COUNT, "ak09916_mag_overruns")), _mag_overflows(perf_alloc(PC_COUNT, "ak09916_mag_overflows")), - _mag_duplicates(perf_alloc(PC_COUNT, "ak09916_mag_duplicates")), - _mag_asa_x(1.0), - _mag_asa_y(1.0), - _mag_asa_z(1.0), - _last_mag_data{} + _mag_duplicates(perf_alloc(PC_COUNT, "ak09916_mag_duplicates")) { - // default mag scale factors - _mag_scale.x_offset = 0; - _mag_scale.x_scale = 1.0f; - _mag_scale.y_offset = 0; - _mag_scale.y_scale = 1.0f; - _mag_scale.z_offset = 0; - _mag_scale.z_scale = 1.0f; - - _mag_range_scale = AK09916_MAG_RANGE_GA; - - - // work_cancel in the dtor will explode if we don't do this... - memset(&_work, 0, sizeof(_work)); + _px4_mag.set_device_type(DRV_MAG_DEVTYPE_AK09916); + _px4_mag.set_scale(AK09916_MAG_RANGE_GA); } AK09916::~AK09916() { - if (_mag_class_instance != -1) { - unregister_class_devname(MAG_BASE_DEVICE_PATH, _mag_class_instance); - } - - if (_mag_reports != nullptr) { - delete _mag_reports; - } - - orb_unadvertise(_mag_topic); - perf_free(_mag_reads); perf_free(_mag_errors); perf_free(_mag_overruns); @@ -311,30 +198,15 @@ AK09916::init() return ret; } - reset(); - - _mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); + ret = reset(); - if (_mag_reports == nullptr) { - return -ENOMEM; + if (ret != PX4_OK) { + return PX4_ERROR; } - _mag_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH); + start(); - /* advertise sensor topic, measure manually to initialize valid report */ - struct mag_report mrp; - _mag_reports->get(&mrp); - - _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, - &_mag_orb_class_instance, ORB_PRIO_VERY_HIGH); -// &_mag_orb_class_instance, ORB_PRIO_LOW); - - if (_mag_topic == nullptr) { - PX4_ERR("ADVERT FAIL"); - return -ENOMEM; - } - - return OK; + return PX4_OK; } bool AK09916::check_duplicate(uint8_t *mag_data) @@ -352,213 +224,52 @@ bool AK09916::check_duplicate(uint8_t *mag_data) void AK09916::measure() { - uint8_t ret, cmd = AK09916REG_ST1; + uint8_t cmd = AK09916REG_ST1; struct ak09916_regs raw_data; - ret = transfer(&cmd, 1, (uint8_t *)(&raw_data), sizeof(struct ak09916_regs)); + const hrt_abstime timestamp_sample = hrt_absolute_time(); + uint8_t ret = transfer(&cmd, 1, (uint8_t *)(&raw_data), sizeof(struct ak09916_regs)); if (ret == OK) { raw_data.st2 = raw_data.st2; - _measure(raw_data); - } -} - -void -AK09916::_measure(struct ak09916_regs data) -{ - bool mag_notify = true; - - if (check_duplicate((uint8_t *)&data.x) && !(data.st1 & 0x02)) { - perf_count(_mag_duplicates); - return; - } - - /* monitor for if data overrun flag is ever set */ - if (data.st1 & 0x02) { - perf_count(_mag_overruns); - } - - /* monitor for if magnetic sensor overflow flag is ever set noting that st2 - * is usually not even refreshed, but will always be in the same place in the - * mpu's buffers regardless, hence the actual count would be bogus - */ - if (data.st2 & 0x08) { - perf_count(_mag_overflows); - } - - mag_report mrb; - mrb.timestamp = hrt_absolute_time(); - mrb.is_external = true; - - float xraw_f, yraw_f, zraw_f; - mrb.x_raw = data.x; - mrb.y_raw = data.y; - mrb.z_raw = data.z; - - xraw_f = data.x; - yraw_f = data.y; - zraw_f = data.z; - /* apply user specified rotation */ - rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - - - mrb.x = ((xraw_f * _mag_range_scale * _mag_asa_x) - _mag_scale.x_offset) * _mag_scale.x_scale; - mrb.y = ((yraw_f * _mag_range_scale * _mag_asa_y) - _mag_scale.y_offset) * _mag_scale.y_scale; - mrb.z = ((zraw_f * _mag_range_scale * _mag_asa_z) - _mag_scale.z_offset) * _mag_scale.z_scale; - _last_report.x = mrb.x; - _last_report.y = mrb.y; - _last_report.z = mrb.z; - mrb.scaling = _mag_range_scale; - mrb.device_id = _device_id.devid; - - mrb.error_count = perf_event_count(_mag_errors); - - _mag_reports->force(&mrb); - - /* notify anyone waiting for data */ - if (mag_notify) { - poll_notify(POLLIN); - } - - if (mag_notify && !(_pub_blocked)) { - /* publish it */ - orb_publish(ORB_ID(sensor_mag), _mag_topic, &mrb); - } -} - -ssize_t -AK09916::read(struct file *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(mag_report); - struct mag_report *mag_buf = reinterpret_cast(buffer); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* manual measurement - run one conversion */ - /* XXX really it'd be nice to lock against other readers here */ - do { - _mag_reports->flush(); - - /* trigger a measurement */ - measure(); + if (check_duplicate((uint8_t *)&raw_data.x) && !(raw_data.st1 & 0x02)) { + perf_count(_mag_duplicates); + return; + } - if (_mag_reports->get(mag_buf)) { - ret = sizeof(struct mag_report); + /* monitor for if data overrun flag is ever set */ + if (raw_data.st1 & 0x02) { + perf_count(_mag_overruns); } - } while (0); - /* return the number of bytes transferred */ - return ret; + /* monitor for if magnetic sensor overflow flag is ever set noting that st2 + * is usually not even refreshed, but will always be in the same place in the + * mpu's buffers regardless, hence the actual count would be bogus + */ + if (raw_data.st2 & 0x08) { + perf_count(_mag_overflows); + } + _px4_mag.set_error_count(perf_event_count(_mag_errors)); + _px4_mag.update(timestamp_sample, raw_data.x, raw_data.y, raw_data.z); + } } - void AK09916::print_info() { perf_print_counter(_mag_reads); perf_print_counter(_mag_errors); perf_print_counter(_mag_overruns); - _mag_reports->print_info("mag queue"); - - printf("output (%.2f %.2f %.2f)\n", (double)_last_report.x, (double)_last_report.y, (double)_last_report.z); - printf("\n"); - -} - - -int -AK09916::ioctl(struct file *filp, int cmd, unsigned long arg) -{ - /* - * Repeated in ICM20948_accel::ioctl - * Both accel and mag CDev could be unused in case of magnetometer only mode or MPU6500 - */ - - switch (cmd) { - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default polling rate */ - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(AK09916_CONVERSION_INTERVAL); - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - return OK; - } - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); - - /* check against maximum rate */ - if (ticks < USEC2TICK(AK09916_CONVERSION_INTERVAL)) { - return -EINVAL; - } - - /* update interval for next measurement */ - _measure_ticks = ticks; - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - return OK; - } - } - } - - case SENSORIOCRESET: - return reset(); - - case MAGIOCSSCALE: - /* copy scale in */ - memcpy(&_mag_scale, (struct mag_scale *) arg, sizeof(_mag_scale)); - return OK; - - case MAGIOCGSCALE: - /* copy scale out */ - memcpy((struct mag_scale *) arg, &_mag_scale, sizeof(_mag_scale)); - return OK; - - case MAGIOCCALIBRATE: - return OK; - - case MAGIOCEXSTRAP: - return OK; - - default: - return (int)I2C::ioctl(filp, cmd, arg); - } + _px4_mag.print_status(); } uint8_t AK09916::read_reg(uint8_t reg) { const uint8_t cmd = reg; - uint8_t ret; + uint8_t ret{}; transfer(&cmd, 1, &ret, 1); @@ -581,7 +292,7 @@ AK09916::write_reg(uint8_t reg, uint8_t value) } int -AK09916::reset(void) +AK09916::reset() { int rv = probe(); @@ -597,7 +308,7 @@ AK09916::reset(void) } int -AK09916::probe(void) +AK09916::probe() { int retries = 10; @@ -617,60 +328,44 @@ AK09916::probe(void) } int -AK09916::setup(void) +AK09916::setup() { write_reg(AK09916REG_CNTL2, AK09916_CNTL2_CONTINOUS_MODE_100HZ); return OK; } - -void -AK09916::cycle_trampoline(void *arg) -{ - AK09916 *dev = (AK09916 *)arg; - - dev->cycle(); -} - - void AK09916::start() { - /* reset the report ring and state machine */ - _mag_reports->flush(); + _measure_interval = AK09916_CONVERSION_INTERVAL; /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&AK09916::cycle_trampoline, this, 1); + ScheduleNow(); } void AK09916::stop() { - if (_measure_ticks > 0) { - /* ensure no new items are queued while we cancel this one */ - _measure_ticks = 0; - work_cancel(HPWORK, &_work); - } + /* ensure no new items are queued while we cancel this one */ + _measure_interval = 0; + + ScheduleClear(); } void -AK09916::cycle() +AK09916::Run() { - if (_measure_ticks == 0) { + if (_measure_interval == 0) { return; } /* measurement phase */ measure(); - if (_measure_ticks > 0) { + if (_measure_interval > 0) { /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&AK09916::cycle_trampoline, - this, - USEC2TICK(AK09916_CONVERSION_INTERVAL)); + ScheduleDelayed(_measure_interval); } } @@ -713,19 +408,11 @@ ak09916_main(int argc, char *argv[]) ak09916::start(external_bus, rotation); } - - /* - * Test the driver/device. - */ - if (!strcmp(verb, "test")) { - ak09916::test(external_bus); - } - /* - * Reset the driver. + * Stop the driver. */ - if (!strcmp(verb, "reset")) { - ak09916::reset(external_bus); + if (!strcmp(verb, "stop")) { + ak09916::stop(external_bus); } /* diff --git a/src/drivers/magnetometer/ak09916/ak09916.hpp b/src/drivers/magnetometer/ak09916/ak09916.hpp index c02b59f4a5..5e6f15dc65 100644 --- a/src/drivers/magnetometer/ak09916/ak09916.hpp +++ b/src/drivers/magnetometer/ak09916/ak09916.hpp @@ -35,47 +35,18 @@ #include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include +#include #include - -#include -#include - -#include #include - -#include -#include #include -#include +#include +#include - -#define AK09916_SAMPLE_RATE 100 #define AK09916_DEVICE_PATH_MAG "/dev/ak09916_i2c_int" - #define AK09916_DEVICE_PATH_MAG_EXT "/dev/ak09916_i2c_ext" /* in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit */ - -#define AK09916_MAG_RANGE_GA 1.5e-3f; +static constexpr float AK09916_MAG_RANGE_GA{1.5e-3f}; /* ak09916 deviating register addresses and bit definitions */ #define AK09916_I2C_ADDR 0x0C @@ -85,30 +56,13 @@ #define AK09916REG_WIA 0x00 -#define AK09916REG_HXL 0x11 -#define AK09916REG_HXH 0x12 -#define AK09916REG_HYL 0x13 -#define AK09916REG_HYH 0x14 -#define AK09916REG_HZL 0x15 -#define AK09916REG_HZH 0x16 #define AK09916REG_ST1 0x10 -#define AK09916REG_ST2 0x18 #define AK09916REG_CNTL2 0x31 #define AK09916REG_CNTL3 0x32 - -#define AK09916_CNTL2_POWERDOWN_MODE 0x00 -#define AK09916_RESET 0x01 +#define AK09916_RESET 0x01 #define AK09916_CNTL2_SINGLE_MODE 0x01 /* default */ -#define AK09916_CNTL2_CONTINOUS_MODE_10HZ 0x02 -#define AK09916_CNTL2_CONTINOUS_MODE_20HZ 0x04 -#define AK09916_CNTL2_CONTINOUS_MODE_50HZ 0x06 #define AK09916_CNTL2_CONTINOUS_MODE_100HZ 0x08 -#define AK09916_CNTL2_SELFTEST_MODE 0x10 -#define AK09916_CNTL3_SRST 0x01 -#define AK09916_ST1_DRDY 0x01 -#define AK09916_ST1_DOR 0x02 - #pragma pack(push, 1) struct ak09916_regs { @@ -121,20 +75,16 @@ struct ak09916_regs { }; #pragma pack(pop) - - /** * Helper class implementing the magnetometer driver node. */ -class AK09916 : public device::I2C +class AK09916 : public device::I2C, px4::ScheduledWorkItem { public: AK09916(int bus, const char *path, enum Rotation rotation); ~AK09916(); - virtual int ioctl(struct file *filp, int cmd, unsigned long arg); virtual int init(); - virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); void read_block(uint8_t reg, uint8_t *val, uint8_t count); @@ -145,51 +95,35 @@ public: int setup_master_i2c(void); bool check_id(uint8_t &id); + void Run(); - static void cycle_trampoline(void *arg); void start(void); void stop(void); - void cycle(void); protected: - friend class ICM20948; - /* Directly measure from the _interface if possible */ void measure(); - /* Update the state with prefetched data (internally called by the regular measure() )*/ - void _measure(struct ak09916_regs data); - uint8_t read_reg(uint8_t reg); void write_reg(uint8_t reg, uint8_t value); private: - work_s _work{}; - unsigned _measure_ticks; - - enum Rotation _rotation; - orb_advert_t _mag_topic; - int _mag_orb_class_instance; - int _mag_class_instance; - bool _mag_reading_data; - ringbuffer::RingBuffer *_mag_reports; - mag_report _last_report {}; /**< used for info() */ - struct mag_calibration_s _mag_scale; - float _mag_range_scale; + + PX4Magnetometer _px4_mag; + + uint32_t _measure_interval{0}; + perf_counter_t _mag_reads; perf_counter_t _mag_errors; perf_counter_t _mag_overruns; perf_counter_t _mag_overflows; perf_counter_t _mag_duplicates; - float _mag_asa_x; - float _mag_asa_y; - float _mag_asa_z; bool check_duplicate(uint8_t *mag_data); // keep last mag reading for duplicate detection - uint8_t _last_mag_data[6]; + uint8_t _last_mag_data[6] {}; /* do not allow to copy this class due to pointer data members */ AK09916(const AK09916 &);