From 60a7f7f9f2091d8949d8bc08446d3a5408010ca2 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 25 Aug 2020 13:46:04 -0400 Subject: [PATCH] isentek/ist8308: small style and consistency cleanup --- .../magnetometer/isentek/ist8308/IST8308.cpp | 140 ++++++++++-------- .../magnetometer/isentek/ist8308/IST8308.hpp | 27 ++-- .../isentek/ist8308/ist8308_main.cpp | 30 ++-- 3 files changed, 97 insertions(+), 100 deletions(-) diff --git a/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp b/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp index db6b56dc19..f00fd6e76e 100644 --- a/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp +++ b/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp @@ -40,7 +40,7 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) return (msb << 8u) | lsb; } -IST8308::IST8308(I2CSPIBusOption bus_option, int bus, enum Rotation rotation, int bus_frequency) : +IST8308::IST8308(I2CSPIBusOption bus_option, int bus, int bus_frequency, enum Rotation rotation) : I2C(DRV_MAG_DEVTYPE_IST8308, 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(), rotation) @@ -50,7 +50,7 @@ IST8308::IST8308(I2CSPIBusOption bus_option, int bus, enum Rotation rotation, in IST8308::~IST8308() { - perf_free(_transfer_perf); + perf_free(_reset_perf); perf_free(_bad_register_perf); perf_free(_bad_transfer_perf); } @@ -78,17 +78,18 @@ bool IST8308::Reset() void IST8308::print_status() { I2CSPIDriverBase::print_status(); - perf_print_counter(_transfer_perf); + + perf_print_counter(_reset_perf); perf_print_counter(_bad_register_perf); perf_print_counter(_bad_transfer_perf); } int IST8308::probe() { - const uint8_t whoami = RegisterRead(Register::WAI); + const uint8_t WAI = RegisterRead(Register::WAI); - if (whoami != Device_ID) { - DEVICE_DEBUG("unexpected WAI 0x%02x", whoami); + if (WAI != Device_ID) { + DEVICE_DEBUG("unexpected WAI 0x%02x", WAI); return PX4_ERROR; } @@ -97,13 +98,17 @@ int IST8308::probe() void IST8308::RunImpl() { + const hrt_abstime now = hrt_absolute_time(); + switch (_state) { case STATE::RESET: // CNTL3: Software Reset - RegisterSetAndClearBits(Register::CNTL3, CNTL3_BIT::SRST, 0); - _reset_timestamp = hrt_absolute_time(); + RegisterWrite(Register::CNTL3, CNTL3_BIT::SRST); + _reset_timestamp = now; + _failure_count = 0; _state = STATE::WAIT_FOR_RESET; - ScheduleDelayed(50_ms); // Power On Reset: max:50ms + perf_count(_reset_perf); + ScheduleDelayed(50_ms); // Power On Reset: max 50ms break; case STATE::WAIT_FOR_RESET: @@ -114,18 +119,18 @@ void IST8308::RunImpl() // if reset succeeded then configure _state = STATE::CONFIGURE; - ScheduleNow(); + ScheduleDelayed(10_ms); } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 100_ms) { - PX4_ERR("Reset failed, retrying"); + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; - ScheduleNow(); + ScheduleDelayed(100_ms); } else { - PX4_DEBUG("Reset not complete, check again in 50 ms"); - ScheduleDelayed(50_ms); + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); } } @@ -138,9 +143,16 @@ void IST8308::RunImpl() ScheduleOnInterval(20_ms, 20_ms); } else { - PX4_DEBUG("Configure failed, retrying"); - // try again in 50 ms - ScheduleDelayed(50_ms); + // 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; @@ -156,41 +168,52 @@ void IST8308::RunImpl() uint8_t DATAZH; } buffer{}; - perf_begin(_transfer_perf); + bool success = false; + uint8_t cmd = static_cast(Register::STAT); - bool failure = false; - const hrt_abstime timestamp_sample = hrt_absolute_time(); - const uint8_t cmd = static_cast(Register::STAT); + if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) { - if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) != PX4_OK) { + if (buffer.STAT && STAT_BIT::DRDY) { + int16_t x = combine(buffer.DATAXH, buffer.DATAXL); + int16_t y = combine(buffer.DATAYH, buffer.DATAYL); + int16_t z = combine(buffer.DATAZH, buffer.DATAZL); + + // sensor's frame is +x forward, +y right, +z up + z = (z == INT16_MIN) ? INT16_MAX : -z; // flip z + + _px4_mag.update(now, x, y, z); + + success = true; + + if (_failure_count > 0) { + _failure_count--; + } + } + + } else { perf_count(_bad_transfer_perf); - failure = true; } - perf_end(_transfer_perf); + if (!success) { + _failure_count++; - if (!failure && (buffer.STAT && STAT_BIT::DRDY)) { - float x = combine(buffer.DATAXH, buffer.DATAXL); - float y = combine(buffer.DATAYH, buffer.DATAYL); - float z = combine(buffer.DATAZH, buffer.DATAZL); - - // sensor's frame is +x forward, +y right, +z up - z = (z == INT16_MIN) ? INT16_MAX : -z; // flip z - - _px4_mag.update(timestamp_sample, x, y, z); + // full reset if things are failing consistently + if (_failure_count > 10) { + Reset(); + return; + } } - if (failure || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { - // check registers incrementally - if (RegisterCheck(_register_cfg[_checked_register], true)) { - _last_config_check_timestamp = timestamp_sample; + 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 = now; _checked_register = (_checked_register + 1) % size_register_cfg; } else { - // register check failed, force reconfigure - PX4_DEBUG("Health check failed, reconfiguring"); - _state = STATE::CONFIGURE; - ScheduleNow(); + // register check failed, force reset + perf_count(_bad_register_perf); + Reset(); } } } @@ -201,10 +224,16 @@ void IST8308::RunImpl() bool IST8308::Configure() { + // first set and clear all configured register bits + for (const auto ®_cfg : _register_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + // now check that all are configured bool success = true; - for (const auto ® : _register_cfg) { - if (!RegisterCheck(reg)) { + for (const auto ®_cfg : _register_cfg) { + if (!RegisterCheck(reg_cfg)) { success = false; } } @@ -215,7 +244,7 @@ bool IST8308::Configure() return success; } -bool IST8308::RegisterCheck(const register_config_t ®_cfg, bool notify) +bool IST8308::RegisterCheck(const register_config_t ®_cfg) { bool success = true; @@ -231,15 +260,6 @@ bool IST8308::RegisterCheck(const register_config_t ®_cfg, bool notify) success = false; } - if (!success) { - RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); - - if (notify) { - perf_count(_bad_register_perf); - _px4_mag.increase_error_count(); - } - } - return success; } @@ -260,15 +280,9 @@ void IST8308::RegisterWrite(Register reg, uint8_t value) void IST8308::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) { const uint8_t orig_val = RegisterRead(reg); - uint8_t val = orig_val; + uint8_t val = (orig_val & ~clearbits) | setbits; - if (setbits) { - val |= setbits; + if (orig_val != val) { + RegisterWrite(reg, val); } - - if (clearbits) { - val &= ~clearbits; - } - - RegisterWrite(reg, val); } diff --git a/src/drivers/magnetometer/isentek/ist8308/IST8308.hpp b/src/drivers/magnetometer/isentek/ist8308/IST8308.hpp index b3d1b3ed81..9d55ac59b1 100644 --- a/src/drivers/magnetometer/isentek/ist8308/IST8308.hpp +++ b/src/drivers/magnetometer/isentek/ist8308/IST8308.hpp @@ -46,7 +46,6 @@ #include #include #include -#include #include using namespace iSentek_IST8308; @@ -54,25 +53,19 @@ using namespace iSentek_IST8308; class IST8308 : public device::I2C, public I2CSPIDriver { public: - IST8308(I2CSPIBusOption bus_option, int bus, enum Rotation rotation, int bus_frequency); + IST8308(I2CSPIBusOption bus_option, int bus, int bus_frequency, enum Rotation rotation = ROTATION_NONE); ~IST8308() override; static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, int runtime_instance); static void print_usage(); - void print_status(); + void RunImpl(); int init() override; - - void Start(); - bool Reset(); - - void RunImpl(); + void print_status() override; private: - void custom_method(const BusCLIArguments &cli) override; - // Sensor Configuration struct register_config_t { Register reg; @@ -82,9 +75,11 @@ private: int probe() override; + bool Reset(); + bool Configure(); - bool RegisterCheck(const register_config_t ®_cfg, bool notify = false); + bool RegisterCheck(const register_config_t ®_cfg); uint8_t RegisterRead(Register reg); void RegisterWrite(Register reg, uint8_t value); @@ -92,24 +87,22 @@ private: 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 _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")}; hrt_abstime _reset_timestamp{0}; hrt_abstime _last_config_check_timestamp{0}; - - uint8_t _checked_register{0}; + int _failure_count{0}; enum class STATE : uint8_t { RESET, WAIT_FOR_RESET, CONFIGURE, READ, - }; - - STATE _state{STATE::RESET}; + } _state{STATE::RESET}; + uint8_t _checked_register{0}; static constexpr uint8_t size_register_cfg{5}; register_config_t _register_cfg[size_register_cfg] { // Register | Set bits, Clear bits diff --git a/src/drivers/magnetometer/isentek/ist8308/ist8308_main.cpp b/src/drivers/magnetometer/isentek/ist8308/ist8308_main.cpp index 126e2fb0ea..839d1f303b 100644 --- a/src/drivers/magnetometer/isentek/ist8308/ist8308_main.cpp +++ b/src/drivers/magnetometer/isentek/ist8308/ist8308_main.cpp @@ -36,39 +36,33 @@ #include #include -void -IST8308::print_usage() -{ - PRINT_MODULE_USAGE_NAME("ist8308", "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_COMMAND("reset"); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); -} - I2CSPIDriverBase *IST8308::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, int runtime_instance) { - IST8308 *instance = new IST8308(iterator.configuredBusOption(), iterator.bus(), cli.rotation, cli.bus_frequency); + IST8308 *instance = new IST8308(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.rotation); if (!instance) { PX4_ERR("alloc failed"); return nullptr; } - if (OK != instance->init()) { + 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 IST8308::custom_method(const BusCLIArguments &cli) +void IST8308::print_usage() { - Reset(); + PRINT_MODULE_USAGE_NAME("ist8308", "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" int ist8308_main(int argc, char *argv[]) @@ -107,10 +101,6 @@ extern "C" int ist8308_main(int argc, char *argv[]) return ThisDriver::module_status(iterator); } - if (!strcmp(verb, "reset")) { - return ThisDriver::module_custom_method(cli, iterator); - } - ThisDriver::print_usage(); return -1; }