pohui_pohui/firmware
1
0
Fork 0
mirror of https://github.com/meshtastic/firmware.git synced 2025-10-29 07:36:46 +00:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

Moved GPSIMUFusion related files to Fusion folder

Browse Source
This commit is contained in:
MATBckh22 2025-09-24 09:29:32 +08:00
parent 5a32afb485
commit c0684033d1
10 changed files with 56 additions and 199 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

210
src/motion/GPSIMUFusion.cpp → src/Fusion/GPSIMUFusion.cpp
View File

@ -4,7 +4,7 @@
#if !MESHTASTIC_EXCLUDE_GPS
#include "gps/GPS.h"
#include "SensorLiveData.h"
#include "motion/SensorLiveData.h"
#include "Fusion/Fusion.h"
#include <math.h>
#include <Arduino.h>
@ -168,46 +168,37 @@ void GPSIMUFusion::updateGPS() {
}
// Smart GPS filtering for improved accuracy
// Adjust filter strength based on GPS quality
float base_alpha = 0.4f; // Moderate filtering (was 0.8f - too light)
float base_alpha = 0.4f;
float gps_alpha = base_alpha;
// Get GPS quality indicators
float hdop_m = gps->p.HDOP / 100.0f; // Convert cm to meters
float hdop_m = gps->p.HDOP / 100.0f; // cm -> m
uint8_t sats = gps->p.sats_in_view;
// Adjust filter based on GPS quality (tighter filtering for poor quality)
if (hdop_m > 5.0f || sats < 4) {
gps_alpha = 0.2f; // Heavier filtering for poor GPS
gps_alpha = 0.2f; // heavier filtering
} else if (hdop_m < 2.0f && sats >= 6) {
gps_alpha = 0.6f; // Lighter filtering for good GPS
gps_alpha = 0.6f; // lighter filtering
}
if (gps_state.lat_filtered == 0.0 && gps_state.lon_filtered == 0.0) {
// First GPS fix - initialize directly without filtering
gps_state.lat_filtered = gps_lat;
gps_state.lon_filtered = gps_lon;
gps_state.alt_filtered = gps_alt;
LOG_INFO("GPS INIT: First fix set to lat=%.8f lon=%.8f alt=%.1f (hdop=%.1fm sats=%d)",
gps_lat, gps_lon, gps_alt, hdop_m, sats);
LOG_INFO("GPS INIT: lat=%.8f lon=%.8f alt=%.1f (hdop=%.1fm sats=%d)", gps_lat, gps_lon, gps_alt, hdop_m, sats);
} else {
// Calculate distance from current filtered position to new GPS reading
double lat_diff = gps_lat - gps_state.lat_filtered;
double lon_diff = gps_lon - gps_state.lon_filtered;
double distance_deg = sqrt(lat_diff*lat_diff + lon_diff*lon_diff);
double distance_m = distance_deg * 111320.0; // Rough conversion to meters
double distance_m = distance_deg * 111320.0; // approx meters
// Adaptive jump detection based on GPS quality
float max_jump = (hdop_m > 10.0f) ? 200.0f : 50.0f; // Allow larger jumps for poor GPS
// If GPS reading is very different, reset to new position (might be location jump)
float max_jump = (hdop_m > 10.0f) ? 200.0f : 50.0f;
if (distance_m > max_jump) {
LOG_INFO("GPS RESET: Large jump detected (%.1fm > %.1fm), resetting filter", distance_m, max_jump);
LOG_INFO("GPS RESET: jump %.1fm > %.1fm, resetting", distance_m, max_jump);
gps_state.lat_filtered = gps_lat;
gps_state.lon_filtered = gps_lon;
gps_state.alt_filtered = gps_alt;
} else {
// Apply adaptive low-pass filter - convert double to float for filtering
float lat_f = (float)gps_lat;
float lon_f = (float)gps_lon;
float lat_filtered_f = (float)gps_state.lat_filtered;
@ -217,223 +208,96 @@ void GPSIMUFusion::updateGPS() {
lowPassFilter(lon_filtered_f, lon_f, gps_alpha);
lowPassFilter(gps_state.alt_filtered, gps_alt, gps_alpha);
// Convert back to double
gps_state.lat_filtered = (double)lat_filtered_f;
gps_state.lon_filtered = (double)lon_filtered_f;
// Debug position filtering
static uint32_t lastPosDebug = 0;
if (now_ms - lastPosDebug > 5000) {
lastPosDebug = now_ms;
LOG_INFO("GPS FILTER: raw(%.8f,%.8f) -> filtered(%.8f,%.8f) diff=%.1fm alpha=%.2f",
gps_lat, gps_lon, gps_state.lat_filtered, gps_state.lon_filtered, distance_m, gps_alpha);
}
}
}
// Get GPS velocity and course from the public position structure
// Velocity/course
float gps_speed = 0.0f;
float gps_course = 0.0f;
// Extract speed and course from GPS position structure
// ground_speed is in km/h, ground_track is in degrees * 10^-5
// Note: these are uint32_t, so we check for has_* flags or reasonable values
if (gps->p.has_ground_speed) { // Use protobuf flag for validity
gps_speed = gps->p.ground_speed / 3.6f; // Convert km/h to m/s
if (gps->p.has_ground_speed) {
gps_speed = gps->p.ground_speed / 3.6f; // km/h -> m/s
}
if (gps->p.has_ground_track) { // Use protobuf flag for validity
gps_course = gps->p.ground_track / 1e5f; // Convert from degrees * 10^-5 to degrees
if (gps->p.has_ground_track) {
gps_course = gps->p.ground_track / 1e5f;
// Check if vehicle is moving
gps_state.moving = (gps_speed > GPS_VELOCITY_THRESHOLD);
if (gps_state.moving) {
// Apply smoothing to course only when moving
if (gps_state.last_course_ms == 0) {
gps_state.course_filtered = gps_course;
} else {
// Handle angle wrapping for course filtering
float course_diff = gps_course - gps_state.course_filtered;
if (course_diff > 180.0f) course_diff -= 360.0f;
if (course_diff < -180.0f) course_diff += 360.0f;
gps_state.course_filtered += course_diff * 0.2f; // Light smoothing
float diff = gps_course - gps_state.course_filtered;
if (diff > 180.0f) diff -= 360.0f;
if (diff < -180.0f) diff += 360.0f;
gps_state.course_filtered += diff * 0.2f;
gps_state.course_filtered = normalizeAngle(gps_state.course_filtered);
}
gps_state.last_course_ms = now_ms;
}
// Apply smoothing to speed
lowPassFilter(gps_state.speed_filtered, gps_speed, 0.4f);
}
// Store GPS quality information
fusionData.hdop = gps->p.HDOP / 100.0f; // Convert from cm to meters
fusionData.hdop = gps->p.HDOP / 100.0f;
fusionData.satellites = gps->p.sats_in_view;
// Estimate heading accuracy based on speed and HDOP
if (gps_state.moving && gps_speed > 2.0f) {
fusionData.heading_accuracy = constrain(5.0f / gps_speed + fusionData.hdop, 2.0f, 45.0f);
} else {
fusionData.heading_accuracy = 180.0f; // No reliable heading when stationary
}
// Debug GPS data processing with more detail
static uint32_t lastGpsDataDebug = 0;
if (now_ms - lastGpsDataDebug > 3000) {
lastGpsDataDebug = now_ms;
LOG_INFO("GPS CONVERSION: lat_i=%ld -> lat=%.8f", gps->p.latitude_i, gps_lat);
LOG_INFO("GPS CONVERSION: lon_i=%ld -> lon=%.8f", gps->p.longitude_i, gps_lon);
LOG_INFO("GPS SPEED: raw_kmh=%.2f -> speed_ms=%.3f (filtered=%.3f)",
gps->p.ground_speed, gps_speed, gps_state.speed_filtered);
LOG_INFO("GPS COURSE: raw_1e5=%ld -> course_deg=%.2f (filtered=%.2f)",
gps->p.ground_track, gps_course, gps_state.course_filtered);
LOG_INFO("GPS QUALITY: hdop=%dcm(%.2fm) sats=%d moving=%s",
gps->p.HDOP, fusionData.hdop, fusionData.satellites, gps_state.moving ? "YES" : "NO");
fusionData.heading_accuracy = 180.0f;
}
}
void GPSIMUFusion::fuseNavigationData(float dt) {
// Position fusion: GPS is the primary reference, IMU provides smoothing
if (fusionData.gps_valid) {
fusionData.latitude = gps_state.lat_filtered;
fusionData.longitude = gps_state.lon_filtered;
fusionData.altitude = gps_state.alt_filtered;
// Reset IMU velocity integration periodically to prevent drift
if (fusionData.imu_valid) {
// Simple complementary filter for velocity
float gps_weight = 0.1f; // GPS has lower weight due to lower update rate but higher accuracy
// Convert GPS course and speed to velocity components (if moving)
float gps_weight = 0.1f;
if (gps_state.moving) {
float course_rad = gps_state.course_filtered * M_PI / 180.0f;
float gps_vel_north = gps_state.speed_filtered * cos(course_rad);
float gps_vel_east = gps_state.speed_filtered * sin(course_rad);
// Blend GPS and IMU velocities
fusionData.velocity_north = (1.0f - gps_weight) * imu_state.velocity.axis.x + gps_weight * gps_vel_north;
fusionData.velocity_east = (1.0f - gps_weight) * imu_state.velocity.axis.y + gps_weight * gps_vel_east;
// Correct IMU velocity integration
imu_state.velocity.axis.x = fusionData.velocity_north;
imu_state.velocity.axis.y = fusionData.velocity_east;
}
}
fusionData.speed = gps_state.speed_filtered;
}
// Heading fusion: Use GPS course when moving, IMU yaw otherwise
if (fusionData.gps_valid && gps_state.moving && fusionData.heading_accuracy < 20.0f) {
// Vehicle is moving and GPS heading is reliable
float heading_weight = constrain(1.0f / (fusionData.heading_accuracy / 10.0f), 0.1f, 0.8f);
// Blend GPS course and IMU yaw
float yaw_diff = gps_state.course_filtered - fusionData.yaw;
if (yaw_diff > 180.0f) yaw_diff -= 360.0f;
if (yaw_diff < -180.0f) yaw_diff += 360.0f;
fusionData.yaw += yaw_diff * heading_weight;
fusionData.yaw = normalizeAngle(fusionData.yaw);
}
// If only IMU is valid, use pure IMU data (this is handled in updateIMU)
// Debug logging - periodic detailed output
static uint32_t lastDetailedLog = 0;
uint32_t now_ms = millis();
if (now_ms - lastDetailedLog > 5000) { // Every 5 seconds
lastDetailedLog = now_ms;
logFusionDataDetailed();
}
// High-frequency fusion status log
static uint32_t lastQuickLog = 0;
if (now_ms - lastQuickLog > 1000) { // Every 1 second
lastQuickLog = now_ms;
logFusionDataQuick();
}
}
bool GPSIMUFusion::isGPSDataValid() {
if (!gps) return false;
/*
* GPS VALIDATION LOGIC - T-Beam Supreme
*
* CURRENT: Very lenient for testing pipeline
* TODO: Once GPS data flows properly, tighten these for production:
*
* For better accuracy (reduce 50m offset):
* - hasMinSats >= 4 (not 1)
* - hasHDOP < 300cm (3m) for good fixes, < 500cm (5m) acceptable
* - Re-enable hasLock requirement: dataValid = hasLock && hasPositionData && coordinatesReasonable
* - Add recentData check back to timing validation
*
* For T-Beam Supreme indoor/weak signal:
* - Keep hasLock optional if coordinates look valid
* - Use GPS accuracy field (if available) instead of raw HDOP
* - Consider fix type (2D vs 3D) from GPS status
*/
uint32_t now_ms = millis();
bool hasLock = gps->hasLock();
bool recentData = (now_ms - fusionData.last_gps_ms) < GPS_TIMEOUT_MS;
// Check for actual position data (coordinates not zero and reasonable)
bool hasPositionData = (gps->p.latitude_i != 0 || gps->p.longitude_i != 0);
bool coordinatesReasonable = (abs(gps->p.latitude_i) <= 900000000 && abs(gps->p.longitude_i) <= 1800000000);
// TEMPORARY: Very lenient validation for testing the pipeline
// TODO: Tighten these checks once pipeline is confirmed working
bool hasMinSats = (gps->p.sats_in_view >= 1); // PRODUCTION: Change to >= 4
bool hasHDOP = (gps->p.HDOP >= 0); // PRODUCTION: Change to < 300-500cm
// For T-Beam Supreme: Accept coordinates even without hasLock() for testing
// hasLock() may be too strict for indoor/weak signal conditions
bool dataValid = hasPositionData && coordinatesReasonable; // PRODUCTION: Add hasLock back
bool qualityOk = hasMinSats || hasHDOP || (fusionData.last_gps_ms == 0); // Very lenient
// Debug GPS validation issues - more detailed
static uint32_t lastGpsDebug = 0;
if (now_ms - lastGpsDebug > 1000) { // Every second for debugging
lastGpsDebug = now_ms;
LOG_INFO("GPS DEBUG: lock=%s pos=%s coords=%s sats=%s hdop=%s recent=%s",
hasLock ? "OK" : "FAIL",
hasPositionData ? "OK" : "FAIL",
coordinatesReasonable ? "OK" : "FAIL",
hasMinSats ? "OK" : "FAIL",
hasHDOP ? "OK" : "FAIL",
recentData ? "OK" : "FAIL");
LOG_INFO("GPS RAW: lat_i=%ld lon_i=%ld sats=%d hdop=%dcm hasLock=%s",
gps->p.latitude_i, gps->p.longitude_i, gps->p.sats_in_view, gps->p.HDOP,
gps->hasLock() ? "YES" : "NO");
LOG_INFO("GPS CALC: lat=%.8f lon=%.8f age=%ums",
gps->p.latitude_i * 1e-7, gps->p.longitude_i * 1e-7,
fusionData.last_gps_ms > 0 ? (now_ms - fusionData.last_gps_ms) : 0);
// Show detailed validation results with new lenient logic
bool finalResult = dataValid && qualityOk;
LOG_INFO("GPS VALIDATION: dataValid=%s qualityOk=%s firstTime=%s FINAL=%s",
dataValid ? "YES" : "NO",
qualityOk ? "YES" : "NO",
(fusionData.last_gps_ms == 0) ? "YES" : "NO",
finalResult ? "PASS" : "FAIL");
LOG_INFO("GPS LENIENT: Removed hasLock requirement, accepting 1+ sats, any HDOP");
}
// Very lenient validation for T-Beam Supreme testing
bool hasMinSats = (gps->p.sats_in_view >= 1);
bool hasHDOP = (gps->p.HDOP >= 0);
bool dataValid = hasPositionData && coordinatesReasonable; // lenient for testing
bool qualityOk = hasMinSats || hasHDOP || (fusionData.last_gps_ms == 0);
bool finalResult = dataValid && qualityOk;
return finalResult;
}
bool GPSIMUFusion::isIMUDataValid() {
if (!g_qmi8658Live.initialized) return false;
uint32_t now_ms = millis();
bool recentData = (now_ms - g_qmi8658Live.last_ms) < IMU_TIMEOUT_MS;
return recentData;
}
@ -448,7 +312,6 @@ void GPSIMUFusion::lowPassFilter(float& filtered, float new_value, float alpha)
}
void GPSIMUFusion::reset() {
// Reset GPS state
gps_state.lat_filtered = 0.0;
gps_state.lon_filtered = 0.0;
gps_state.alt_filtered = 0.0;
@ -457,12 +320,10 @@ void GPSIMUFusion::reset() {
gps_state.last_course_ms = 0;
gps_state.moving = false;
// Reset IMU state
imu_state.velocity = (FusionVector){.axis = {0, 0, 0}};
imu_state.position = (FusionVector){.axis = {0, 0, 0}};
imu_state.initialized = false;
// Reset fusion data
fusionData.gps_valid = false;
fusionData.imu_valid = false;
fusionData.last_gps_ms = 0;
@ -477,49 +338,32 @@ void GPSIMUFusion::logFusionDataDetailed() {
LOG_INFO("GPS+IMU Fusion: Not initialized");
return;
}
uint32_t now_ms = millis();
LOG_INFO("=== GPS+IMU FUSION DEBUG ===");
LOG_INFO("Status: GPS=%s IMU=%s Initialized=%s",
fusionData.gps_valid ? "VALID" : "INVALID",
fusionData.imu_valid ? "VALID" : "INVALID",
fusionData.initialized ? "YES" : "NO");
if (fusionData.gps_valid || fusionData.imu_valid) {
// Position data with maximum precision display
LOG_INFO("Position: %.8f°, %.8f°, %.1fm",
fusionData.latitude, fusionData.longitude, fusionData.altitude);
// Velocity data
LOG_INFO("Position: %.8f°, %.8f°, %.1fm", fusionData.latitude, fusionData.longitude, fusionData.altitude);
LOG_INFO("Velocity: N=%.2f E=%.2f D=%.2f m/s (Speed=%.2f m/s)",
fusionData.velocity_north, fusionData.velocity_east,
fusionData.velocity_down, fusionData.speed);
// Orientation data (Madgwick filter output)
LOG_INFO("Orientation: Roll=%.1f° Pitch=%.1f° Yaw=%.1f°",
fusionData.roll, fusionData.pitch, fusionData.yaw);
// Quality indicators
LOG_INFO("Quality: HDOP=%.2f Sats=%d HeadingAcc=%.1f°",
fusionData.hdop, fusionData.satellites, fusionData.heading_accuracy);
// GPS state details
if (fusionData.gps_valid) {
LOG_INFO("GPS State: Moving=%s Speed=%.2f Course=%.1f° (filtered)",
gps_state.moving ? "YES" : "NO",
gps_state.speed_filtered, gps_state.course_filtered);
}
// IMU state details
if (fusionData.imu_valid) {
LOG_INFO("IMU State: AccX=%.2f AccY=%.2f AccZ=%.2f",
g_qmi8658Live.acc.x, g_qmi8658Live.acc.y, g_qmi8658Live.acc.z);
LOG_INFO("IMU State: GyrX=%.2f GyrY=%.2f GyrZ=%.2f",
g_qmi8658Live.gyr.x, g_qmi8658Live.gyr.y, g_qmi8658Live.gyr.z);
}
// Timing information
LOG_INFO("Timing: GPS=%ums IMU=%ums Fusion=%ums ago",
now_ms - fusionData.last_gps_ms,
now_ms - fusionData.last_imu_ms,
@ -527,7 +371,6 @@ void GPSIMUFusion::logFusionDataDetailed() {
} else {
LOG_INFO("No valid sensor data available");
}
LOG_INFO("=== END FUSION DEBUG ===");
}
@ -535,7 +378,6 @@ void GPSIMUFusion::logFusionDataQuick() {
if (!fusionData.initialized) {
return;
}
if (fusionData.gps_valid || fusionData.imu_valid) {
LOG_INFO("FUSION: Pos(%.8f,%.8f) Spd=%.3fm/s Hdg=%.1f° GPS=%s IMU=%s",
fusionData.latitude, fusionData.longitude,

2
src/motion/GPSIMUFusion.h → src/Fusion/GPSIMUFusion.h
View File

@ -1,7 +1,7 @@
#pragma once
#include "configuration.h"
#include "SensorLiveData.h"
#include "motion/SensorLiveData.h"
#include "Fusion/Fusion.h"
#include <stdint.h>

4
src/motion/GPSIMUFusionDebug.h → src/Fusion/GPSIMUFusionDebug.h
View File

@ -11,7 +11,7 @@
* Include this in any file where you want to access fusion debug data.
*
* Usage:
* 1. Include this header: #include "motion/GPSIMUFusionDebug.h"
* 1. Include this header: #include "Fusion/GPSIMUFusionDebug.h"
* 2. Call debug functions as needed
*
* Auto Logging:
@ -97,4 +97,4 @@ inline float getFusionSpeed() {
return -1.0f;
}
#endif // !MESHTASTIC_EXCLUDE_GPS
#endif // !MESHTASTIC_EXCLUDE_GPS

5
src/motion/GPSIMUFusionDemo.cpp → src/Fusion/GPSIMUFusionDemo.cpp
View File

@ -1,9 +1,10 @@
#include "GPSIMUFusionDemo.h"
#include "configuration.h"
#if !MESHTASTIC_EXCLUDE_GPS
#include "GPSIMUFusion.h"
#include "SensorLiveData.h"
#include "motion/SensorLiveData.h"
#include <Arduino.h>
// Forward declarations for easy access to debug functions
@ -135,4 +136,4 @@ void exampleFusionUsage() {
}
}
#endif // !MESHTASTIC_EXCLUDE_GPS
#endif // !MESHTASTIC_EXCLUDE_GPS

13
src/Fusion/GPSIMUFusionDemo.h Normal file
View File

@ -0,0 +1,13 @@
#pragma once
#include "configuration.h"
#if !MESHTASTIC_EXCLUDE_GPS
// Forward declarations for demo functions
void debugGPSIMUFusionNow();
void quickGPSIMUFusionStatus();
void demonstrateGPSIMUFusion();
void exampleFusionUsage();
#endif // !MESHTASTIC_EXCLUDE_GPS

2
src/gps/GPS.cpp
View File

@ -23,7 +23,7 @@
// Optional GPS+IMU fusion support: enable with -DGPS_IMU_FUSION_ENABLE in build flags
#if defined(GPS_IMU_FUSION_ENABLE)
#include "motion/GPSIMUFusion.h"
#include "Fusion/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

2
src/motion/AccelerometerThread.h
View File

@ -27,7 +27,7 @@
#endif
#if !MESHTASTIC_EXCLUDE_GPS
#include "GPSIMUFusion.h"
#include "Fusion/GPSIMUFusion.h"
#endif
extern ScanI2C::DeviceAddress accelerometer_found;

6
src/motion/README_GPS_IMU_Debug.md
View File

@ -35,7 +35,7 @@ Timing: GPS=234ms IMU=45ms Fusion=12ms ago
### Option 1: Include Debug Header (Recommended)
```cpp
#include "motion/GPSIMUFusionDebug.h"
#include "Fusion/GPSIMUFusionDebug.h"
// In your code:
DEBUG_FUSION_NOW(); // Detailed debug output immediately
@ -44,7 +44,7 @@ DEBUG_FUSION_QUICK(); // Quick status check
### Option 2: Direct Function Calls
```cpp
#include "motion/GPSIMUFusion.h"
#include "Fusion/GPSIMUFusion.h"
// Force immediate debug output
debugGPSIMUFusionNow(); // Detailed debug
@ -178,7 +178,7 @@ if (speed >= 0) {
## Customization
### Adjust Log Frequency
Edit `GPSIMUFusion.cpp`:
Edit `src/Fusion/GPSIMUFusion.cpp`:
```cpp
// Change logging intervals
if (now_ms - lastDetailedLog > 10000) { // 10 seconds instead of 5

9
src/motion/README_GPS_IMU_Fusion.md
View File

@ -99,7 +99,7 @@ if (fusion && fusion->initialized) {
## Configuration
Key parameters can be adjusted in `GPSIMUFusion.cpp`:
Key parameters can be adjusted in `src/Fusion/GPSIMUFusion.cpp`:
```cpp
GPS_VELOCITY_THRESHOLD = 1.0f; // m/s - min speed for GPS heading
@ -118,9 +118,10 @@ The fusion system automatically:
## Files Added/Modified
### New Files
- `src/motion/GPSIMUFusion.h` - Main fusion class header
- `src/motion/GPSIMUFusion.cpp` - Fusion implementation
- `src/motion/GPSIMUFusionDemo.cpp` - Usage examples and demo code
- `src/Fusion/GPSIMUFusion.h` - Main fusion class header
- `src/Fusion/GPSIMUFusion.cpp` - Fusion implementation
- `src/Fusion/GPSIMUFusionDemo.cpp` - Usage examples and demo code
- `src/Fusion/GPSIMUFusionDebug.h` - Debug macros and helper functions
### Modified Files
- `src/motion/SensorLiveData.h` - Added fusion data accessor

2
src/motion/SensorLiveData.cpp
View File

@ -4,7 +4,7 @@ QMI8658LiveData g_qmi8658Live;
QMC6310LiveData g_qmc6310Live;
#if !MESHTASTIC_EXCLUDE_GPS
#include "GPSIMUFusion.h"
#include "Fusion/GPSIMUFusion.h"
const GPSIMUFusionData* getGPSIMUFusionData() {
if (g_gps_imu_fusion.isValid()) {