diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp
index db7f4323f8..0f931ec2ec 100644
--- a/libraries/AP_Compass/AP_Compass.cpp
+++ b/libraries/AP_Compass/AP_Compass.cpp
@@ -16,6 +16,7 @@
#include "AP_Compass_QURT.h"
#include "AP_Compass_qflight.h"
#include "AP_Compass_LIS3MDL.h"
+#include "AP_Compass_AK09916.h"
#include "AP_Compass.h"
extern AP_HAL::HAL& hal;
@@ -522,6 +523,14 @@ void Compass::_detect_backends(void)
ADD_BACKEND(AP_Compass_LIS3MDL::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_LIS3MDL_I2C_ADDR),
both_i2c_external, both_i2c_external?ROTATION_YAW_90:ROTATION_NONE),
AP_Compass_LIS3MDL::name, both_i2c_external);
+
+ // AK09916
+ ADD_BACKEND(AP_Compass_AK09916::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_AK09916_I2C_ADDR),
+ true, ROTATION_YAW_270),
+ AP_Compass_AK09916::name, true);
+ ADD_BACKEND(AP_Compass_AK09916::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_AK09916_I2C_ADDR),
+ both_i2c_external, both_i2c_external?ROTATION_YAW_270:ROTATION_NONE),
+ AP_Compass_AK09916::name, both_i2c_external);
}
if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXHAWK) {
ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.spi->get_device(HAL_COMPASS_HMC5843_NAME),
diff --git a/libraries/AP_Compass/AP_Compass_AK09916.cpp b/libraries/AP_Compass/AP_Compass_AK09916.cpp
new file mode 100644
index 0000000000..d11ea802db
--- /dev/null
+++ b/libraries/AP_Compass/AP_Compass_AK09916.cpp
@@ -0,0 +1,179 @@
+/*
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program. If not, see .
+ */
+/*
+ Driver by Andrew Tridgell, Nov 2016
+ */
+#include "AP_Compass_AK09916.h"
+
+#include
+#include
+#include
+#include
+
+#define REG_COMPANY_ID 0x00
+#define REG_DEVICE_ID 0x01
+#define REG_ST1 0x10
+#define REG_HXL 0x11
+#define REG_HXH 0x12
+#define REG_HYL 0x13
+#define REG_HYH 0x14
+#define REG_HZL 0x15
+#define REG_HZH 0x16
+#define REG_TMPS 0x17
+#define REG_ST2 0x18
+#define REG_CNTL1 0x30
+#define REG_CNTL2 0x31
+#define REG_CNTL3 0x32
+
+extern const AP_HAL::HAL &hal;
+
+AP_Compass_Backend *AP_Compass_AK09916::probe(Compass &compass,
+ AP_HAL::OwnPtr dev,
+ bool force_external,
+ enum Rotation rotation)
+{
+ if (!dev) {
+ return nullptr;
+ }
+ AP_Compass_AK09916 *sensor = new AP_Compass_AK09916(compass, std::move(dev), force_external, rotation);
+ if (!sensor || !sensor->init()) {
+ delete sensor;
+ return nullptr;
+ }
+
+ return sensor;
+}
+
+AP_Compass_AK09916::AP_Compass_AK09916(Compass &compass,
+ AP_HAL::OwnPtr _dev,
+ bool _force_external,
+ enum Rotation _rotation)
+ : AP_Compass_Backend(compass)
+ , dev(std::move(_dev))
+ , force_external(_force_external)
+ , rotation(_rotation)
+{
+}
+
+bool AP_Compass_AK09916::init()
+{
+ if (!dev->get_semaphore()->take(0)) {
+ return false;
+ }
+
+ uint16_t whoami;
+ if (!dev->read_registers(REG_COMPANY_ID, (uint8_t *)&whoami, 2) ||
+ whoami != 0x0948) {
+ // not a AK09916
+ goto fail;
+ }
+
+ dev->setup_checked_registers(2);
+
+ dev->write_register(REG_CNTL2, 0x08, true); // continuous 100Hz
+ dev->write_register(REG_CNTL3, 0x00, true);
+
+ dev->get_semaphore()->give();
+
+ /* register the compass instance in the frontend */
+ compass_instance = register_compass();
+
+ printf("Found a AK09916 on 0x%x as compass %u\n", dev->get_bus_id(), compass_instance);
+
+ set_rotation(compass_instance, rotation);
+
+ if (force_external) {
+ set_external(compass_instance, true);
+ }
+
+ dev->set_device_type(DEVTYPE_AK09916);
+ set_dev_id(compass_instance, dev->get_bus_id());
+
+ // call timer() at 100Hz
+ dev->register_periodic_callback(10000,
+ FUNCTOR_BIND_MEMBER(&AP_Compass_AK09916::timer, bool));
+
+ return true;
+
+fail:
+ dev->get_semaphore()->give();
+ return false;
+}
+
+bool AP_Compass_AK09916::timer()
+{
+ struct PACKED {
+ int16_t magx;
+ int16_t magy;
+ int16_t magz;
+ uint8_t tmps;
+ uint8_t status2;
+ } data;
+ const float to_utesla = 4912.0f / 32752.0f;
+ const float utesla_to_milliGauss = 10;
+ const float range_scale = to_utesla * utesla_to_milliGauss;
+ Vector3f field;
+
+ // check data ready
+ uint8_t st1;
+ if (!dev->read_registers(REG_ST1, &st1, 1) || !(st1 & 1)) {
+ goto check_registers;
+ }
+
+ if (!dev->read_registers(REG_HXL, (uint8_t *)&data, sizeof(data))) {
+ goto check_registers;
+ }
+
+ field(data.magx * range_scale, data.magy * range_scale, data.magz * range_scale);
+
+ /* rotate raw_field from sensor frame to body frame */
+ rotate_field(field, compass_instance);
+
+ /* publish raw_field (uncorrected point sample) for calibration use */
+ publish_raw_field(field, AP_HAL::micros(), compass_instance);
+
+ /* correct raw_field for known errors */
+ correct_field(field, compass_instance);
+
+ if (_sem->take(0)) {
+ accum += field;
+ accum_count++;
+ _sem->give();
+ }
+
+check_registers:
+ dev->check_next_register();
+ return true;
+}
+
+void AP_Compass_AK09916::read()
+{
+ if (!_sem->take_nonblocking()) {
+ return;
+ }
+ if (accum_count == 0) {
+ _sem->give();
+ return;
+ }
+
+ accum /= accum_count;
+
+ publish_filtered_field(accum, compass_instance);
+
+ accum.zero();
+ accum_count = 0;
+
+ _sem->give();
+}
diff --git a/libraries/AP_Compass/AP_Compass_AK09916.h b/libraries/AP_Compass/AP_Compass_AK09916.h
new file mode 100644
index 0000000000..c108cee129
--- /dev/null
+++ b/libraries/AP_Compass/AP_Compass_AK09916.h
@@ -0,0 +1,59 @@
+/*
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program. If not, see .
+ */
+#pragma once
+
+#include
+#include
+#include
+#include
+
+#include "AP_Compass.h"
+#include "AP_Compass_Backend.h"
+
+#ifndef HAL_COMPASS_AK09916_I2C_ADDR
+# define HAL_COMPASS_AK09916_I2C_ADDR 0x0C
+#endif
+
+class AP_Compass_AK09916 : public AP_Compass_Backend
+{
+public:
+ static AP_Compass_Backend *probe(Compass &compass,
+ AP_HAL::OwnPtr dev,
+ bool force_external = false,
+ enum Rotation rotation = ROTATION_NONE);
+
+ void read() override;
+
+ static constexpr const char *name = "AK09916";
+
+private:
+ AP_Compass_AK09916(Compass &compass, AP_HAL::OwnPtr dev,
+ bool force_external,
+ enum Rotation rotation);
+
+ AP_HAL::OwnPtr dev;
+
+ /**
+ * Device periodic callback to read data from the sensor.
+ */
+ bool init();
+ bool timer();
+
+ uint8_t compass_instance;
+ Vector3f accum;
+ uint16_t accum_count;
+ enum Rotation rotation;
+ bool force_external;
+};
diff --git a/libraries/AP_Compass/AP_Compass_Backend.h b/libraries/AP_Compass/AP_Compass_Backend.h
index 5cca9d8624..00df86ead7 100644
--- a/libraries/AP_Compass/AP_Compass_Backend.h
+++ b/libraries/AP_Compass/AP_Compass_Backend.h
@@ -54,6 +54,7 @@ public:
DEVTYPE_BMM150 = 0x05,
DEVTYPE_LSM9DS1 = 0x06,
DEVTYPE_LIS3MDL = 0x08,
+ DEVTYPE_AK09916 = 0x09,
};