pohui_pohui/firmware
1
0
Fork 0
mirror of https://github.com/meshtastic/firmware.git synced 2025-10-27 15:02:41 +00:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

Replaced old GPS data with GPS+IMU fusion data with fallback support when imu is unavailable

Browse Source
This commit is contained in:
MATBckh22 2025-09-23 22:06:23 +08:00
parent 30943653c5
commit 5a32afb485
2 changed files with 71 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
.vscode/tasks.json vendored
View File

@ -1,15 +1,26 @@
{
"version": "2.0.0",
"tasks": [
{
"type": "PlatformIO",
"task": "Build",
"problemMatcher": ["$platformio"],
"group": {
"kind": "build",
"isDefault": true
},
"label": "PlatformIO: Build"
}
]
}
"version": "2.0.0",
"tasks": [
{
"type": "PlatformIO",
"task": "Build",
"problemMatcher": [
"$platformio"
],
"group": {
"kind": "build",
"isDefault": true
},
"label": "PlatformIO: Build"
},
{
"label": "PlatformIO: Build",
"type": "shell",
"command": "python -m platformio run -e tbeam-s3-core",
"problemMatcher": [
"$platformio"
],
"group": "build"
}
]
}

46
src/gps/GPS.cpp
View File

@ -21,6 +21,13 @@
#include "cas.h"
#include "ubx.h"
// Optional GPS+IMU fusion support: enable with -DGPS_IMU_FUSION_ENABLE in build flags
#if defined(GPS_IMU_FUSION_ENABLE)
#include "motion/GPSIMUFusion.h"
// Optional runtime gate: if future config flag is added, we can check it here.
// For now, fusion overrides are enabled whenever the build flag is set and fusion reports valid data.
#endif
#ifdef ARCH_PORTDUINO
#include "PortduinoGlue.h"
#include "meshUtils.h"
@ -1711,6 +1718,45 @@ bool GPS::lookForLocation()
p.ground_speed = reader.speed.kmph();
}
#if defined(GPS_IMU_FUSION_ENABLE)
// If fusion is active, override final lat/lon/heading/speed before returning
if (g_gps_imu_fusion.isValid()) {
const auto &fusion = g_gps_imu_fusion.getFusionData();
auto clampLat = [](int32_t v) {
if (v > 900000000) return 900000000; // +90 deg
if (v < -900000000) return -900000000; // -90 deg
return v;
};
auto clampLon = [](int32_t v) {
if (v > 1800000000) return 1800000000; // +180 deg
if (v < -1800000000) return -1800000000; // -180 deg
return v;
};
// Replace coordinates if fusion has sane values (degrees -> int32 degrees*1e7)
int32_t fused_lat_i = (int32_t)(fusion.latitude * 1e7);
int32_t fused_lon_i = (int32_t)(fusion.longitude * 1e7);
if (fused_lat_i != 0 || fused_lon_i != 0) {
p.latitude_i = clampLat(fused_lat_i);
p.longitude_i = clampLon(fused_lon_i);
}
// Replace heading (yaw in degrees -> degrees*1e5)
if (fusion.imu_valid || fusion.gps_valid) {
float yaw = fusion.yaw;
while (yaw < 0.0f) yaw += 360.0f;
while (yaw >= 360.0f) yaw -= 360.0f;
p.ground_track = (uint32_t)(yaw * 1e5f);
}
// Replace speed (fusion.speed is m/s -> km/h)
if (fusion.speed >= 0.0f) {
p.ground_speed = fusion.speed * 3.6f;
}
}
#endif // GPS_IMU_FUSION_ENABLE
return true;
}