diff --git a/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp index e5baf14ac5..58df6e8d23 100644 --- a/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp +++ b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp @@ -35,6 +35,10 @@ using namespace time_literals; +#define IST8310_TEMP_RESOLUTION (1/67.2) +#define IST8310_TEMP_25C_OFFSET 27100 +#define IST8310_OTP_SENSITIVITY 330 + static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; @@ -81,6 +85,21 @@ void IST8310::print_status() perf_print_counter(_reset_perf); perf_print_counter(_bad_register_perf); perf_print_counter(_bad_transfer_perf); + print_cross_axis_info(); +} + +void IST8310::print_cross_axis_info() +{ + PX4_INFO("cross axis cal: [%+3.2f,%+3.2f,%+3.2f|%+3.2f,%+3.2f,%+3.2f|%+3.2f,%+3.2f,%+3.2f]", + (double)_crossaxis_inv[0], + (double)_crossaxis_inv[1], + (double)_crossaxis_inv[2], + (double)_crossaxis_inv[3], + (double)_crossaxis_inv[4], + (double)_crossaxis_inv[5], + (double)_crossaxis_inv[6], + (double)_crossaxis_inv[7], + (double)_crossaxis_inv[8]); } int IST8310::probe() @@ -190,21 +209,34 @@ void IST8310::RunImpl() uint8_t DATAZH; } buffer{}; + struct Temp { + uint8_t TEMPL; + uint8_t TEMPH; + } buffer_temp; + bool success = false; uint8_t cmd = static_cast(Register::STAT1); + uint8_t cmd_temp = static_cast(Register::TEMPL); - if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) { + if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK + && transfer(&cmd_temp, 1, (uint8_t *)&buffer_temp, sizeof(buffer_temp)) == PX4_OK) { if (buffer.STAT1 & STAT1_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); + int16_t t = combine(buffer_temp.TEMPH, buffer_temp.TEMPL); - // sensor's frame is +x forward, +y right, +z up - z = (z == INT16_MIN) ? INT16_MAX : -z; // flip z + _px4_mag.set_temperature((IST8310_TEMP_25C_OFFSET - t) * IST8310_TEMP_RESOLUTION + 25); + + float xf = (float)x * _crossaxis_inv[0] + (float)y * _crossaxis_inv[1] + (float)z * _crossaxis_inv[2]; + float yf = (float)x * _crossaxis_inv[3] + (float)y * _crossaxis_inv[4] + (float)z * _crossaxis_inv[5]; + float zf = (float)x * _crossaxis_inv[6] + (float)y * _crossaxis_inv[7] + (float)z * _crossaxis_inv[8]; _px4_mag.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf)); - _px4_mag.update(now, x, y, z); + + // sensor's frame is +x forward, +y right, +z up + _px4_mag.update(now, xf, yf, -zf); success = true; @@ -268,9 +300,158 @@ bool IST8310::Configure() _px4_mag.set_scale(1.f / 1320.f); // 1320 LSB/Gauss + /* Pull & process cross-axis compensation */ + cross_axis_comp(); + return success; } +void +IST8310::cross_axis_comp() +{ + int ret; + uint8_t bOTPDataFlags; + uint8_t tempBuff[2]; + bool crossaxis_enable = 0; + float inv[9] = {0}; + short OTPcrossaxis[9] = {0}; + + uint8_t crossxbuf[6]; + uint8_t crossybuf[6]; + uint8_t crosszbuf[6]; + + update_crossaxis(); + + //check otp_date (Sensors before some date appears to have the matrix stored transposed) + bOTPDataFlags = 0; + + ret = read(Register::OTP, (uint8_t *)&tempBuff, sizeof(tempBuff)); + + if (OK != ret) { + perf_count(_bad_transfer_perf); + return; + } + + + if (tempBuff[0] == 0xff) { + bOTPDataFlags = 0; + + } else if (tempBuff[0] <= 0x12) { + if (tempBuff[1] <= 0x07) { + bOTPDataFlags = 1; + } + } + + ret = read(Register::XX_CROSS_L, (uint8_t *)&tempBuff, sizeof(tempBuff)); + + if (OK != ret) { + perf_count(_bad_transfer_perf); + return; + } + + if ((tempBuff[0] == 0xFF) && (tempBuff[1] == 0xFF)) { + crossaxis_enable = 0; + + } else { + crossaxis_enable = 1; + } + + if (crossaxis_enable == 0) { + update_crossaxis(); + return; + + } else { + ret = read(Register::XX_CROSS_L, (uint8_t *)&crossxbuf, sizeof(crossxbuf)); + + if (OK != ret) { + perf_count(_bad_transfer_perf); + return; + } + + ret = read(Register::YX_CROSS_L, (uint8_t *)&crossybuf, sizeof(crossybuf)); + + if (OK != ret) { + perf_count(_bad_transfer_perf); + return; + } + + ret = read(Register::ZX_CROSS_L, (uint8_t *)&crosszbuf, sizeof(crosszbuf)); + + if (OK != ret) { + perf_count(_bad_transfer_perf); + return; + } + + if (bOTPDataFlags) { + //before + OTPcrossaxis[0] = ((int16_t) crossxbuf[1]) << 8 | crossxbuf[0]; + OTPcrossaxis[1] = ((int16_t) crossxbuf[3]) << 8 | crossxbuf[2]; + OTPcrossaxis[2] = ((int16_t) crossxbuf[5]) << 8 | crossxbuf[4]; + OTPcrossaxis[3] = ((int16_t) crossybuf[1]) << 8 | crossybuf[0]; + OTPcrossaxis[4] = ((int16_t) crossybuf[3]) << 8 | crossybuf[2]; + OTPcrossaxis[5] = ((int16_t) crossybuf[5]) << 8 | crossybuf[4]; + OTPcrossaxis[6] = ((int16_t) crosszbuf[1]) << 8 | crosszbuf[0]; + OTPcrossaxis[7] = ((int16_t) crosszbuf[3]) << 8 | crosszbuf[2]; + OTPcrossaxis[8] = ((int16_t) crosszbuf[5]) << 8 | crosszbuf[4]; + + } else { + //after + OTPcrossaxis[0] = ((int16_t) crossxbuf[1]) << 8 | crossxbuf[0]; + OTPcrossaxis[3] = ((int16_t) crossxbuf[3]) << 8 | crossxbuf[2]; + OTPcrossaxis[6] = ((int16_t) crossxbuf[5]) << 8 | crossxbuf[4]; + OTPcrossaxis[1] = ((int16_t) crossybuf[1]) << 8 | crossybuf[0]; + OTPcrossaxis[4] = ((int16_t) crossybuf[3]) << 8 | crossybuf[2]; + OTPcrossaxis[7] = ((int16_t) crossybuf[5]) << 8 | crossybuf[4]; + OTPcrossaxis[2] = ((int16_t) crosszbuf[1]) << 8 | crosszbuf[0]; + OTPcrossaxis[5] = ((int16_t) crosszbuf[3]) << 8 | crosszbuf[2]; + OTPcrossaxis[8] = ((int16_t) crosszbuf[5]) << 8 | crosszbuf[4]; + } + + _crossaxis_det = ((int32_t)OTPcrossaxis[0]) * ((int32_t)OTPcrossaxis[4]) * ((int32_t)OTPcrossaxis[8]) + + ((int32_t)OTPcrossaxis[1]) * ((int32_t)OTPcrossaxis[5]) * ((int32_t)OTPcrossaxis[6]) + + ((int32_t)OTPcrossaxis[2]) * ((int32_t)OTPcrossaxis[3]) * ((int32_t)OTPcrossaxis[7]) - + ((int32_t)OTPcrossaxis[0]) * ((int32_t)OTPcrossaxis[5]) * ((int32_t)OTPcrossaxis[7]) - + ((int32_t)OTPcrossaxis[2]) * ((int32_t)OTPcrossaxis[4]) * ((int32_t)OTPcrossaxis[6]) - + ((int32_t)OTPcrossaxis[1]) * ((int32_t)OTPcrossaxis[3]) * ((int32_t)OTPcrossaxis[8]); + + if (_crossaxis_det == 0) { + update_crossaxis(); + } + + inv[0] = (float)OTPcrossaxis[4] * (float)OTPcrossaxis[8] - (float)OTPcrossaxis[5] * (float)OTPcrossaxis[7]; + inv[1] = (float)OTPcrossaxis[2] * (float)OTPcrossaxis[7] - (float)OTPcrossaxis[1] * (float)OTPcrossaxis[8]; + inv[2] = (float)OTPcrossaxis[1] * (float)OTPcrossaxis[5] - (float)OTPcrossaxis[2] * (float)OTPcrossaxis[4]; + inv[3] = (float)OTPcrossaxis[5] * (float)OTPcrossaxis[6] - (float)OTPcrossaxis[3] * (float)OTPcrossaxis[8]; + inv[4] = (float)OTPcrossaxis[0] * (float)OTPcrossaxis[8] - (float)OTPcrossaxis[2] * (float)OTPcrossaxis[6]; + inv[5] = (float)OTPcrossaxis[2] * (float)OTPcrossaxis[3] - (float)OTPcrossaxis[0] * (float)OTPcrossaxis[5]; + inv[6] = (float)OTPcrossaxis[3] * (float)OTPcrossaxis[7] - (float)OTPcrossaxis[4] * (float)OTPcrossaxis[6]; + inv[7] = (float)OTPcrossaxis[1] * (float)OTPcrossaxis[6] - (float)OTPcrossaxis[0] * (float)OTPcrossaxis[7]; + inv[8] = (float)OTPcrossaxis[0] * (float)OTPcrossaxis[4] - (float)OTPcrossaxis[1] * (float)OTPcrossaxis[3]; + + for (int i = 0; i < 9; i++) { + _crossaxis_inv[i] = inv[i] * ((float)IST8310_OTP_SENSITIVITY) / ((float)_crossaxis_det); + } + + } + + return; +} + +void +IST8310::update_crossaxis() +{ + *_crossaxis_inv = 1; + *(_crossaxis_inv + 1) = 0; + *(_crossaxis_inv + 2) = 0; + *(_crossaxis_inv + 3) = 0; + *(_crossaxis_inv + 4) = 1; + *(_crossaxis_inv + 5) = 0; + *(_crossaxis_inv + 6) = 0; + *(_crossaxis_inv + 7) = 0; + *(_crossaxis_inv + 8) = 1; + _crossaxis_det = 1; +} + bool IST8310::RegisterCheck(const register_config_t ®_cfg) { bool success = true; @@ -298,6 +479,12 @@ uint8_t IST8310::RegisterRead(Register reg) return buffer; } +int IST8310::read(Register reg, void *data, unsigned count) +{ + const uint8_t cmd = static_cast(reg); + return transfer(&cmd, 1, (uint8_t *)data, count); +} + void IST8310::RegisterWrite(Register reg, uint8_t value) { uint8_t buffer[2] { (uint8_t)reg, value }; diff --git a/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp b/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp index a251f49cf0..04482d37b3 100644 --- a/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp +++ b/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp @@ -83,6 +83,23 @@ private: void RegisterWrite(Register reg, uint8_t value); void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits); + int read(Register reg, void *data, unsigned count); + + /** + * Pull cross axis compensation from device and calculate compensation matrix + */ + void cross_axis_comp(); + + /** + * Update cross axis matrix to identity + */ + void update_crossaxis(); + + /** + * Print cross axis matrix + */ + void print_cross_axis_info(); + PX4Magnetometer _px4_mag; perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; @@ -110,4 +127,7 @@ private: { 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 }, }; + + float _crossaxis_inv[9]; + int32_t _crossaxis_det; }; diff --git a/src/drivers/magnetometer/isentek/ist8310/iSentek_IST8310_registers.hpp b/src/drivers/magnetometer/isentek/ist8310/iSentek_IST8310_registers.hpp index 2a49ee03e9..eae0da7ddd 100644 --- a/src/drivers/magnetometer/isentek/ist8310/iSentek_IST8310_registers.hpp +++ b/src/drivers/magnetometer/isentek/ist8310/iSentek_IST8310_registers.hpp @@ -82,6 +82,8 @@ enum class Register : uint8_t { AVGCNTL = 0x41, // Average Control Register PDCNTL = 0x42, // Pulse Duration Control Register + OTP = 0x99, + XX_CROSS_L = 0x9C, // cross axis xx low byte XX_CROSS_H = 0x9D, // cross axis xx high byte XY_CROSS_L = 0x9E, // cross axis xy low byte