diff --git a/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp index b7d8512ce9..d605dc7675 100644 --- a/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp +++ b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -117,7 +117,7 @@ void IST8310::RunImpl() && ((RegisterRead(Register::CNTL2) & CNTL2_BIT::SRST) == 0)) { // if reset succeeded then configure - _state = STATE::CONFIGURE; + _state = STATE::SELF_TEST_CHECK; ScheduleDelayed(10_ms); } else { @@ -135,6 +135,129 @@ void IST8310::RunImpl() break; + case STATE::SELF_TEST_CHECK: { + + // Self-Test mode is used to check if the 3-axis outputs read in Single Measurement Mode are correct. + // It is activated by setting Self-Test Register, STR(0x0Ch) to 0x40h; then all 3-axis outputs will change their polarity. + RegisterWrite(Register::TCCNTL, 0x0); + RegisterWrite(Register::CNTL2, CNTL2_BIT::DREN); + + + + // User can check the 3-axis output values before and after activating Self-Test Mode; + // if the absolute values are the same, then the IC is working correctly. + RegisterWrite(Register::CNTL1, CNTL1_BIT::MODE_SINGLE_MEASUREMENT); + + px4_usleep(5000); + + + // read STAT1 until DRDY? + + int16_t read_0[3] {}; + + { + struct TransferBuffer { + uint8_t STAT1; + uint8_t DATAXL; + uint8_t DATAXH; + uint8_t DATAYL; + uint8_t DATAYH; + uint8_t DATAZL; + uint8_t DATAZH; + } buffer{}; + + uint8_t cmd = static_cast(Register::STAT1); + + if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) { + if (buffer.STAT1 & STAT1_BIT::DRDY) { + read_0[0] = combine(buffer.DATAXH, buffer.DATAXL); + read_0[1] = combine(buffer.DATAYH, buffer.DATAYL); + read_0[2] = combine(buffer.DATAZH, buffer.DATAZL); + + } else { + PX4_ERR("read 0 data not ready"); + } + + } + } + + + // activate self test + RegisterSetAndClearBits(Register::STR, STR_BIT::SELF_TEST, 0); + + + + // read again + + + + // read 2 + RegisterWrite(Register::CNTL1, CNTL1_BIT::MODE_SINGLE_MEASUREMENT); + + px4_usleep(5000); + + int16_t read_1[3] {}; + + { + struct TransferBuffer { + uint8_t STAT1; + uint8_t DATAXL; + uint8_t DATAXH; + uint8_t DATAYL; + uint8_t DATAYH; + uint8_t DATAZL; + uint8_t DATAZH; + } buffer{}; + + uint8_t cmd = static_cast(Register::STAT1); + + if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) { + + if (buffer.STAT1 & STAT1_BIT::DRDY) { + read_1[0] = combine(buffer.DATAXH, buffer.DATAXL); + read_1[1] = combine(buffer.DATAYH, buffer.DATAYL); + read_1[2] = combine(buffer.DATAZH, buffer.DATAZL); + + } else { + PX4_ERR("read 1 data not ready"); + } + } + } + + + + PX4_INFO("self test X %d -> %d", read_0[0], read_1[0]); + PX4_INFO("self test Y %d -> %d", read_0[1], read_1[1]); + PX4_INFO("self test Z %d -> %d", read_0[2], read_1[2]); + + + + + + + + // It can be turned off by setting STR(0x0Ch) to 0x00h. + RegisterSetAndClearBits(Register::STR, 0, STR_BIT::SELF_TEST); + + + // Please set Temperature Compensation Control Register, TCCNTL (0x40h) to 0x01h to disable temperature + // compensation function to avoid wrong compensation while using this self-test function and set it back to 0x00h in real measurement. + + + // User can check the 3-axis output values before and after activating Self-Test Mode + + // 0x01h into Control register 1, CNTL1(0x0Ah), IST8310 enters Single Measurement + + + // the minimum waiting time between two measurements is 5ms (ODR=200Hz) + + + _state = STATE::CONFIGURE; + ScheduleDelayed(10_ms); + + } + break; + case STATE::CONFIGURE: if (Configure()) { // if configure succeeded then start measurement cycle @@ -184,7 +307,7 @@ void IST8310::RunImpl() 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 + z = math::negate(z); // flip z _px4_mag.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf)); _px4_mag.update(now, x, y, z); diff --git a/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp b/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp index a251f49cf0..b5d9e7f9c9 100644 --- a/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp +++ b/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -96,17 +96,20 @@ private: enum class STATE : uint8_t { RESET, WAIT_FOR_RESET, + SELF_TEST_CHECK, CONFIGURE, MEASURE, READ, } _state{STATE::RESET}; uint8_t _checked_register{0}; - static constexpr uint8_t size_register_cfg{4}; + static constexpr uint8_t size_register_cfg{6}; register_config_t _register_cfg[size_register_cfg] { // Register | Set bits, Clear bits - { Register::CNTL2, 0, CNTL2_BIT::SRST }, + { Register::CNTL2, CNTL2_BIT::DREN, CNTL2_BIT::SRST }, + { Register::STR, 0, STR_BIT::SELF_TEST }, { Register::CNTL3, CNTL3_BIT::Z_16BIT | CNTL3_BIT::Y_16BIT | CNTL3_BIT::X_16BIT, 0 }, + { Register::TCCNTL, 0, TCCNTL_BIT::TEMP_COMP_DIS }, { Register::AVGCNTL, AVGCNTL_BIT::Y_16TIMES_SET | AVGCNTL_BIT::XZ_16TIMES_SET, AVGCNTL_BIT::Y_16TIMES_CLEAR | AVGCNTL_BIT::XZ_16TIMES_CLEAR }, { Register::PDCNTL, PDCNTL_BIT::PULSE_NORMAL, 0 }, }; diff --git a/src/drivers/magnetometer/isentek/ist8310/iSentek_IST8310_registers.hpp b/src/drivers/magnetometer/isentek/ist8310/iSentek_IST8310_registers.hpp index 2a49ee03e9..22150c154b 100644 --- a/src/drivers/magnetometer/isentek/ist8310/iSentek_IST8310_registers.hpp +++ b/src/drivers/magnetometer/isentek/ist8310/iSentek_IST8310_registers.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -116,6 +116,7 @@ enum CNTL1_BIT : uint8_t { // CNTL2 enum CNTL2_BIT : uint8_t { + DREN = Bit3, // Data ready enable control SRST = Bit0, // Soft reset, perform the same routine as POR }; @@ -131,6 +132,11 @@ enum CNTL3_BIT : uint8_t { X_16BIT = Bit4, // Sensor output resolution adjustment for X axis: 16-bit (Sensitivity: 1320 LSB/Gauss) }; +// TCCNTL +enum TCCNTL_BIT : uint8_t { + TEMP_COMP_DIS = Bit0, // Set to ‘1’ to disable temperature compensation function +}; + // AVGCNTL enum AVGCNTL_BIT : uint8_t { // 5:3 Average times for y sensor data. Times of average will be done before switch to next channel