mirror of
https://github.com/meshtastic/firmware.git
synced 2025-10-27 23:12:39 +00:00
Roll Pitch Yaw
This commit is contained in:
MATBckh22
parent
934c14bfc4
commit
b602bca6f2
@ -55,7 +55,7 @@ Dependency pulled via PlatformIO:
|
||||
|
||||
If `Δ` exceeds a small threshold (`0.15 g`), we wake the screen.
|
||||
|
||||
### Debug Stream
|
||||
### Debug Stream & Fused Orientation (RPY)
|
||||
|
||||
- The debug line (1 Hz) prints:
|
||||
|
||||
@ -63,6 +63,8 @@ Dependency pulled via PlatformIO:
|
||||
|
||||
This is also mirrored into the live data struct `g_qmi8658Live` that the UI reads.
|
||||
|
||||
- An AHRS (Fusion library by Seb Madgwick) runs in the IMU thread using gyroscope + accelerometer and, when fresh, magnetometer from QMC6310. It outputs roll/pitch/yaw (ZYX, degrees) into `g_qmi8658Live.roll/pitch/yaw`. The QMI8658 UI screen displays “RPY r p y”.
|
||||
|
||||
---
|
||||
|
||||
## QMC6310 (I2C) – Implementation and Math
|
||||
@ -96,6 +98,26 @@ mz = rawZ − offsetZ
|
||||
|
||||
This removes hard‑iron bias (DC offset) and is adequate for real‑time heading stabilization. For best results, slowly rotate the device on all axes for several seconds to let min/max settle.
|
||||
|
||||
### Soft‑Iron Compensation (axis scaling)
|
||||
|
||||
Ferric materials and PCB + enclosure can distort the local field, making the calibration cloud elliptical. We approximate this by computing per‑axis radii and scaling them toward the average radius:
|
||||
|
||||
```
|
||||
R_x = (maxX − minX)/2
|
||||
R_y = (maxY − minY)/2
|
||||
R_z = (maxZ − minZ)/2
|
||||
R_avg = mean of available radii (ignore zeros)
|
||||
s_x = R_avg / R_x (if R_x > 0 else 1)
|
||||
s_y = R_avg / R_y
|
||||
s_z = R_avg / R_z
|
||||
|
||||
mx' = (rawX − offsetX) * s_x
|
||||
my' = (rawY − offsetY) * s_y
|
||||
mz' = (rawZ − offsetZ) * s_z
|
||||
```
|
||||
|
||||
This improves the circularity of the cloud and reduces heading bias caused by anisotropy. For fully accurate compensation, an ellipsoid fit can be added later.
|
||||
|
||||
Soft‑iron distortion (elliptical scaling) is NOT corrected here. A future enhancement can compute per‑axis scale from `(max−min)/2` or use an ellipsoid fit.
|
||||
|
||||
### Heading Computation
|
||||
@ -103,7 +125,7 @@ Soft‑iron distortion (elliptical scaling) is NOT corrected here. A future enha
|
||||
Raw 2‑D horizontal heading (no tilt compensation):
|
||||
|
||||
```
|
||||
heading_deg = atan2(my, mx) * 180/π
|
||||
heading_deg = atan2(my, mx) * 180/π (Arduino‑style)
|
||||
heading_true = wrap_0_360( heading_deg + declination_deg + yaw_mount_offset )
|
||||
```
|
||||
|
||||
@ -115,6 +137,14 @@ Where:
|
||||
|
||||
Screen orientation (0/90/180/270) is applied after heading is computed and normalized.
|
||||
|
||||
Heading style and axis mapping are configurable via build flags:
|
||||
|
||||
- `QMC6310_SWAP_XY` (0/1) – swap X and Y axes before heading.
|
||||
- `QMC6310_X_SIGN`, `QMC6310_Y_SIGN` (+1/−1) – flip axes if needed.
|
||||
- `QMC6310_HEADING_STYLE` (0/1)
|
||||
- 0 → `atan2(my, mx)` (Arduino sketch style)
|
||||
- 1 → `atan2(x, −y)` (Lewis He QST library `readPolar()` style)
|
||||
|
||||
### Tilt‑Compensated Heading (future option)
|
||||
|
||||
If pitch/roll are available (from IMU), heading can be tilt‑compensated:
|
||||
@ -129,7 +159,7 @@ Where `φ` is roll and `θ` is pitch (radians), derived from accelerometer. This
|
||||
|
||||
### Live Data
|
||||
|
||||
The magnetometer thread writes the latest raw XYZ, offsets and heading into `g_qmc6310Live` for the UI to display without touching hardware.
|
||||
The magnetometer thread writes the latest raw XYZ, offsets, µT (scaled), scale factors, and heading into `g_qmc6310Live` for the UI to display without touching hardware.
|
||||
|
||||
---
|
||||
|
||||
@ -208,4 +238,3 @@ The values on the UI screens should match these, because both screens read from
|
||||
- If QMI8658 shows zeros on the UI screen, ensure `QMI8658_DEBUG_STREAM` is enabled or let the background IMU thread initialize first (it sets `g_qmi8658Live.initialized`).
|
||||
- If QMC6310 heading appears constrained or jumps, rotate the device slowly on all axes for 10–20 seconds to update min/max; verify you’re on the correct I2C port and address (`0x1C`).
|
||||
- If the I2C scan does not find the IMU on power‑on, check that the late‑rescan log appears; some boards power the sensor rail slightly later.
|
||||
|
||||
|
||||
@ -711,7 +711,14 @@ void drawQMC6310Screen(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t
|
||||
snprintf(buf, sizeof(buf), "Head %3.0f offX %.0f offY %.0f", g_qmc6310Live.heading, g_qmc6310Live.offX,
|
||||
g_qmc6310Live.offY);
|
||||
display->drawString(x, getTextPositions(display)[line++], buf);
|
||||
snprintf(buf, sizeof(buf), "rawX %d rawY %d", g_qmc6310Live.rawX, g_qmc6310Live.rawY);
|
||||
snprintf(buf, sizeof(buf), "uT X %.1f Y %.1f", g_qmc6310Live.uT_X, g_qmc6310Live.uT_Y);
|
||||
display->drawString(x, getTextPositions(display)[line++], buf);
|
||||
snprintf(buf, sizeof(buf), "scale X %.2f Y %.2f", g_qmc6310Live.scaleX, g_qmc6310Live.scaleY);
|
||||
display->drawString(x, getTextPositions(display)[line++], buf);
|
||||
// total field vs expected
|
||||
float mag = sqrtf(g_qmc6310Live.uT_X * g_qmc6310Live.uT_X + g_qmc6310Live.uT_Y * g_qmc6310Live.uT_Y +
|
||||
g_qmc6310Live.uT_Z * g_qmc6310Live.uT_Z);
|
||||
snprintf(buf, sizeof(buf), "|B| %.1f uT vs %.1f", mag, (float)QMC6310_EXPECTED_FIELD_uT);
|
||||
display->drawString(x, getTextPositions(display)[line++], buf);
|
||||
}
|
||||
} else {
|
||||
@ -733,6 +740,8 @@ void drawQMI8658Screen(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t
|
||||
display->drawString(x, getTextPositions(display)[line++], buf);
|
||||
snprintf(buf, sizeof(buf), "GYR %.2f %.2f %.2f", g_qmi8658Live.gyr.x, g_qmi8658Live.gyr.y, g_qmi8658Live.gyr.z);
|
||||
display->drawString(x, getTextPositions(display)[line++], buf);
|
||||
snprintf(buf, sizeof(buf), "RPY %.1f %.1f %.1f", g_qmi8658Live.roll, g_qmi8658Live.pitch, g_qmi8658Live.yaw);
|
||||
display->drawString(x, getTextPositions(display)[line++], buf);
|
||||
} else {
|
||||
display->drawString(x, getTextPositions(display)[line++], "No data");
|
||||
}
|
||||
|
||||
@ -74,9 +74,37 @@ int32_t QMC6310Sensor::runOnce()
|
||||
|
||||
float mx = float(rx) - offsetX;
|
||||
float my = float(ry) - offsetY;
|
||||
(void)rz; // mz is currently unused for heading
|
||||
float mz = float(rz) - offsetZ;
|
||||
|
||||
float heading = atan2f(my, mx) * 180.0f / PI;
|
||||
// Soft-iron scaling based on radii along axes
|
||||
float radiusX = (maxX - minX) * 0.5f;
|
||||
float radiusY = (maxY - minY) * 0.5f;
|
||||
float radiusZ = (maxZ - minZ) * 0.5f;
|
||||
float avgR = 0.0f; int rcount = 0;
|
||||
if (radiusX > 1) { avgR += radiusX; rcount++; }
|
||||
if (radiusY > 1) { avgR += radiusY; rcount++; }
|
||||
if (radiusZ > 1) { avgR += radiusZ; rcount++; }
|
||||
if (rcount > 0) avgR /= rcount; else avgR = 1.0f;
|
||||
scaleX = (radiusX > 1) ? (avgR / radiusX) : 1.0f;
|
||||
scaleY = (radiusY > 1) ? (avgR / radiusY) : 1.0f;
|
||||
scaleZ = (radiusZ > 1) ? (avgR / radiusZ) : 1.0f;
|
||||
|
||||
mx *= scaleX; my *= scaleY; mz *= scaleZ;
|
||||
|
||||
// Axis mapping / sign
|
||||
float hx = mx, hy = my;
|
||||
#if QMC6310_SWAP_XY
|
||||
hx = my; hy = mx;
|
||||
#endif
|
||||
hx *= (float)QMC6310_X_SIGN;
|
||||
hy *= (float)QMC6310_Y_SIGN;
|
||||
|
||||
float heading;
|
||||
#if QMC6310_HEADING_STYLE == 1
|
||||
heading = atan2f(hx, -hy) * 180.0f / PI; // QST library style
|
||||
#else
|
||||
heading = atan2f(hy, hx) * 180.0f / PI; // Arduino sketch style
|
||||
#endif
|
||||
heading += QMC6310_DECLINATION_DEG + QMC6310_YAW_MOUNT_OFFSET;
|
||||
while (heading < 0.0f) heading += 360.0f;
|
||||
while (heading >= 360.0f) heading -= 360.0f;
|
||||
@ -88,6 +116,14 @@ int32_t QMC6310Sensor::runOnce()
|
||||
g_qmc6310Live.offX = offsetX;
|
||||
g_qmc6310Live.offY = offsetY;
|
||||
g_qmc6310Live.offZ = offsetZ;
|
||||
// Update live data (also publish scaled µT)
|
||||
const float GAUSS_PER_LSB = QMC6310_SENS_GAUSS_PER_LSB;
|
||||
g_qmc6310Live.uT_X = mx * GAUSS_PER_LSB * 100.0f;
|
||||
g_qmc6310Live.uT_Y = my * GAUSS_PER_LSB * 100.0f;
|
||||
g_qmc6310Live.uT_Z = mz * GAUSS_PER_LSB * 100.0f;
|
||||
g_qmc6310Live.scaleX = scaleX;
|
||||
g_qmc6310Live.scaleY = scaleY;
|
||||
g_qmc6310Live.scaleZ = scaleZ;
|
||||
g_qmc6310Live.heading = heading;
|
||||
g_qmc6310Live.last_ms = millis();
|
||||
|
||||
|
||||
@ -17,6 +17,29 @@
|
||||
#define QMC6310_YAW_MOUNT_OFFSET 0.0f
|
||||
#endif
|
||||
|
||||
// Axis mapping and heading style controls
|
||||
#ifndef QMC6310_SWAP_XY
|
||||
#define QMC6310_SWAP_XY 0 // 0: normal, 1: swap X and Y
|
||||
#endif
|
||||
#ifndef QMC6310_X_SIGN
|
||||
#define QMC6310_X_SIGN 1 // +1 or -1 to flip X
|
||||
#endif
|
||||
#ifndef QMC6310_Y_SIGN
|
||||
#define QMC6310_Y_SIGN 1 // +1 or -1 to flip Y
|
||||
#endif
|
||||
#ifndef QMC6310_HEADING_STYLE
|
||||
#define QMC6310_HEADING_STYLE 0 // 0: atan2(my, mx); 1: atan2(x, -y) (QST library style)
|
||||
#endif
|
||||
|
||||
// Sensitivity (Gauss/LSB) based on range; we set RANGE_2G in init()
|
||||
#ifndef QMC6310_SENS_GAUSS_PER_LSB
|
||||
#define QMC6310_SENS_GAUSS_PER_LSB 0.0066f
|
||||
#endif
|
||||
|
||||
#ifndef QMC6310_EXPECTED_FIELD_uT
|
||||
#define QMC6310_EXPECTED_FIELD_uT 42.0f
|
||||
#endif
|
||||
|
||||
class QMC6310Sensor : public MotionSensor
|
||||
{
|
||||
private:
|
||||
@ -27,6 +50,8 @@ class QMC6310Sensor : public MotionSensor
|
||||
float minY = 1e9f, maxY = -1e9f;
|
||||
float minZ = 1e9f, maxZ = -1e9f;
|
||||
float offsetX = 0.0f, offsetY = 0.0f, offsetZ = 0.0f;
|
||||
// Soft-iron scale (computed from half-ranges)
|
||||
float scaleX = 1.0f, scaleY = 1.0f, scaleZ = 1.0f;
|
||||
|
||||
public:
|
||||
explicit QMC6310Sensor(ScanI2C::FoundDevice foundDevice);
|
||||
|
||||
@ -1,6 +1,7 @@
|
||||
#include "QMI8658Sensor.h"
|
||||
#include "NodeDB.h"
|
||||
#include "SensorLiveData.h"
|
||||
#include "Fusion/Fusion.h"
|
||||
|
||||
#if !defined(ARCH_STM32WL) && !MESHTASTIC_EXCLUDE_I2C && __has_include(<SensorQMI8658.hpp>)
|
||||
|
||||
@ -108,6 +109,49 @@ int32_t QMI8658Sensor::runOnce()
|
||||
LOG_DEBUG("QMI8658: ready=%d ACC[x=%.3f y=%.3f z=%.3f] m/s^2 GYR[x=%.3f y=%.3f z=%.3f] dps",
|
||||
(int)ready, acc.x, acc.y, acc.z, gyr.x, gyr.y, gyr.z);
|
||||
}
|
||||
// AHRS fusion (roll/pitch/yaw) using accelerometer, gyroscope and (optionally) magnetometer
|
||||
{
|
||||
static bool ahrsInit = false;
|
||||
static FusionAhrs ahrs;
|
||||
static uint32_t lastTime = 0;
|
||||
const uint32_t now_ms = millis();
|
||||
const float dt = (lastTime == 0) ? 0.01f : (now_ms - lastTime) / 1000.0f;
|
||||
lastTime = now_ms;
|
||||
|
||||
if (!ahrsInit) {
|
||||
FusionAhrsInitialise(&ahrs);
|
||||
FusionAhrsSettings settings;
|
||||
settings.convention = FusionConventionNed; // NED frame
|
||||
settings.gain = 0.5f; // fusion gain
|
||||
settings.gyroscopeRange = 512.0f; // dps range (matches config)
|
||||
settings.accelerationRejection = 10.0f; // deg
|
||||
settings.magneticRejection = 10.0f; // deg
|
||||
settings.recoveryTriggerPeriod = 5; // cycles
|
||||
FusionAhrsSetSettings(&ahrs, &settings);
|
||||
ahrsInit = true;
|
||||
}
|
||||
|
||||
// Map live sensor values into Fusion vectors
|
||||
FusionVector g = {.axis = {.x = g_qmi8658Live.gyr.x, .y = g_qmi8658Live.gyr.y, .z = g_qmi8658Live.gyr.z}}; // dps
|
||||
FusionVector a = {.axis = {.x = g_qmi8658Live.acc.x, .y = g_qmi8658Live.acc.y, .z = g_qmi8658Live.acc.z}}; // m/s2
|
||||
|
||||
// Use magnetometer if recent (<= 200 ms old). Use µT values; the library normalises internally.
|
||||
const bool magFresh = (now_ms - g_qmc6310Live.last_ms) <= 200 && g_qmc6310Live.initialized;
|
||||
FusionVector m = {.axis = {.x = g_qmc6310Live.uT_X, .y = g_qmc6310Live.uT_Y, .z = g_qmc6310Live.uT_Z}};
|
||||
|
||||
if (magFresh) {
|
||||
FusionAhrsUpdate(&ahrs, g, a, m, dt);
|
||||
} else {
|
||||
FusionAhrsUpdateNoMagnetometer(&ahrs, g, a, dt);
|
||||
}
|
||||
|
||||
const FusionQuaternion q = FusionAhrsGetQuaternion(&ahrs);
|
||||
const FusionEuler e = FusionQuaternionToEuler(q);
|
||||
g_qmi8658Live.roll = e.angle.roll;
|
||||
g_qmi8658Live.pitch = e.angle.pitch;
|
||||
g_qmi8658Live.yaw = e.angle.yaw; // degrees
|
||||
}
|
||||
|
||||
return MOTION_SENSOR_CHECK_INTERVAL_MS;
|
||||
#endif
|
||||
|
||||
@ -129,6 +173,46 @@ int32_t QMI8658Sensor::runOnce()
|
||||
}
|
||||
}
|
||||
|
||||
// When not streaming, we still update AHRS at the same cadence
|
||||
{
|
||||
static bool ahrsInit = false;
|
||||
static FusionAhrs ahrs;
|
||||
static uint32_t lastTime = 0;
|
||||
const uint32_t now_ms = millis();
|
||||
const float dt = (lastTime == 0) ? 0.01f : (now_ms - lastTime) / 1000.0f;
|
||||
lastTime = now_ms;
|
||||
|
||||
if (!ahrsInit) {
|
||||
FusionAhrsInitialise(&ahrs);
|
||||
FusionAhrsSettings settings;
|
||||
settings.convention = FusionConventionNed;
|
||||
settings.gain = 0.5f;
|
||||
settings.gyroscopeRange = 512.0f;
|
||||
settings.accelerationRejection = 10.0f;
|
||||
settings.magneticRejection = 10.0f;
|
||||
settings.recoveryTriggerPeriod = 5;
|
||||
FusionAhrsSetSettings(&ahrs, &settings);
|
||||
ahrsInit = true;
|
||||
}
|
||||
|
||||
FusionVector g = {.axis = {.x = g_qmi8658Live.gyr.x, .y = g_qmi8658Live.gyr.y, .z = g_qmi8658Live.gyr.z}};
|
||||
FusionVector a = {.axis = {.x = g_qmi8658Live.acc.x, .y = g_qmi8658Live.acc.y, .z = g_qmi8658Live.acc.z}};
|
||||
const bool magFresh = (now_ms - g_qmc6310Live.last_ms) <= 200 && g_qmc6310Live.initialized;
|
||||
FusionVector m = {.axis = {.x = g_qmc6310Live.uT_X, .y = g_qmc6310Live.uT_Y, .z = g_qmc6310Live.uT_Z}};
|
||||
|
||||
if (magFresh) {
|
||||
FusionAhrsUpdate(&ahrs, g, a, m, dt);
|
||||
} else {
|
||||
FusionAhrsUpdateNoMagnetometer(&ahrs, g, a, dt);
|
||||
}
|
||||
|
||||
const FusionQuaternion q = FusionAhrsGetQuaternion(&ahrs);
|
||||
const FusionEuler e = FusionQuaternionToEuler(q);
|
||||
g_qmi8658Live.roll = e.angle.roll;
|
||||
g_qmi8658Live.pitch = e.angle.pitch;
|
||||
g_qmi8658Live.yaw = e.angle.yaw;
|
||||
}
|
||||
|
||||
return MOTION_SENSOR_CHECK_INTERVAL_MS;
|
||||
}
|
||||
|
||||
|
||||
@ -13,6 +13,10 @@ struct QMI8658LiveData {
|
||||
bool ready = false;
|
||||
Vec3f acc; // m/s^2
|
||||
Vec3f gyr; // dps
|
||||
// Fused orientation (degrees), using Fusion AHRS with QMC6310 magnetometer when available
|
||||
float roll = 0.0f;
|
||||
float pitch = 0.0f;
|
||||
float yaw = 0.0f;
|
||||
uint32_t last_ms = 0;
|
||||
};
|
||||
|
||||
@ -21,9 +25,12 @@ struct QMC6310LiveData {
|
||||
int16_t rawX = 0, rawY = 0, rawZ = 0;
|
||||
float offX = 0, offY = 0, offZ = 0;
|
||||
float heading = 0; // degrees 0..360
|
||||
// Scaled field strength in microtesla (after hard/soft iron corrections)
|
||||
float uT_X = 0, uT_Y = 0, uT_Z = 0;
|
||||
// Soft-iron scale factors applied to calibrated axes
|
||||
float scaleX = 1.0f, scaleY = 1.0f, scaleZ = 1.0f;
|
||||
uint32_t last_ms = 0;
|
||||
};
|
||||
|
||||
extern QMI8658LiveData g_qmi8658Live;
|
||||
extern QMC6310LiveData g_qmc6310Live;
|
||||
|
||||
|
||||
@ -14,3 +14,5 @@ build_flags =
|
||||
-I variants/esp32s3/tbeam-s3-core
|
||||
-D PCF8563_RTC=0x51 ;Putting definitions in variant.h does not compile correctly
|
||||
-D QMI8658_DEBUG_STREAM
|
||||
-D QMC6310_DECLINATION_DEG=-0.0167
|
||||
-D QMC6310_EXPECTED_FIELD_uT=41.9154
|
||||
|
||||
Loading…
Reference in New Issue
Block a user