From 95a0803b149770b8d1a5817611cd55e43a4ad296 Mon Sep 17 00:00:00 2001 From: Andreas Daniel Antener Date: Thu, 11 Apr 2019 18:43:50 +0200 Subject: [PATCH] Adding AK09916 driver to support the Here2 GNSS emulated mag (I2C) (#11837) * drivers, magnetometer: added ak09916 driver to support the mag driver emulated by the Here2 gps * boards: set yaw 270 rotation for external ak09916 driver and only start it for the PH21 * drivers: only start ak09916 when icm20948 is not available on a ph21 setup --- boards/px4/fmu-v3/init/rc.board_sensors | 6 +- src/drivers/magnetometer/CMakeLists.txt | 1 + .../magnetometer/ak09916/CMakeLists.txt | 43 + src/drivers/magnetometer/ak09916/ak09916.cpp | 735 ++++++++++++++++++ src/drivers/magnetometer/ak09916/ak09916.hpp | 196 +++++ 5 files changed, 980 insertions(+), 1 deletion(-) create mode 100644 src/drivers/magnetometer/ak09916/CMakeLists.txt create mode 100644 src/drivers/magnetometer/ak09916/ak09916.cpp create mode 100644 src/drivers/magnetometer/ak09916/ak09916.hpp diff --git a/boards/px4/fmu-v3/init/rc.board_sensors b/boards/px4/fmu-v3/init/rc.board_sensors index 0cabab7d28..d1dee11088 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 ccbbe267b6..8decabd5a2 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 0000000000..51b89e3621 --- /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 0000000000..09b7fb485e --- /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 +#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[]); } + +#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(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 0000000000..de15aac728 --- /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 + +#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; + +/* 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 &); +};