fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 18:10:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

magnetometers/akm: new standalone AKM AK8963 & AK09916 drivers

Browse Source
This commit is contained in:
Daniel Agar
2020-06-18 11:16:31 -04:00
parent 8c34f47b3d
commit 4bcee8e13f
13 changed files with 1309 additions and 373 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/magnetometer/CMakeLists.txt
+1 -1
View File
@@ -31,7 +31,7 @@
#
############################################################################
add_subdirectory(ak09916)
add_subdirectory(akm)
add_subdirectory(bmm150)
add_subdirectory(hmc5883)
add_subdirectory(qmc5883)
src/drivers/magnetometer/ak09916/ak09916.cpp
-304
View File
@@ -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 <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/time.h>
#include <lib/perf/perf_counter.h>
#include <drivers/drv_hrt.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#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<uint8_t *>(&regs), 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;
}
src/drivers/magnetometer/akm/CMakeLists.txt
+35
View File
@@ -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)
src/drivers/magnetometer/akm/ak09916/AK09916.cpp
+279
View File
@@ -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<uint8_t>(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 &reg_cfg : _register_cfg) {
RegisterWrite(reg_cfg.reg, reg_cfg.set_bits);
}
// now check that all are configured
bool success = true;
for (const auto &reg_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 &reg_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<uint8_t>(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);
}
}
src/drivers/magnetometer/ak09916/ak09916.hpp → src/drivers/magnetometer/akm/ak09916/AK09916.hpp
+68 -64
View File
@@ -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 <px4_platform_common/px4_config.h>
#include <lib/perf/perf_counter.h>
#include <systemlib/conversions.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/i2c.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <lib/drivers/device/i2c.h>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/i2c_spi_buses.h>
// 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<AK09916>
{
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 &reg_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 },
};
};
src/drivers/magnetometer/akm/ak09916/AKM_AK09916_registers.hpp
+107
View File
@@ -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 <cstdint>
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
src/drivers/magnetometer/ak09916/CMakeLists.txt → src/drivers/magnetometer/akm/ak09916/CMakeLists.txt
+7 -4
View File
@@ -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
)
src/drivers/magnetometer/akm/ak09916/ak09916_main.cpp
+106
View File
@@ -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 <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
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;
}
src/drivers/magnetometer/akm/ak8963/AK8963.cpp
+317
View File
@@ -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<uint8_t>(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<uint8_t>(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 &reg_cfg : _register_cfg) {
RegisterWrite(reg_cfg.reg, reg_cfg.set_bits);
}
// now check that all are configured
bool success = true;
for (const auto &reg_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 &reg_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<uint8_t>(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);
}
}
src/drivers/magnetometer/akm/ak8963/AK8963.hpp
+127
View File
@@ -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 <drivers/drv_hrt.h>
#include <lib/drivers/device/i2c.h>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/i2c_spi_buses.h>
using namespace AKM_AK8963;
class AK8963 : public device::I2C, public I2CSPIDriver<AK8963>
{
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 &reg_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 },
};
};
src/drivers/magnetometer/akm/ak8963/AKM_AK8963_registers.hpp
+110
View File
@@ -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 <cstdint>
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
src/drivers/magnetometer/akm/ak8963/CMakeLists.txt
+46
View File
@@ -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
)
src/drivers/magnetometer/akm/ak8963/ak8963_main.cpp
+106
View File
@@ -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 <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
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;
}