diff --git a/boards/px4/fmu-v3/init/rc.board_sensors b/boards/px4/fmu-v3/init/rc.board_sensors
index 0cabab7d28801148b6f27635ec6cf9b55cc4c4f1..d1dee1108853e023f0e192470b043e6f21594b3c 100644
--- a/boards/px4/fmu-v3/init/rc.board_sensors
+++ b/boards/px4/fmu-v3/init/rc.board_sensors
@@ -60,7 +60,11 @@ then
param set SENS_EN_THERMAL 0
# ICM20948 as external magnetometer on I2C (e.g. Here GPS)
- icm20948 -X -M -R 6 start
+ if ! icm20948 -X -M -R 6 start
+ then
+ # external emulated AK09916 (Here2) is rotated 270 degrees yaw
+ ak09916 -X -R 6 start
+ fi
# external L3GD20H is rotated 180 degrees yaw
l3gd20 -X -R 4 start
diff --git a/src/drivers/magnetometer/CMakeLists.txt b/src/drivers/magnetometer/CMakeLists.txt
index ccbbe267b6127e02ea1d9f1e67a4e330e356ca8f..8decabd5a2691fa855a7486dda63150633d87045 100644
--- a/src/drivers/magnetometer/CMakeLists.txt
+++ b/src/drivers/magnetometer/CMakeLists.txt
@@ -31,6 +31,7 @@
#
############################################################################
+add_subdirectory(ak09916)
add_subdirectory(bmm150)
add_subdirectory(hmc5883)
add_subdirectory(qmc5883)
diff --git a/src/drivers/magnetometer/ak09916/CMakeLists.txt b/src/drivers/magnetometer/ak09916/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..51b89e3621aaa8c0281dd86a3f349b7bf1264169
--- /dev/null
+++ b/src/drivers/magnetometer/ak09916/CMakeLists.txt
@@ -0,0 +1,43 @@
+############################################################################
+#
+# Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+ MODULE drivers__ak09916
+ MAIN ak09916
+ STACK_MAIN 1200
+ COMPILE_FLAGS
+ -Wno-cast-align # TODO: fix and enable
+ SRCS
+ ak09916.cpp
+ DEPENDS
+ )
+
diff --git a/src/drivers/magnetometer/ak09916/ak09916.cpp b/src/drivers/magnetometer/ak09916/ak09916.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..09b7fb485e9f2be2a901c94688a512abad3455fa
--- /dev/null
+++ b/src/drivers/magnetometer/ak09916/ak09916.cpp
@@ -0,0 +1,735 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mag.cpp
+ *
+ * Driver for the ak09916 magnetometer within the Invensense mpu9250
+ *
+ * @author Robert Dickenson
+ *
+ */
+
+#include <px4_config.h>
+#include <px4_log.h>
+#include <px4_time.h>
+#include <lib/perf/perf_counter.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/device/spi.h>
+#include <drivers/device/ringbuffer.h>
+#include <drivers/device/integrator.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_mag.h>
+#include <lib/mathlib/math/filter/LowPassFilter2p.hpp>
+#include <lib/conversion/rotation.h>
+
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include "ak09916.hpp"
+#include <px4_getopt.h>
+
+/** driver 'main' command */
+extern "C" { __EXPORT int ak09916_main(int argc, char *argv[]); }
+
+#define AK09916_CONVERSION_INTERVAL (1000000 / 100) /* microseconds */
+
+namespace ak09916
+{
+
+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.
+ *
+ * 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)
+{
+ 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);
+
+ if (*g_dev_ptr != nullptr)
+ /* if already started, the still command succeeded */
+ {
+ 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);
+#else
+ PX4_ERR("External I2C not available");
+ exit(0);
+#endif
+
+ } else {
+ PX4_ERR("Internal I2C not available");
+ exit(0);
+ }
+
+ if (*g_dev_ptr == nullptr) {
+ goto fail;
+ }
+
+
+ if (OK != (*g_dev_ptr)->init()) {
+ 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:
+
+ if (*g_dev_ptr != nullptr) {
+ delete (*g_dev_ptr);
+ *g_dev_ptr = nullptr;
+ }
+
+ 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)
+{
+ 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);
+ }
+
+ if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
+ PX4_ERR("driver reset failed");
+ exit(1);
+ }
+
+ if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+ PX4_ERR("driver poll restart failed");
+ exit(1);
+ }
+
+ exit(0);
+
+}
+
+void
+info(bool external_bus)
+{
+ AK09916 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
+
+ if (*g_dev_ptr == nullptr) {
+ PX4_ERR("driver not running");
+ exit(1);
+ }
+
+ printf("state @ %p\n", *g_dev_ptr);
+ (*g_dev_ptr)->print_info();
+
+ exit(0);
+
+}
+
+void
+usage()
+{
+ PX4_WARN("missing command: try 'start', 'info', 'test', 'stop',\n'reset'");
+ PX4_WARN("options:");
+ PX4_WARN(" -X (external bus)");
+
+}
+
+} // 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),
+ _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(),
+ _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{}
+{
+ // 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));
+}
+
+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);
+ perf_free(_mag_overflows);
+ perf_free(_mag_duplicates);
+}
+
+int
+AK09916::init()
+{
+ int ret = I2C::init();
+
+ /* if cdev init failed, bail now */
+ if (ret != OK) {
+ DEVICE_DEBUG("AK09916 mag init failed");
+
+ return ret;
+ }
+
+ _mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));
+
+ if (_mag_reports == nullptr) {
+ return -ENOMEM;
+ }
+
+ _mag_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
+
+ reset();
+
+ /* 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;
+}
+
+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
+ return true;
+
+ } else {
+ memcpy(&_last_mag_data, mag_data, sizeof(_last_mag_data));
+ return false;
+ }
+}
+
+void
+AK09916::measure()
+{
+ uint8_t ret, cmd = AK09916REG_ST1;
+ struct ak09916_regs raw_data;
+
+ 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<struct mag_report *>(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 (_mag_reports->get(mag_buf)) {
+ ret = sizeof(struct mag_report);
+ }
+ } while (0);
+
+ /* return the number of bytes transferred */
+ return ret;
+
+}
+
+
+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);
+ }
+}
+
+uint8_t
+AK09916::read_reg(uint8_t reg)
+{
+ const uint8_t cmd = reg;
+ uint8_t ret;
+
+ transfer(&cmd, 1, &ret, 1);
+
+ return ret;
+}
+
+bool
+AK09916::check_id(uint8_t &deviceid)
+{
+ deviceid = read_reg(AK09916REG_WIA);
+
+ return (AK09916_DEVICE_ID_A == deviceid);
+}
+
+void
+AK09916::write_reg(uint8_t reg, uint8_t value)
+{
+ const uint8_t cmd[2] = { reg, value};
+ transfer(cmd, 2, nullptr, 0);
+}
+
+int
+AK09916::reset(void)
+{
+ // First initialize it to use the bus
+ int rv = setup();
+
+ if (rv == OK) {
+ // Now reset the mag
+ write_reg(AK09916REG_CNTL3, AK09916_RESET);
+
+ // Then re-initialize the bus/mag
+ rv = setup();
+ }
+
+ return rv;
+}
+
+int
+AK09916::setup(void)
+{
+ int retries = 10;
+
+ do {
+ write_reg(AK09916REG_CNTL3, AK09916_RESET);
+
+ uint8_t id = 0;
+
+ if (check_id(id)) {
+ break;
+ }
+
+ retries--;
+ } while (retries > 0);
+
+ 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();
+
+ /* schedule a cycle to start things */
+ work_queue(HPWORK, &_work, (worker_t)&AK09916::cycle_trampoline, this, 1);
+}
+
+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);
+ }
+}
+
+void
+AK09916::cycle()
+{
+ if (_measure_ticks == 0) {
+ return;
+ }
+
+ /* measurement phase */
+ measure();
+
+ if (_measure_ticks > 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));
+ }
+}
+
+int
+ak09916_main(int argc, char *argv[])
+{
+ int myoptind = 1;
+ int ch;
+ const char *myoptarg = nullptr;
+ bool external_bus = false;
+ enum Rotation rotation = ROTATION_NONE;
+
+ while ((ch = px4_getopt(argc, argv, "XR:", &myoptind, &myoptarg)) != EOF) {
+ switch (ch) {
+ case 'X':
+ external_bus = true;
+ break;
+
+ case 'R':
+ rotation = (enum Rotation)atoi(myoptarg);
+ break;
+
+ default:
+ ak09916::usage();
+ return 0;
+ }
+ }
+
+ if (myoptind >= argc) {
+ ak09916::usage();
+ return -1;
+ }
+
+ const char *verb = argv[myoptind];
+
+ /*
+ * Start/load the driver.
+ */
+ if (!strcmp(verb, "start")) {
+ ak09916::start(external_bus, rotation);
+ }
+
+
+ /*
+ * Test the driver/device.
+ */
+ if (!strcmp(verb, "test")) {
+ ak09916::test(external_bus);
+ }
+
+ /*
+ * Reset the driver.
+ */
+ if (!strcmp(verb, "reset")) {
+ ak09916::reset(external_bus);
+ }
+
+ /*
+ * Print driver information.
+ */
+ if (!strcmp(verb, "info")) {
+ ak09916::info(external_bus);
+ }
+
+ ak09916::usage();
+ return -1;
+}
\ No newline at end of file
diff --git a/src/drivers/magnetometer/ak09916/ak09916.hpp b/src/drivers/magnetometer/ak09916/ak09916.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..de15aac72806fb7d78c3d92bc58e8f6bec8cddd0
--- /dev/null
+++ b/src/drivers/magnetometer/ak09916/ak09916.hpp
@@ -0,0 +1,196 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+
+#include <px4_config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+#include <px4_log.h>
+
+#include <perf/perf_counter.h>
+#include <systemlib/err.h>
+#include <nuttx/wqueue.h>
+#include <systemlib/conversions.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/clock.h>
+
+#include <board_config.h>
+#include <drivers/drv_hrt.h>
+
+#include <drivers/device/ringbuffer.h>
+#include <drivers/device/integrator.h>
+#include <drivers/device/i2c.h>
+#include <drivers/drv_mag.h>
+
+
+#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;
+
+/* ak09916 deviating register addresses and bit definitions */
+#define 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.)
+
+#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_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 {
+ uint8_t st1;
+ int16_t x;
+ int16_t y;
+ int16_t z;
+ uint8_t tmps;
+ uint8_t st2;
+};
+#pragma pack(pop)
+
+
+
+/**
+ * Helper class implementing the magnetometer driver node.
+ */
+class AK09916 : public device::I2C
+{
+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);
+
+ int reset(void);
+ int setup(void);
+ void print_info(void);
+ int setup_master_i2c(void);
+ bool check_id(uint8_t &id);
+
+
+ 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;
+ 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];
+
+ /* do not allow to copy this class due to pointer data members */
+ AK09916(const AK09916 &);
+ AK09916 operator=(const AK09916 &);
+};