From 00a7ac013c17824590239d02119014078d8e6632 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 12 Nov 2019 11:33:16 +0100 Subject: [PATCH] ak09916: fix mag spikes This fixes spuriously occuring mag spikes in the external mag of Here2. The reason for the spikes was that the fact that the I2C registers were not read out correctly as suggested in the datasheet. Before we were reading out ST1, data, and ST2 in one pass and ignoring the data ready bit (DRDY) in ST1. This meant that we could run into race conditions where the data was not ready when we started reading and being updated as the registers are read. Instead, we need to check the read the ST1 register first to check the data ready bit and then read the data and ST2 if the data is ready. By reading ST2 we then trigger the next measurement in the chip. --- src/drivers/magnetometer/ak09916/ak09916.cpp | 74 ++++++++++---------- src/drivers/magnetometer/ak09916/ak09916.hpp | 17 +++-- 2 files changed, 46 insertions(+), 45 deletions(-) diff --git a/src/drivers/magnetometer/ak09916/ak09916.cpp b/src/drivers/magnetometer/ak09916/ak09916.cpp index 7d25358843..b0b302977b 100644 --- a/src/drivers/magnetometer/ak09916/ak09916.cpp +++ b/src/drivers/magnetometer/ak09916/ak09916.cpp @@ -141,8 +141,7 @@ AK09916::AK09916(int bus, const char *path, enum Rotation rotation) : _mag_reads(perf_alloc(PC_COUNT, "ak09916_mag_reads")), _mag_errors(perf_alloc(PC_COUNT, "ak09916_mag_errors")), _mag_overruns(perf_alloc(PC_COUNT, "ak09916_mag_overruns")), - _mag_overflows(perf_alloc(PC_COUNT, "ak09916_mag_overflows")), - _mag_duplicates(perf_alloc(PC_COUNT, "ak09916_mag_duplicates")) + _mag_overflows(perf_alloc(PC_COUNT, "ak09916_mag_overflows")) { _px4_mag.set_device_type(DRV_MAG_DEVTYPE_AK09916); _px4_mag.set_scale(AK09916_MAG_RANGE_GA); @@ -154,7 +153,6 @@ AK09916::~AK09916() perf_free(_mag_errors); perf_free(_mag_overruns); perf_free(_mag_overflows); - perf_free(_mag_duplicates); } int @@ -178,52 +176,56 @@ AK09916::init() return PX4_OK; } -bool AK09916::check_duplicate(uint8_t *mag_data) +void +AK09916::try_measure() { - if (memcmp(mag_data, &_last_mag_data, sizeof(_last_mag_data)) == 0) { - // It isn't new data - wait for next timer. - return true; + if (!is_ready()) { + return; + } - } else { - memcpy(&_last_mag_data, mag_data, sizeof(_last_mag_data)); + 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() { - uint8_t cmd = AK09916REG_ST1; - struct ak09916_regs raw_data; + ak09916_regs regs; - const hrt_abstime timestamp_sample = hrt_absolute_time(); - uint8_t ret = transfer(&cmd, 1, (uint8_t *)(&raw_data), sizeof(struct ak09916_regs)); + const hrt_abstime now = hrt_absolute_time(); - if (ret == OK) { - raw_data.st2 = raw_data.st2; - - if (check_duplicate((uint8_t *)&raw_data.x) && !(raw_data.st1 & 0x02)) { - perf_count(_mag_duplicates); - return; - } - - /* Monitor for if data overrun flag is ever set. */ - if (raw_data.st1 & 0x02) { - perf_count(_mag_overruns); - } - - /* Monitor for if magnetic sensor overflow flag is ever set noting that st2 - * is usually not even refreshed, but will always be in the same place in the - * mpu's buffers regardless, hence the actual count would be bogus. - */ - if (raw_data.st2 & 0x08) { - perf_count(_mag_overflows); - } + 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)); - _px4_mag.set_external(external()); - _px4_mag.update(timestamp_sample, raw_data.x, raw_data.y, raw_data.z); + 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 @@ -327,7 +329,7 @@ AK09916::Run() return; } - measure(); + try_measure(); if (_cycle_interval > 0) { ScheduleDelayed(_cycle_interval); diff --git a/src/drivers/magnetometer/ak09916/ak09916.hpp b/src/drivers/magnetometer/ak09916/ak09916.hpp index 40f2f8ebb7..ba195030df 100644 --- a/src/drivers/magnetometer/ak09916/ak09916.hpp +++ b/src/drivers/magnetometer/ak09916/ak09916.hpp @@ -54,18 +54,23 @@ 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 { - uint8_t st1; int16_t x; int16_t y; int16_t z; @@ -84,7 +89,6 @@ public: virtual int init() override; void start(); void stop(); - void cycle(); void print_info(); int probe(); @@ -93,6 +97,8 @@ protected: int setup_master_i2c(); bool check_id(); void Run(); + void try_measure(); + bool is_ready(); void measure(); int reset(); @@ -111,13 +117,6 @@ private: perf_counter_t _mag_overruns; perf_counter_t _mag_overflows; - perf_counter_t _mag_duplicates; - - bool check_duplicate(uint8_t *mag_data); - - // keep last mag reading for duplicate detection - uint8_t _last_mag_data[6] {}; - AK09916(const AK09916 &) = delete; AK09916 operator=(const AK09916 &) = delete; };