diff --git a/components/icm20948/src/icm20948.cpp b/components/icm20948/src/icm20948.cpp index 52f151fac..c86a2f314 100644 --- a/components/icm20948/src/icm20948.cpp +++ b/components/icm20948/src/icm20948.cpp @@ -698,9 +698,9 @@ Icm20948::Value Icm20948::read_magnetometer(std::error_code &ec) { } float sensitivity = get_magnetometer_sensitivity(); Value value = { - static_cast(raw.x) / sensitivity, - static_cast(raw.y) / sensitivity, - static_cast(raw.z) / sensitivity, + static_cast(raw.x) * sensitivity, + static_cast(raw.y) * sensitivity, + static_cast(raw.z) * sensitivity, }; // update values mag_values_ = value; @@ -1021,7 +1021,17 @@ Icm20948::RawValue Icm20948::get_gyroscope_raw(std::error_code &ec) { template Icm20948::RawValue Icm20948::get_magnetometer_raw(std::error_code &ec) { - return get_raw(RegisterBank0::MAG_DATA, ec); + // the magnetometer data is read through the I2C master, but it has a + // different endianess than the accelerometer and gyroscope data. + auto raw_mag = get_raw(RegisterBank0::MAG_DATA, ec); + if (ec) { + return {0, 0, 0}; + } + return { + static_cast(((raw_mag.x & 0xFF00) >> 8) | ((raw_mag.x & 0xFF) << 8)), + static_cast(((raw_mag.y & 0xFF00) >> 8) | ((raw_mag.y & 0xFF) << 8)), + static_cast(((raw_mag.z & 0xFF00) >> 8) | ((raw_mag.z & 0xFF) << 8)), + }; } template