dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ak09916: PX4_ERR if reset/configure is failing after 5s

This commit is contained in:
Daniel Agar 2022-09-22 20:17:00 -04:00
parent 16b4149492
commit eb2a714aa5
5 changed files with 69 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

103
src/drivers/magnetometer/akm/ak09916/AK09916.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-2022 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
@ -50,7 +50,6 @@ AK09916::AK09916(const I2CSPIDriverConfig &config) :
AK09916::~AK09916()
{
perf_free(_reset_perf);
perf_free(_transfer_perf);
perf_free(_bad_register_perf);
perf_free(_bad_transfer_perf);
perf_free(_magnetic_sensor_overflow_perf);
@ -65,8 +64,6 @@ int AK09916::init()
return ret;
}
_retries = 2;
return Reset() ? 0 : -1;
}
@ -83,7 +80,6 @@ void AK09916::print_status()
I2CSPIDriverBase::print_status();
perf_print_counter(_reset_perf);
perf_print_counter(_transfer_perf);
perf_print_counter(_bad_register_perf);
perf_print_counter(_bad_transfer_perf);
perf_print_counter(_magnetic_sensor_overflow_perf);
@ -91,32 +87,39 @@ void AK09916::print_status()
int AK09916::probe()
{
const uint8_t WIA1 = RegisterRead(Register::WIA1);
// 3 retries
for (int i = 0; i < 3; i++) {
if (WIA1 != Company_ID) {
DEVICE_DEBUG("unexpected WIA1 0x%02x", WIA1);
return PX4_ERROR;
const uint8_t WIA1 = RegisterRead(Register::WIA1);
const uint8_t WIA2 = RegisterRead(Register::WIA2);
if ((WIA1 == Company_ID) && (WIA2 == Device_ID)) {
return PX4_OK;
}
if (WIA1 != Company_ID) {
DEVICE_DEBUG("unexpected WIA1 0x%02x", WIA1);
}
if (WIA2 != Device_ID) {
DEVICE_DEBUG("unexpected WIA2 0x%02x", WIA2);
}
}
const uint8_t WIA2 = RegisterRead(Register::WIA2);
if (WIA2 != Device_ID) {
DEVICE_DEBUG("unexpected WIA2 0x%02x", WIA2);
return PX4_ERROR;
}
return PX4_OK;
return PX4_ERROR;
}
void AK09916::RunImpl()
{
const hrt_abstime now = hrt_absolute_time();
switch (_state) {
case STATE::RESET:
perf_count(_reset_perf);
// CNTL3 SRST: Soft reset
RegisterWrite(Register::CNTL3, CNTL3_BIT::SRST);
_reset_timestamp = hrt_absolute_time();
_consecutive_failures = 0;
_reset_timestamp = now;
_failure_count = 0;
_state = STATE::WAIT_FOR_RESET;
ScheduleDelayed(100_ms);
break;
@ -124,16 +127,14 @@ void AK09916::RunImpl()
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);
if (hrt_elapsed_time(&_reset_timestamp) > 30_s) {
PX4_ERR("Reset failed, retrying");
Reset();
} else {
PX4_DEBUG("Reset not complete, check again in 100 ms");
@ -151,26 +152,34 @@ void AK09916::RunImpl()
} else {
// CONFIGURE not complete
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Configure failed, resetting");
_state = STATE::RESET;
if (hrt_elapsed_time(&_reset_timestamp) > 30_s) {
PX4_ERR("Configure failed, resetting");
Reset();
} else {
PX4_DEBUG("Configure failed, retrying");
ScheduleDelayed(100_ms);
}
ScheduleDelayed(100_ms);
}
break;
case STATE::READ: {
perf_begin(_transfer_perf);
TransferBuffer buffer{};
const hrt_abstime timestamp_sample = hrt_absolute_time();
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;
} buffer{};
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;
@ -186,31 +195,39 @@ void AK09916::RunImpl()
// sensor's frame is +X forward (X), +Y right (Y), +Z down (Z)
_px4_mag.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf)
+ perf_event_count(_magnetic_sensor_overflow_perf));
_px4_mag.update(timestamp_sample, x, y, z);
_px4_mag.update(now, x, y, z);
success = true;
_consecutive_failures = 0;
if (_failure_count > 0) {
_failure_count--;
}
}
}
if (!success) {
_failure_count++;
// full reset if things are failing consistently
if (_failure_count > 10) {
Reset();
return;
}
}
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;
_last_config_check_timestamp = now;
_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;
@ -219,9 +236,11 @@ void AK09916::RunImpl()
bool AK09916::Configure()
{
_retries = 2;
// first set and clear all configured register bits
for (const auto &reg_cfg : _register_cfg) {
RegisterWrite(reg_cfg.reg, reg_cfg.set_bits);
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
}
// now check that all are configured

20
src/drivers/magnetometer/akm/ak09916/AK09916.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-2022 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
@ -64,19 +64,6 @@ public:
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};
@ -98,14 +85,13 @@ private:
PX4Magnetometer _px4_mag;
perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
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};
int _failure_count{0};
enum class STATE : uint8_t {
RESET,
@ -118,6 +104,6 @@ private:
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 },
{ Register::CNTL2, CNTL2_BIT::MODE3_SET, CNTL2_BIT::MODE3_CLEAR },
};
};

8
src/drivers/magnetometer/akm/ak09916/AKM_AK09916_registers.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-2022 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
@ -93,10 +93,8 @@ enum ST2_BIT : uint8_t {
// 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)
MODE3_SET = Bit2 | Bit1, // “00110”: Continuous measurement mode 3 (50Hz)
MODE3_CLEAR = Bit4 | Bit3 | Bit0,
};
// CNTL3

2
src/drivers/magnetometer/akm/ak09916/CMakeLists.txt
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
# Copyright (c) 2019-2022 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

2
src/drivers/magnetometer/akm/ak09916/ak09916_main.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020, 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-2022 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