diff --git a/src/drivers/magnetometer/akm/ak09916/AK09916.cpp b/src/drivers/magnetometer/akm/ak09916/AK09916.cpp index 14573ccaa03b..be062e5e9f5e 100644 --- a/src/drivers/magnetometer/akm/ak09916/AK09916.cpp +++ b/src/drivers/magnetometer/akm/ak09916/AK09916.cpp @@ -78,7 +78,7 @@ bool AK09916::Reset() void AK09916::print_status() { I2CSPIDriverBase::print_status(); - PX4_INFO("Variant: %s", _is_ak09918 ? "AK09918" : "AK09916"); + PX4_INFO("Variant: %s", device_name()); perf_print_counter(_reset_perf); perf_print_counter(_bad_register_perf); @@ -94,23 +94,25 @@ int AK09916::probe() const uint8_t WIA1 = RegisterRead(Register::WIA1); const uint8_t WIA2 = RegisterRead(Register::WIA2); - if ((WIA1 == Company_ID) && (WIA2 == Device_ID)) { - _is_ak09918 = false; - return PX4_OK; - } - - if ((WIA1 == Company_ID) && (WIA2 == Device_ID_AK09918)) { - _is_ak09918 = true; - return PX4_OK; - } - - if (WIA1 != Company_ID) { + if ((WIA1 != Company_ID)) { PX4_DEBUG("unexpected WIA1 0x%02x", WIA1); + return PX4_ERROR; } - if (WIA2 != Device_ID && WIA2 != Device_ID_AK09918) { + switch(static_cast(WIA2)) { + case AKTYPE::AK09916: + _device = AKTYPE::AK09916; + return PX4_OK; + case AKTYPE::AK09915: + _device = AKTYPE::AK09915; + return PX4_OK; + case AKTYPE::AK09918: + _device = AKTYPE::AK09918; + return PX4_OK; + default: PX4_DEBUG("unexpected WIA2 0x%02x", WIA2); - } + + }; } return PX4_ERROR; @@ -132,9 +134,7 @@ void AK09916::RunImpl() break; case STATE::WAIT_FOR_RESET: { - uint8_t device_id = _is_ak09918 ? Device_ID_AK09918 : Device_ID; - - if ((RegisterRead(Register::WIA1) == Company_ID) && (RegisterRead(Register::WIA2) == device_id)) { + if ((RegisterRead(Register::WIA1) == Company_ID) && (static_cast(RegisterRead(Register::WIA2)) == _device)) { // if reset succeeded then configure _state = STATE::CONFIGURE; ScheduleDelayed(100_ms); diff --git a/src/drivers/magnetometer/akm/ak09916/AK09916.hpp b/src/drivers/magnetometer/akm/ak09916/AK09916.hpp index 49c92f445f3b..f735c00c575b 100644 --- a/src/drivers/magnetometer/akm/ak09916/AK09916.hpp +++ b/src/drivers/magnetometer/akm/ak09916/AK09916.hpp @@ -64,6 +64,26 @@ class AK09916 : public device::I2C, public I2CSPIDriver void print_status() override; private: + enum class AKTYPE : uint8_t { + AK09916 = 0X09, + AK09915 = 0X10, + AK09918 = 0x0c, + }; + + constexpr char const* device_name() { + switch(_device) { + case AKTYPE::AK09916: + return "AK09916"; + case AKTYPE::AK09915: + return "AK09915"; + case AKTYPE::AK09918: + return "AK09918"; + default: + return "Unknown"; + + }; + } + struct register_config_t { Register reg; uint8_t set_bits{0}; @@ -107,5 +127,5 @@ class AK09916 : public device::I2C, public I2CSPIDriver { Register::CNTL2, CNTL2_BIT::MODE3_SET, CNTL2_BIT::MODE3_CLEAR }, }; - bool _is_ak09918 {false}; + AKTYPE _device; }; diff --git a/src/drivers/magnetometer/akm/ak09916/AKM_AK09916_registers.hpp b/src/drivers/magnetometer/akm/ak09916/AKM_AK09916_registers.hpp index 0096e65a09dd..c408463db4cf 100644 --- a/src/drivers/magnetometer/akm/ak09916/AKM_AK09916_registers.hpp +++ b/src/drivers/magnetometer/akm/ak09916/AKM_AK09916_registers.hpp @@ -59,8 +59,6 @@ static constexpr uint32_t I2C_SPEED = 400 * 1000; // 400 kHz I2C serial interfac static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0b0001100; static constexpr uint8_t Company_ID = 0x48; -static constexpr uint8_t Device_ID = 0x09; -static constexpr uint8_t Device_ID_AK09918 = 0x0C; enum class Register : uint8_t { WIA1 = 0x00, // Company ID of AKM