diff --git a/src/drivers/magnetometer/ak09916/ak09916.cpp b/src/drivers/magnetometer/ak09916/ak09916.cpp index 9b78673ee2..6c82bc4491 100644 --- a/src/drivers/magnetometer/ak09916/ak09916.cpp +++ b/src/drivers/magnetometer/ak09916/ak09916.cpp @@ -32,12 +32,7 @@ ****************************************************************************/ /** - * @file mag.cpp - * - * Driver for the ak09916 magnetometer within the Invensense mpu9250 - * - * @author Robert Dickenson - * + * Driver for the standalone AK09916 magnetometer. */ #include @@ -51,10 +46,8 @@ #include "ak09916.hpp" -/** driver 'main' command */ extern "C" { __EXPORT int ak09916_main(int argc, char *argv[]); } -#define AK09916_CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ namespace ak09916 { @@ -66,25 +59,16 @@ void start(bool, enum Rotation); void info(bool); void usage(); -/** - * Start the driver. - * - * This function only returns if the driver is up and running - * or failed to detect the sensor. - */ void start(bool external_bus, enum Rotation rotation) { 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); - if (*g_dev_ptr != nullptr) - /* if already started, the still command succeeded */ - { + if (*g_dev_ptr != nullptr) { PX4_ERR("already started"); exit(0); } - /* create the driver */ if (external_bus) { #if defined(PX4_I2C_BUS_EXPANSION) *g_dev_ptr = new AK09916(PX4_I2C_BUS_EXPANSION, path, rotation); @@ -98,25 +82,14 @@ void start(bool external_bus, enum Rotation rotation) exit(0); } - if (*g_dev_ptr == nullptr) { - goto fail; - } - - - if (OK != (*g_dev_ptr)->init()) { - goto fail; + if (*g_dev_ptr == nullptr || (OK != (*g_dev_ptr)->init())) { + PX4_ERR("driver start failed"); + delete (*g_dev_ptr); + *g_dev_ptr = nullptr; + exit(1); } exit(0); -fail: - - if (*g_dev_ptr != nullptr) { - delete (*g_dev_ptr); - *g_dev_ptr = nullptr; - } - - PX4_ERR("driver start failed"); - exit(1); } void @@ -153,16 +126,14 @@ info(bool external_bus) void usage() { - PX4_WARN("missing command: try 'start', 'info', stop'"); - PX4_WARN("options:"); - PX4_WARN(" -X (external bus)"); - + PX4_INFO("missing command: try 'start', 'info', stop'"); + PX4_INFO("options:"); + PX4_INFO(" -X (external bus)"); + PX4_INFO(" -R (rotation)"); } -} // namespace AK09916 +} // namespace ak09916 -// If interface is non-null, then it will used for interacting with the device. -// 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), ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), @@ -191,17 +162,15 @@ AK09916::init() { int ret = I2C::init(); - /* if cdev init failed, bail now */ if (ret != OK) { - DEVICE_DEBUG("AK09916 mag init failed"); - + PX4_WARN("AK09916 mag init failed"); return ret; } ret = reset(); if (ret != PX4_OK) { - return PX4_ERROR; + return ret; } start(); @@ -212,7 +181,7 @@ AK09916::init() bool AK09916::check_duplicate(uint8_t *mag_data) { if (memcmp(mag_data, &_last_mag_data, sizeof(_last_mag_data)) == 0) { - // it isn't new data - wait for next timer + // It isn't new data - wait for next timer. return true; } else { @@ -238,15 +207,15 @@ AK09916::measure() return; } - /* monitor for if data overrun flag is ever set */ + /* Monitor for if data overrun flag is ever set. */ if (raw_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 - */ + /* 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); } @@ -278,9 +247,9 @@ AK09916::read_reg(uint8_t reg) } bool -AK09916::check_id(uint8_t &deviceid) +AK09916::check_id() { - deviceid = read_reg(AK09916REG_WIA); + const uint8_t deviceid = read_reg(AK09916REG_WIA); return (AK09916_DEVICE_ID_A == deviceid); } @@ -298,10 +267,10 @@ AK09916::reset() int rv = probe(); if (rv == OK) { - // Now reset the mag + // Now reset the mag. write_reg(AK09916REG_CNTL3, AK09916_RESET); - // Then re-initialize the bus/mag + // Then re-initialize the bus/mag. rv = setup(); } @@ -316,9 +285,7 @@ AK09916::probe() do { write_reg(AK09916REG_CNTL3, AK09916_RESET); - uint8_t id = 0; - - if (check_id(id)) { + if (check_id()) { return OK; } @@ -339,17 +306,16 @@ AK09916::setup() void AK09916::start() { - _measure_interval = AK09916_CONVERSION_INTERVAL; + _cycle_interval = AK09916_CONVERSION_INTERVAL_us; - /* schedule a cycle to start things */ ScheduleNow(); } void AK09916::stop() { - /* ensure no new items are queued while we cancel this one */ - _measure_interval = 0; + // Ensure no new items are queued while we cancel this one. + _cycle_interval = 0; ScheduleClear(); } @@ -357,16 +323,14 @@ AK09916::stop() void AK09916::Run() { - if (_measure_interval == 0) { + if (_cycle_interval == 0) { return; } - /* measurement phase */ measure(); - if (_measure_interval > 0) { - /* schedule a fresh cycle call when the measurement is done */ - ScheduleDelayed(_measure_interval); + if (_cycle_interval > 0) { + ScheduleDelayed(_cycle_interval); } } @@ -402,23 +366,14 @@ ak09916_main(int argc, char *argv[]) const char *verb = argv[myoptind]; - /* - * Start/load the driver. - */ if (!strcmp(verb, "start")) { ak09916::start(external_bus, rotation); } - /* - * Stop the driver. - */ if (!strcmp(verb, "stop")) { ak09916::stop(external_bus); } - /* - * Print driver information. - */ if (!strcmp(verb, "info")) { ak09916::info(external_bus); } diff --git a/src/drivers/magnetometer/ak09916/ak09916.hpp b/src/drivers/magnetometer/ak09916/ak09916.hpp index d42ff99098..0bfd3c3528 100644 --- a/src/drivers/magnetometer/ak09916/ak09916.hpp +++ b/src/drivers/magnetometer/ak09916/ak09916.hpp @@ -42,27 +42,26 @@ #include #include -#define AK09916_DEVICE_PATH_MAG "/dev/ak09916_i2c_int" -#define AK09916_DEVICE_PATH_MAG_EXT "/dev/ak09916_i2c_ext" +static constexpr auto AK09916_DEVICE_PATH_MAG = "/dev/ak09916_i2c_int"; +static constexpr auto AK09916_DEVICE_PATH_MAG_EXT = "/dev/ak09916_i2c_ext"; -/* in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit */ -static constexpr float AK09916_MAG_RANGE_GA{1.5e-3f}; +// in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit. +static constexpr float AK09916_MAG_RANGE_GA = 1.5e-3f; -/* ak09916 deviating register addresses and bit definitions */ -#define AK09916_I2C_ADDR 0x0C +static constexpr uint8_t AK09916_I2C_ADDR = 0x0C; -#define AK09916_DEVICE_ID_A 0x48 -#define AK09916_DEVICE_ID_B 0x09 // additional ID byte ("INFO" on AK9063 without content specification.) +static constexpr uint8_t AK09916_DEVICE_ID_A = 0x48; +static constexpr uint8_t AK09916REG_WIA = 0x00; -#define AK09916REG_WIA 0x00 +static constexpr uint8_t AK09916REG_ST1 = 0x10; +static constexpr uint8_t AK09916REG_CNTL2 = 0x31; +static constexpr uint8_t AK09916REG_CNTL3 = 0x32; -#define AK09916REG_ST1 0x10 -#define AK09916REG_CNTL2 0x31 -#define AK09916REG_CNTL3 0x32 +static constexpr uint8_t AK09916_RESET = 0x01; +static constexpr uint8_t AK09916_CNTL2_CONTINOUS_MODE_100HZ = 0x08; -#define AK09916_RESET 0x01 -#define AK09916_CNTL2_SINGLE_MODE 0x01 /* default */ -#define AK09916_CNTL2_CONTINOUS_MODE_100HZ 0x08 +// Run at 100 Hz. +static constexpr unsigned AK09916_CONVERSION_INTERVAL_us = 1000000 / 100; #pragma pack(push, 1) struct ak09916_regs { @@ -75,49 +74,43 @@ struct ak09916_regs { }; #pragma pack(pop) -/** - * Helper class implementing the magnetometer driver node. - */ + class AK09916 : public device::I2C, px4::ScheduledWorkItem { public: AK09916(int bus, const char *path, enum Rotation rotation); ~AK09916(); - virtual int init(); - - void read_block(uint8_t reg, uint8_t *val, uint8_t count); - - int reset(void); - int probe(void); - int setup(void); - void print_info(void); - int setup_master_i2c(void); - bool check_id(uint8_t &id); - - void Run(); - - void start(void); - void stop(void); + virtual int init() override; + void start(); + void stop(); + void cycle(); + void print_info(); + int probe(); protected: - - /* Directly measure from the _interface if possible */ + int setup(); + int setup_master_i2c(); + bool check_id(); + void Run(); void measure(); + int reset(); uint8_t read_reg(uint8_t reg); + void read_block(uint8_t reg, uint8_t *val, uint8_t count); void write_reg(uint8_t reg, uint8_t value); private: PX4Magnetometer _px4_mag; - uint32_t _measure_interval{0}; + uint32_t _cycle_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; bool check_duplicate(uint8_t *mag_data); @@ -125,7 +118,6 @@ private: // keep last mag reading for duplicate detection uint8_t _last_mag_data[6] {}; - /* do not allow to copy this class due to pointer data members */ - AK09916(const AK09916 &); - AK09916 operator=(const AK09916 &); + AK09916(const AK09916 &) = delete; + AK09916 operator=(const AK09916 &) = delete; };