MrYoshii/WAVE_ROVER
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix IMU magnetometer diagnostics

Browse Source
This commit is contained in:
Josh
2026-04-24 00:31:47 +02:00
parent 53ba59e4cd
commit 92a2ccc689
10 changed files with 828 additions and 221 deletions
Download Patch File Download Diff File Expand all files Collapse all files
QMI8658.cpp
+40 -9
View File
@@ -3,9 +3,11 @@
#include <Wire.h>
#include <math.h>
#define QMI8658_UINT_MG_DPS
//#define M_PI (3.14159265358979323846f)
#define ONE_G (9.807f)
#define QMI8658_UINT_MG_DPS
//#define M_PI (3.14159265358979323846f)
#define ONE_G (9.807f)
#define QMI8658_MAG_DEV_AKM09918 0x00
#define QMI8658_MAG_ODR_125HZ 0x03
static qmi8658_state g_imu;
@@ -163,6 +165,30 @@ void QMI8658::read_gyro(float gyro[3])
#endif
}
bool QMI8658::read_mag(int16_t *mag_x, int16_t *mag_y, int16_t *mag_z)
{
if (mag_x == nullptr || mag_y == nullptr || mag_z == nullptr) {
return false;
}
*mag_x = (int16_t)((unsigned short)readWord_reg(Qmi8658Register_Mx_L));
*mag_y = (int16_t)((unsigned short)readWord_reg(Qmi8658Register_My_L));
*mag_z = (int16_t)((unsigned short)readWord_reg(Qmi8658Register_Mz_L));
return *mag_x != 0 || *mag_y != 0 || *mag_z != 0;
}
void QMI8658::enable_magnetometer()
{
write_reg(Qmi8658Register_Ctrl4, QMI8658_MAG_DEV_AKM09918 | QMI8658_MAG_ODR_125HZ);
enableSensors(g_imu.cfg.enSensors | QMI8658_MAG_ENABLE);
}
uint8_t QMI8658::read_debug_reg(uint8_t reg)
{
return read_reg(reg);
}
float QMI8658::read_temperature()
{
const int16_t raw_temp = (int16_t)((unsigned short)(readWord_reg(Qmi8658Register_Tempearture_L)));
@@ -387,7 +413,7 @@ void QMI8658::enableSensors(unsigned char enableFlags)
#else
write_reg(Qmi8658Register_Ctrl7, enableFlags);
#endif
g_imu.cfg.enSensors = enableFlags&0x03;
g_imu.cfg.enSensors = enableFlags & 0x07;
delay(1);
}
@@ -416,11 +442,16 @@ void QMI8658::config_reg(unsigned char low_power)
{
config_acc(g_imu.cfg.accRange, g_imu.cfg.accOdr, Qmi8658Lpf_Disable, Qmi8658St_Disable);
}
if(g_imu.cfg.enSensors & QMI8658_GYR_ENABLE)
{
config_gyro(g_imu.cfg.gyrRange, g_imu.cfg.gyrOdr, Qmi8658Lpf_Disable, Qmi8658St_Disable);
}
}
if(g_imu.cfg.enSensors & QMI8658_GYR_ENABLE)
{
config_gyro(g_imu.cfg.gyrRange, g_imu.cfg.gyrOdr, Qmi8658Lpf_Disable, Qmi8658St_Disable);
}
// Waveshare's General Driver board pairs QMI8658 with AK09918C. Their
// reference firmware programs Ctrl4 even when accel/gyro are the only
// enabled QMI inputs, so keep the board-level magnetometer routing sane.
write_reg(Qmi8658Register_Ctrl4, QMI8658_MAG_DEV_AKM09918 | QMI8658_MAG_ODR_125HZ);
}
unsigned char QMI8658::get_id(void)
{