diff --git a/src/drivers/magnetometer/CMakeLists.txt b/src/drivers/magnetometer/CMakeLists.txt index 58272947af..1366ae32f5 100644 --- a/src/drivers/magnetometer/CMakeLists.txt +++ b/src/drivers/magnetometer/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ -add_subdirectory(ak09916) +add_subdirectory(akm) add_subdirectory(bmm150) add_subdirectory(hmc5883) add_subdirectory(qmc5883) diff --git a/src/drivers/magnetometer/ak09916/ak09916.cpp b/src/drivers/magnetometer/ak09916/ak09916.cpp deleted file mode 100644 index a612e85bf9..0000000000 --- a/src/drivers/magnetometer/ak09916/ak09916.cpp +++ /dev/null @@ -1,304 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -/** - * Driver for the standalone AK09916 magnetometer. - */ - -#include -#include -#include -#include -#include -#include -#include - -#include "ak09916.hpp" - -extern "C" __EXPORT int ak09916_main(int argc, char *argv[]); - -AK09916::AK09916(I2CSPIBusOption bus_option, const int bus, int bus_frequency, enum Rotation rotation) : - I2C(DRV_MAG_DEVTYPE_AK09916, MODULE_NAME, bus, AK09916_I2C_ADDR, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_mag(get_device_id(), external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT, rotation), - _mag_reads(perf_alloc(PC_COUNT, MODULE_NAME": mag_reads")), - _mag_errors(perf_alloc(PC_COUNT, MODULE_NAME": mag_errors")), - _mag_overruns(perf_alloc(PC_COUNT, MODULE_NAME": mag_overruns")), - _mag_overflows(perf_alloc(PC_COUNT, MODULE_NAME": mag_overflows")) -{ - _px4_mag.set_external(external()); - _px4_mag.set_scale(AK09916_MAG_RANGE_GA); -} - -AK09916::~AK09916() -{ - perf_free(_mag_reads); - perf_free(_mag_errors); - perf_free(_mag_overruns); - perf_free(_mag_overflows); -} - -int -AK09916::init() -{ - int ret = I2C::init(); - - if (ret != OK) { - DEVICE_DEBUG("AK09916 mag init failed (%i)", ret); - return ret; - } - - ret = reset(); - - if (ret != PX4_OK) { - return ret; - } - - start(); - - return PX4_OK; -} - -void -AK09916::try_measure() -{ - if (!is_ready()) { - return; - } - - measure(); -} - -bool -AK09916::is_ready() -{ - uint8_t st1; - const int ret = transfer(&AK09916REG_ST1, sizeof(AK09916REG_ST1), &st1, sizeof(st1)); - - if (ret != OK) { - return false; - } - - // Monitor if data overrun flag is ever set. - if (st1 & AK09916_ST1_DOR) { - perf_count(_mag_overruns); - } - - return (st1 & AK09916_ST1_DRDY); -} - -void -AK09916::measure() -{ - ak09916_regs regs; - - const hrt_abstime now = hrt_absolute_time(); - - const int ret = transfer(&AK09916REG_HXL, sizeof(AK09916REG_HXL), - reinterpret_cast(®s), sizeof(regs)); - - if (ret != OK) { - _px4_mag.set_error_count(perf_event_count(_mag_errors)); - return; - } - - // Monitor if magnetic sensor overflow flag is set. - if (regs.st2 & AK09916_ST2_HOFL) { - perf_count(_mag_overflows); - } - - _px4_mag.set_external(external()); - _px4_mag.update(now, regs.x, regs.y, regs.z); -} - -void -AK09916::print_status() -{ - I2CSPIDriverBase::print_status(); - perf_print_counter(_mag_reads); - perf_print_counter(_mag_errors); - perf_print_counter(_mag_overruns); - _px4_mag.print_status(); -} - -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() -{ - const uint8_t deviceid = read_reg(AK09916REG_WIA); - - return (AK09916_DEVICE_ID_A == deviceid); -} - -int -AK09916::write_reg(uint8_t reg, uint8_t value) -{ - const uint8_t cmd[2] = { reg, value}; - return transfer(cmd, 2, nullptr, 0); -} - -int -AK09916::reset() -{ - int rv = probe(); - - 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::probe() -{ - int retries = 10; - - do { - write_reg(AK09916REG_CNTL3, AK09916_RESET); - - if (check_id()) { - return OK; - } - - retries--; - } while (retries > 0); - - return PX4_ERROR; -} - -int -AK09916::setup() -{ - write_reg(AK09916REG_CNTL2, AK09916_CNTL2_CONTINOUS_MODE_100HZ); - - return OK; -} - -void -AK09916::start() -{ - ScheduleNow(); -} - -void -AK09916::RunImpl() -{ - try_measure(); - ScheduleDelayed(_cycle_interval); -} - -I2CSPIDriverBase * -AK09916::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, int runtime_instance) -{ - AK09916 *interface = new AK09916(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.rotation); - - if (interface == nullptr) { - PX4_ERR("failed creating interface for bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); - return nullptr; - } - - if (interface->init() != OK) { - delete interface; - PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); - return nullptr; - } - - return interface; -} - -void -AK09916::print_usage() -{ - PRINT_MODULE_USAGE_NAME("ak09916", "driver"); - PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); - PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); -} - -int -ak09916_main(int argc, char *argv[]) -{ - int ch; - using ThisDriver = AK09916; - BusCLIArguments cli{true, false}; - cli.default_i2c_frequency = 400000; - - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { - switch (ch) { - case 'R': - cli.rotation = (enum Rotation)atoi(cli.optarg()); - break; - } - } - - const char *verb = cli.optarg(); - - if (!verb) { - ThisDriver::print_usage(); - return -1; - } - - BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_AK09916); - - if (!strcmp(verb, "start")) { - return ThisDriver::module_start(cli, iterator); - } - - if (!strcmp(verb, "stop")) { - return ThisDriver::module_stop(iterator); - } - - if (!strcmp(verb, "status")) { - return ThisDriver::module_status(iterator); - } - - ThisDriver::print_usage(); - return 1; -} diff --git a/src/drivers/magnetometer/akm/CMakeLists.txt b/src/drivers/magnetometer/akm/CMakeLists.txt new file mode 100644 index 0000000000..6b3977f977 --- /dev/null +++ b/src/drivers/magnetometer/akm/CMakeLists.txt @@ -0,0 +1,35 @@ +############################################################################ +# +# Copyright (c) 2020 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. +# +############################################################################ + +add_subdirectory(ak8963) +add_subdirectory(ak09916) diff --git a/src/drivers/magnetometer/akm/ak09916/AK09916.cpp b/src/drivers/magnetometer/akm/ak09916/AK09916.cpp new file mode 100644 index 0000000000..f824a1350e --- /dev/null +++ b/src/drivers/magnetometer/akm/ak09916/AK09916.cpp @@ -0,0 +1,279 @@ +/**************************************************************************** + * + * Copyright (c) 2019-2020 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. + * + ****************************************************************************/ + +#include "AK09916.hpp" + +using namespace time_literals; + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +AK09916::AK09916(I2CSPIBusOption bus_option, int bus, int bus_frequency, enum Rotation rotation) : + I2C(DRV_MAG_DEVTYPE_AK09916, MODULE_NAME, bus, I2C_ADDRESS_DEFAULT, bus_frequency), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), + _px4_mag(get_device_id(), external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT, rotation) +{ + _px4_mag.set_external(external()); +} + +AK09916::~AK09916() +{ + perf_free(_transfer_perf); + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); + perf_free(_magnetic_sensor_overflow_perf); +} + +int AK09916::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool AK09916::Reset() +{ + _state = STATE::RESET; + ScheduleClear(); + ScheduleNow(); + return true; +} + +void AK09916::print_status() +{ + I2CSPIDriverBase::print_status(); + + perf_print_counter(_transfer_perf); + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); + perf_print_counter(_magnetic_sensor_overflow_perf); + + _px4_mag.print_status(); +} + +int AK09916::probe() +{ + const uint8_t WIA1 = RegisterRead(Register::WIA1); + + if (WIA1 != Company_ID) { + DEVICE_DEBUG("unexpected WIA1 0x%02x", WIA1); + return PX4_ERROR; + } + + const uint8_t WIA2 = RegisterRead(Register::WIA2); + + if (WIA2 != Device_ID) { + DEVICE_DEBUG("unexpected WIA2 0x%02x", WIA2); + return PX4_ERROR; + } + + return PX4_OK; +} + +void AK09916::RunImpl() +{ + switch (_state) { + case STATE::RESET: + // CNTL3 SRST: Soft reset + RegisterWrite(Register::CNTL3, CNTL3_BIT::SRST); + _reset_timestamp = hrt_absolute_time(); + _consecutive_failures = 0; + _state = STATE::WAIT_FOR_RESET; + ScheduleDelayed(100_ms); + break; + + case STATE::WAIT_FOR_RESET: + if ((RegisterRead(Register::WIA1) == Company_ID) && (RegisterRead(Register::WIA2) == Device_ID)) { + // if reset succeeded then configure + RegisterWrite(Register::CNTL2, CNTL2_BIT::MODE3); + _state = STATE::CONFIGURE; + ScheduleDelayed(100_ms); + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 100 ms"); + ScheduleDelayed(100_ms); + } + } + + break; + + case STATE::CONFIGURE: + if (Configure()) { + // if configure succeeded then start reading + _state = STATE::READ; + ScheduleOnInterval(20_ms, 20_ms); // 50 Hz + + } else { + // CONFIGURE not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Configure failed, resetting"); + _state = STATE::RESET; + + } else { + PX4_DEBUG("Configure failed, retrying"); + } + + ScheduleDelayed(100_ms); + } + + break; + + case STATE::READ: { + perf_begin(_transfer_perf); + TransferBuffer buffer{}; + const hrt_abstime timestamp_sample = hrt_absolute_time(); + uint8_t cmd = static_cast(Register::ST1); + int ret = transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(TransferBuffer)); + perf_end(_transfer_perf); + + bool success = false; + + if (ret == PX4_OK) { + if (buffer.ST2 & ST2_BIT::HOFL) { + perf_count(_magnetic_sensor_overflow_perf); + + } else if (buffer.ST1 & ST1_BIT::DRDY) { + const int16_t x = combine(buffer.HXH, buffer.HXL); + const int16_t y = combine(buffer.HYH, buffer.HYL); + const int16_t z = combine(buffer.HZH, buffer.HZL); + + // sensor's frame is +X forward (X), +Y right (Y), +Z down (Z) + _px4_mag.update(timestamp_sample, x, y, z); + + success = true; + + _consecutive_failures = 0; + } + } + + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + // check configuration registers periodically or immediately following any failure + if (RegisterCheck(_register_cfg[_checked_register])) { + _last_config_check_timestamp = timestamp_sample; + _checked_register = (_checked_register + 1) % size_register_cfg; + + } else { + // register check failed, force reset + perf_count(_bad_register_perf); + Reset(); + return; + } + } + + if (_consecutive_failures > 10) { + Reset(); + } + } + + break; + } +} + +bool AK09916::Configure() +{ + // first set and clear all configured register bits + for (const auto ®_cfg : _register_cfg) { + RegisterWrite(reg_cfg.reg, reg_cfg.set_bits); + } + + // now check that all are configured + bool success = true; + + for (const auto ®_cfg : _register_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + // mag resolution is 1.5 milli Gauss per bit (0.15 μT/LSB) + _px4_mag.set_scale(1.5e-3f); + + return success; +} + +bool AK09916::RegisterCheck(const register_config_t ®_cfg) +{ + bool success = true; + + const uint8_t reg_value = RegisterRead(reg_cfg.reg); + + if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); + success = false; + } + + if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); + success = false; + } + + return success; +} + +uint8_t AK09916::RegisterRead(Register reg) +{ + const uint8_t cmd = static_cast(reg); + uint8_t buffer{}; + transfer(&cmd, 1, &buffer, 1); + return buffer; +} + +void AK09916::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t buffer[2] { (uint8_t)reg, value }; + transfer(buffer, sizeof(buffer), nullptr, 0); +} + +void AK09916::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) +{ + const uint8_t orig_val = RegisterRead(reg); + uint8_t val = (orig_val & ~clearbits) | setbits; + + if (orig_val != val) { + RegisterWrite(reg, val); + } +} diff --git a/src/drivers/magnetometer/ak09916/ak09916.hpp b/src/drivers/magnetometer/akm/ak09916/AK09916.hpp similarity index 56% rename from src/drivers/magnetometer/ak09916/ak09916.hpp rename to src/drivers/magnetometer/akm/ak09916/AK09916.hpp index 3c453b08b7..90046dcb8c 100644 --- a/src/drivers/magnetometer/ak09916/ak09916.hpp +++ b/src/drivers/magnetometer/akm/ak09916/AK09916.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2020 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 @@ -31,90 +31,94 @@ * ****************************************************************************/ +/** + * @file AK09916.hpp + * + * Driver for the AKM AK09916 connected via I2C. + * + */ + #pragma once +#include "AKM_AK09916_registers.hpp" -#include -#include -#include #include -#include -#include +#include #include +#include +#include -// 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; - -static constexpr uint8_t AK09916_I2C_ADDR = 0x0C; - -static constexpr uint8_t AK09916_DEVICE_ID_A = 0x48; -static constexpr uint8_t AK09916REG_WIA = 0x00; - -static constexpr uint8_t AK09916REG_ST1 = 0x10; -static constexpr uint8_t AK09916REG_HXL = 0x11; -static constexpr uint8_t AK09916REG_CNTL2 = 0x31; -static constexpr uint8_t AK09916REG_CNTL3 = 0x32; - -static constexpr uint8_t AK09916_RESET = 0x01; -static constexpr uint8_t AK09916_CNTL2_CONTINOUS_MODE_100HZ = 0x08; - -static constexpr uint8_t AK09916_ST1_DRDY = 0x01; -static constexpr uint8_t AK09916_ST1_DOR = 0x02; - -static constexpr uint8_t AK09916_ST2_HOFL = 0x08; - -// Run at 100 Hz. -static constexpr unsigned AK09916_CONVERSION_INTERVAL_us = 1000000 / 100; - -#pragma pack(push, 1) -struct ak09916_regs { - int16_t x; - int16_t y; - int16_t z; - uint8_t tmps; - uint8_t st2; -}; -#pragma pack(pop) - +using namespace AKM_AK09916; class AK09916 : public device::I2C, public I2CSPIDriver { public: - AK09916(I2CSPIBusOption bus_option, const int bus, int bus_frequency, enum Rotation rotation); - virtual ~AK09916(); + AK09916(I2CSPIBusOption bus_option, int bus, int bus_frequency, enum Rotation rotation = ROTATION_NONE); + ~AK09916() override; static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, int runtime_instance); static void print_usage(); - int init() override; - void start(); - void print_status() override; - int probe() override; - void RunImpl(); -protected: - int setup(); - int setup_master_i2c(); - bool check_id(); - void try_measure(); - bool is_ready(); - void measure(); - int reset(); - - uint8_t read_reg(uint8_t reg); - void read_block(uint8_t reg, uint8_t *val, uint8_t count); - int write_reg(uint8_t reg, uint8_t value); + int init() override; + void print_status() override; private: + struct TransferBuffer { + uint8_t ST1; + uint8_t HXL; + uint8_t HXH; + uint8_t HYL; + uint8_t HYH; + uint8_t HZL; + uint8_t HZH; + uint8_t TMPS; + uint8_t ST2; + }; + + struct register_config_t { + Register reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + int probe() override; + + bool Reset(); + + bool Configure(); + + bool RegisterCheck(const register_config_t ®_cfg); + + uint8_t RegisterRead(Register reg); + void RegisterWrite(Register reg, uint8_t value); + void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits); + PX4Magnetometer _px4_mag; - static constexpr uint32_t _cycle_interval{AK09916_CONVERSION_INTERVAL_us}; + perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")}; + perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; + perf_counter_t _magnetic_sensor_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": magnetic sensor overflow")}; - perf_counter_t _mag_reads; - perf_counter_t _mag_errors; - perf_counter_t _mag_overruns; - perf_counter_t _mag_overflows; + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + unsigned _consecutive_failures{0}; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + CONFIGURE, + READ, + } _state{STATE::RESET}; + + uint8_t _checked_register{0}; + static constexpr uint8_t size_register_cfg{1}; + register_config_t _register_cfg[size_register_cfg] { + // Register | Set bits, Clear bits + { Register::CNTL2, CNTL2_BIT::MODE3, 0 }, + }; }; diff --git a/src/drivers/magnetometer/akm/ak09916/AKM_AK09916_registers.hpp b/src/drivers/magnetometer/akm/ak09916/AKM_AK09916_registers.hpp new file mode 100644 index 0000000000..61340d9c00 --- /dev/null +++ b/src/drivers/magnetometer/akm/ak09916/AKM_AK09916_registers.hpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2019-2020 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 AKM_AK09916_registers.hpp + * + * Asahi Kasei Microdevices (AKM) AK09916 registers. + * + */ + +#pragma once + +#include + +namespace AKM_AK09916 +{ + +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +static constexpr uint8_t Bit7 = (1 << 7); + +static constexpr uint32_t I2C_SPEED = 400 * 1000; // 400 kHz I2C serial interface +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0b0001100; + +static constexpr uint8_t Company_ID = 0x48; +static constexpr uint8_t Device_ID = 0x09; + +enum class Register : uint8_t { + WIA1 = 0x00, // Company ID of AKM + WIA2 = 0x01, // Device ID of AK09916 + + ST1 = 0x10, // Status 1 + HXL = 0x11, + HXH = 0x12, + HYL = 0x13, + HYH = 0x14, + HZL = 0x15, + HZH = 0x16, + + ST2 = 0x18, // Status 2 + + CNTL2 = 0x31, // Control 2 + CNTL3 = 0x32, // Control 3 +}; + +// ST1 +enum ST1_BIT : uint8_t { + DOR = Bit1, // Data overrun + DRDY = Bit0, // Data is ready +}; + +// ST2 +enum ST2_BIT : uint8_t { + HOFL = Bit3, // Magnetic sensor overflow +}; + +// CNTL2 +enum CNTL2_BIT : uint8_t { + // MODE[4:0] bits + MODE1 = Bit1, // “00010”: Continuous measurement mode 1 (10Hz) + MODE2 = Bit2, // “00100”: Continuous measurement mode 2 (20Hz) + MODE3 = Bit2 | Bit1, // “00110”: Continuous measurement mode 3 (50Hz) + MODE4 = Bit3, // “01000”: Continuous measurement mode 4 (100Hz) +}; + +// CNTL3 +enum CNTL3_BIT : uint8_t { + SRST = Bit0, +}; + +} // namespace AKM_AK09916 diff --git a/src/drivers/magnetometer/ak09916/CMakeLists.txt b/src/drivers/magnetometer/akm/ak09916/CMakeLists.txt similarity index 90% rename from src/drivers/magnetometer/ak09916/CMakeLists.txt rename to src/drivers/magnetometer/akm/ak09916/CMakeLists.txt index 161d52f7cb..13c86a7a4d 100644 --- a/src/drivers/magnetometer/ak09916/CMakeLists.txt +++ b/src/drivers/magnetometer/akm/ak09916/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2019-2020 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 @@ -30,14 +30,17 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + px4_add_module( - MODULE drivers__ak09916 + MODULE drivers__magnetometer__akm__ak09916 MAIN ak09916 COMPILE_FLAGS SRCS - ak09916.cpp + AKM_AK09916_registers.hpp + AK09916.cpp + AK09916.hpp + ak09916_main.cpp DEPENDS drivers_magnetometer px4_work_queue ) - diff --git a/src/drivers/magnetometer/akm/ak09916/ak09916_main.cpp b/src/drivers/magnetometer/akm/ak09916/ak09916_main.cpp new file mode 100644 index 0000000000..47704d754b --- /dev/null +++ b/src/drivers/magnetometer/akm/ak09916/ak09916_main.cpp @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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. + * + ****************************************************************************/ + +#include "AK09916.hpp" + +#include +#include + +I2CSPIDriverBase *AK09916::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) +{ + AK09916 *instance = new AK09916(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.rotation); + + if (!instance) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (instance->init() != PX4_OK) { + delete instance; + PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + return nullptr; + } + + return instance; +} + +void AK09916::print_usage() +{ + PRINT_MODULE_USAGE_NAME("ak09916", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" __EXPORT int ak09916_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = AK09916; + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = I2C_SPEED; + + while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + switch (ch) { + case 'R': + cli.rotation = (enum Rotation)atoi(cli.optarg()); + break; + } + } + + const char *verb = cli.optarg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_AK09916); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + } + + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/magnetometer/akm/ak8963/AK8963.cpp b/src/drivers/magnetometer/akm/ak8963/AK8963.cpp new file mode 100644 index 0000000000..edef0aa071 --- /dev/null +++ b/src/drivers/magnetometer/akm/ak8963/AK8963.cpp @@ -0,0 +1,317 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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. + * + ****************************************************************************/ + +#include "AK8963.hpp" + +using namespace time_literals; + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +AK8963::AK8963(I2CSPIBusOption bus_option, int bus, int bus_frequency, enum Rotation rotation) : + I2C(DRV_MAG_DEVTYPE_AK8963, MODULE_NAME, bus, I2C_ADDRESS_DEFAULT, bus_frequency), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), + _px4_mag(get_device_id(), external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT, rotation) +{ + _px4_mag.set_external(external()); +} + +AK8963::~AK8963() +{ + perf_free(_transfer_perf); + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); + perf_free(_magnetic_sensor_overflow_perf); +} + +int AK8963::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool AK8963::Reset() +{ + _state = STATE::RESET; + ScheduleClear(); + ScheduleNow(); + return true; +} + +void AK8963::print_status() +{ + I2CSPIDriverBase::print_status(); + + perf_print_counter(_transfer_perf); + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); + perf_print_counter(_magnetic_sensor_overflow_perf); + + _px4_mag.print_status(); +} + +int AK8963::probe() +{ + const uint8_t WIA = RegisterRead(Register::WIA); + + if (WIA != Device_ID) { + DEVICE_DEBUG("unexpected WIA 0x%02x", WIA); + return PX4_ERROR; + } + + return PX4_OK; +} + +void AK8963::RunImpl() +{ + switch (_state) { + case STATE::RESET: + // CNTL2 SRST: Soft reset + RegisterWrite(Register::CNTL2, CNTL2_BIT::SRST); + _reset_timestamp = hrt_absolute_time(); + _consecutive_failures = 0; + _state = STATE::WAIT_FOR_RESET; + ScheduleDelayed(100_ms); + break; + + case STATE::WAIT_FOR_RESET: + if (RegisterRead(Register::WIA) == Device_ID) { + // if reset succeeded then configure + if (!_sensitivity_adjustments_loaded) { + // Set Fuse ROM Access mode before reading Fuse ROM data. + RegisterWrite(Register::CNTL1, CNTL1_BIT::BIT_16 | CNTL1_BIT::FUSE_ROM_ACCESS_MODE); + _state = STATE::READ_SENSITIVITY_ADJUSTMENTS; + ScheduleDelayed(100_ms); + + } else { + // if reset succeeded then configure + RegisterWrite(Register::CNTL1, CNTL1_BIT::CONTINUOUS_MODE_2 | CNTL1_BIT::BIT_16); + _state = STATE::CONFIGURE; + ScheduleDelayed(100_ms); + } + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 100 ms"); + ScheduleDelayed(100_ms); + } + } + + break; + + case STATE::READ_SENSITIVITY_ADJUSTMENTS: { + // read FUSE ROM (to get ASA corrections) + uint8_t response[3] {}; + uint8_t cmd = static_cast(Register::ASAX); + + if (transfer(&cmd, 1, response, 3) == PX4_OK) { + + bool valid = true; + + for (int i = 0; i < 3; i++) { + if (response[i] != 0 && response[i] != 0xFF) { + _sensitivity[i] = ((float)(response[i] - 128) / 256.f) + 1.f; + + } else { + valid = false; + } + } + + _sensitivity_adjustments_loaded = valid; + + // After reading fuse ROM data, set power-down mode (MODE[3:0]=“0000”) before the transition to another mode. + } + + // reset on success or failure + RegisterWrite(Register::CNTL1, 0); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + } + break; + + case STATE::CONFIGURE: + if (Configure()) { + // if configure succeeded then start reading + _state = STATE::READ; + ScheduleOnInterval(10_ms, 10_ms); // 100 Hz + + } else { + // CONFIGURE not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Configure failed, resetting"); + _state = STATE::RESET; + + } else { + PX4_DEBUG("Configure failed, retrying"); + } + + ScheduleDelayed(100_ms); + } + + break; + + case STATE::READ: { + perf_begin(_transfer_perf); + TransferBuffer buffer{}; + const hrt_abstime timestamp_sample = hrt_absolute_time(); + uint8_t cmd = static_cast(Register::ST1); + int ret = transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(TransferBuffer)); + perf_end(_transfer_perf); + + bool success = false; + + if (ret == PX4_OK) { + if (buffer.ST2 & ST2_BIT::HOFL) { + perf_count(_magnetic_sensor_overflow_perf); + + } else if ((buffer.ST1 & ST1_BIT::DRDY) && (buffer.ST2 & ST2_BIT::BITM)) { + + const int16_t x = combine(buffer.HXH, buffer.HXL); + const int16_t y = combine(buffer.HYH, buffer.HYL); + const int16_t z = combine(buffer.HZH, buffer.HZL); + + // sensor's frame is +Y forward (X), -X right (Y), +Z down (Z) + // adjust with sensitivity scale factors + float x_f = y * _sensitivity[0]; // X := +Y + float y_f = -x * _sensitivity[1]; // Y := -X + float z_f = z * _sensitivity[2]; // Z := +Z + + _px4_mag.update(timestamp_sample, x_f, y_f, z_f); + + success = true; + + _consecutive_failures = 0; + } + } + + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + // check configuration registers periodically or immediately following any failure + if (RegisterCheck(_register_cfg[_checked_register])) { + _last_config_check_timestamp = timestamp_sample; + _checked_register = (_checked_register + 1) % size_register_cfg; + + } else { + // register check failed, force reset + perf_count(_bad_register_perf); + Reset(); + return; + } + } + + if (_consecutive_failures > 10) { + Reset(); + } + } + + break; + } +} + +bool AK8963::Configure() +{ + // first set and clear all configured register bits + for (const auto ®_cfg : _register_cfg) { + RegisterWrite(reg_cfg.reg, reg_cfg.set_bits); + } + + // now check that all are configured + bool success = true; + + for (const auto ®_cfg : _register_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + // in 16-bit sampling mode (ST2 BITM) the mag resolution is 1.5 milli Gauss per bit (0.15 μT/LSB) + _px4_mag.set_scale(1.5e-3f); + + return success; +} + +bool AK8963::RegisterCheck(const register_config_t ®_cfg) +{ + bool success = true; + + const uint8_t reg_value = RegisterRead(reg_cfg.reg); + + if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); + success = false; + } + + if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); + success = false; + } + + return success; +} + +uint8_t AK8963::RegisterRead(Register reg) +{ + const uint8_t cmd = static_cast(reg); + uint8_t buffer{}; + transfer(&cmd, 1, &buffer, 1); + return buffer; +} + +void AK8963::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t buffer[2] { (uint8_t)reg, value }; + transfer(buffer, sizeof(buffer), nullptr, 0); +} + +void AK8963::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) +{ + const uint8_t orig_val = RegisterRead(reg); + uint8_t val = (orig_val & ~clearbits) | setbits; + + if (orig_val != val) { + RegisterWrite(reg, val); + } +} diff --git a/src/drivers/magnetometer/akm/ak8963/AK8963.hpp b/src/drivers/magnetometer/akm/ak8963/AK8963.hpp new file mode 100644 index 0000000000..a0f1881c9c --- /dev/null +++ b/src/drivers/magnetometer/akm/ak8963/AK8963.hpp @@ -0,0 +1,127 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 AK8963.hpp + * + * Driver for the AKM AK8963 connected via I2C. + * + */ + +#pragma once + +#include "AKM_AK8963_registers.hpp" + +#include +#include +#include +#include +#include + +using namespace AKM_AK8963; + +class AK8963 : public device::I2C, public I2CSPIDriver +{ +public: + AK8963(I2CSPIBusOption bus_option, int bus, int bus_frequency, enum Rotation rotation = ROTATION_NONE); + ~AK8963() override; + + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + + struct TransferBuffer { + uint8_t ST1; + uint8_t HXL; + uint8_t HXH; + uint8_t HYL; + uint8_t HYH; + uint8_t HZL; + uint8_t HZH; + uint8_t ST2; + }; + + struct register_config_t { + Register reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + int probe() override; + + bool Reset(); + + bool Configure(); + + bool RegisterCheck(const register_config_t ®_cfg); + + uint8_t RegisterRead(Register reg); + void RegisterWrite(Register reg, uint8_t value); + void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits); + + PX4Magnetometer _px4_mag; + + perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")}; + perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; + perf_counter_t _magnetic_sensor_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": magnetic sensor overflow")}; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + unsigned _consecutive_failures{0}; + + bool _sensitivity_adjustments_loaded{false}; + float _sensitivity[3] {1.f, 1.f, 1.f}; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + READ_SENSITIVITY_ADJUSTMENTS, + CONFIGURE, + READ, + } _state{STATE::RESET}; + + uint8_t _checked_register{0}; + static constexpr uint8_t size_register_cfg{1}; + register_config_t _register_cfg[size_register_cfg] { + // Register | Set bits, Clear bits + { Register::CNTL1, CNTL1_BIT::CONTINUOUS_MODE_2 | CNTL1_BIT::BIT_16, 0 }, + }; +}; diff --git a/src/drivers/magnetometer/akm/ak8963/AKM_AK8963_registers.hpp b/src/drivers/magnetometer/akm/ak8963/AKM_AK8963_registers.hpp new file mode 100644 index 0000000000..2831c4c860 --- /dev/null +++ b/src/drivers/magnetometer/akm/ak8963/AKM_AK8963_registers.hpp @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 AKM_AK8963_registers.hpp + * + * Asahi Kasei Microdevices (AKM) AK8963 registers. + * + */ + +#pragma once + +#include + +namespace AKM_AK8963 +{ + +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +static constexpr uint8_t Bit7 = (1 << 7); + +static constexpr uint32_t I2C_SPEED = 400 * 1000; // 400 kHz I2C serial interface +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x0C; + +static constexpr uint8_t Device_ID = 0x48; // Device ID of AKM + +enum class Register : uint8_t { + WIA = 0x00, // Device ID + + ST1 = 0x02, // Status 1 + HXL = 0x03, + HXH = 0x04, + HYL = 0x05, + HYH = 0x06, + HZL = 0x07, + HZH = 0x08, + ST2 = 0x09, // Status 2 + CNTL1 = 0x0A, // Control 1 + CNTL2 = 0x0B, // Control 2 + + ASAX = 0x10, // X-axis sensitivity adjustment value + ASAY = 0x11, // Y-axis sensitivity adjustment value + ASAZ = 0x12, // Z-axis sensitivity adjustment value +}; + +// ST1 +enum ST1_BIT : uint8_t { + DRDY = Bit0, +}; + +// ST2 +enum ST2_BIT : uint8_t { + BITM = Bit4, // Output bit setting (mirror) + HOFL = Bit3, // Magnetic sensor overflow +}; + +// CNTL1 +enum CNTL1_BIT : uint8_t { + BIT_16 = Bit4, // Output bit setting (16-bit output) + + // MODE[3:0]: Operation mode setting + POWER_DOWN_MODE = 0, + SINGLE_MEASUREMENT_MODE = Bit0, + CONTINUOUS_MODE_1 = Bit1, // 8 Hz + CONTINUOUS_MODE_2 = Bit2 | Bit1, // 100 Hz + FUSE_ROM_ACCESS_MODE = Bit3 | Bit2 | Bit1 | Bit0, // MODE[3:0]=“1111” +}; + +// CNTL2 +enum CNTL2_BIT : uint8_t { + SRST = Bit0, // Reset +}; + +} // namespace AKM_AK8963 diff --git a/src/drivers/magnetometer/akm/ak8963/CMakeLists.txt b/src/drivers/magnetometer/akm/ak8963/CMakeLists.txt new file mode 100644 index 0000000000..c81e466669 --- /dev/null +++ b/src/drivers/magnetometer/akm/ak8963/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2020 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__magnetometer__akm__ak8963 + MAIN ak8963 + COMPILE_FLAGS + SRCS + AKM_AK8963_registers.hpp + AK8963.cpp + AK8963.hpp + ak8963_main.cpp + DEPENDS + drivers_magnetometer + px4_work_queue + ) diff --git a/src/drivers/magnetometer/akm/ak8963/ak8963_main.cpp b/src/drivers/magnetometer/akm/ak8963/ak8963_main.cpp new file mode 100644 index 0000000000..6251986932 --- /dev/null +++ b/src/drivers/magnetometer/akm/ak8963/ak8963_main.cpp @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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. + * + ****************************************************************************/ + +#include "AK8963.hpp" + +#include +#include + +I2CSPIDriverBase *AK8963::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) +{ + AK8963 *instance = new AK8963(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.rotation); + + if (!instance) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (instance->init() != PX4_OK) { + delete instance; + PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + return nullptr; + } + + return instance; +} + +void AK8963::print_usage() +{ + PRINT_MODULE_USAGE_NAME("ak8963", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" __EXPORT int ak8963_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = AK8963; + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = I2C_SPEED; + + while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + switch (ch) { + case 'R': + cli.rotation = (enum Rotation)atoi(cli.optarg()); + break; + } + } + + const char *verb = cli.optarg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_AK8963); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + } + + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +}