From d0e604a7cad3dc41d3b45b571cb9199957e9f2b8 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sun, 14 Mar 2021 20:31:52 +0100 Subject: [PATCH 1/4] =?UTF-8?q?=EF=BB=BFaltitude=20estimation=20debug=20mo?= =?UTF-8?q?de?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/build/debug.h | 1 + src/main/fc/settings.yaml | 2 +- src/main/navigation/navigation_pos_estimator.c | 9 +++++++++ 3 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/build/debug.h b/src/main/build/debug.h index d8916f6d56f..e8f47543ebc 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -82,5 +82,6 @@ typedef enum { DEBUG_DYNAMIC_GYRO_LPF, DEBUG_FW_D, DEBUG_IMU2, + DEBUG_ALTITUDE, DEBUG_COUNT } debugType_e; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 5be6038db37..1521c3cfcb7 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -87,7 +87,7 @@ tables: "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE", "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY", - "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2"] + "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE"] - name: async_mode values: ["NONE", "GYRO", "ALL"] - name: aux_operator diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 496aeb6c1f0..7f8d6245073 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -611,6 +611,15 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) return true; } + DEBUG_SET(DEBUG_ALTITUDE, 0, posEstimator.est.pos.z); // Position estimate + DEBUG_SET(DEBUG_ALTITUDE, 2, posEstimator.baro.alt); // Baro altitude + DEBUG_SET(DEBUG_ALTITUDE, 4, posEstimator.gps.pos.z); // GPS altitude + DEBUG_SET(DEBUG_ALTITUDE, 6, accGetVibrationLevel()); // Vibration level + DEBUG_SET(DEBUG_ALTITUDE, 1, posEstimator.est.vel.z); // Vertical speed estimate + DEBUG_SET(DEBUG_ALTITUDE, 3, posEstimator.gps.vel.z); // GPS vertical speed + DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.imu.accelNEU.z); // Vertical acceleration on earth frame + DEBUG_SET(DEBUG_ALTITUDE, 7, navGetAccelerometerWeight()); // Acceleromter weight in position/speed estimation + return false; } From 431af0fbf71e4c756db270e3ed8c347a04ab9aba Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Tue, 6 Apr 2021 17:21:33 +0200 Subject: [PATCH 2/4] acc weight scaling --- src/main/navigation/navigation_pos_estimator.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 7f8d6245073..594eff636f5 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -618,7 +618,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) DEBUG_SET(DEBUG_ALTITUDE, 1, posEstimator.est.vel.z); // Vertical speed estimate DEBUG_SET(DEBUG_ALTITUDE, 3, posEstimator.gps.vel.z); // GPS vertical speed DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.imu.accelNEU.z); // Vertical acceleration on earth frame - DEBUG_SET(DEBUG_ALTITUDE, 7, navGetAccelerometerWeight()); // Acceleromter weight in position/speed estimation + DEBUG_SET(DEBUG_ALTITUDE, 7, navGetAccelerometerWeight()*1000); // Acceleromter weight in position/speed estimation return false; } From 9b6a805c96dadfb20cfc12f3ab3847ee9f3133ac Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Tue, 6 Apr 2021 17:26:54 +0200 Subject: [PATCH 3/4] debug element order --- src/main/navigation/navigation_pos_estimator.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 594eff636f5..88a25c418b0 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -616,8 +616,8 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) DEBUG_SET(DEBUG_ALTITUDE, 4, posEstimator.gps.pos.z); // GPS altitude DEBUG_SET(DEBUG_ALTITUDE, 6, accGetVibrationLevel()); // Vibration level DEBUG_SET(DEBUG_ALTITUDE, 1, posEstimator.est.vel.z); // Vertical speed estimate - DEBUG_SET(DEBUG_ALTITUDE, 3, posEstimator.gps.vel.z); // GPS vertical speed - DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.imu.accelNEU.z); // Vertical acceleration on earth frame + DEBUG_SET(DEBUG_ALTITUDE, 3, posEstimator.imu.accelNEU.z); // Vertical acceleration on earth frame + DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.gps.vel.z); // GPS vertical speed DEBUG_SET(DEBUG_ALTITUDE, 7, navGetAccelerometerWeight()*1000); // Acceleromter weight in position/speed estimation return false; From 1b8d890a672c77296dbdcb42bb08bebd7fb9d816 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Tue, 6 Apr 2021 21:09:57 +0200 Subject: [PATCH 4/4] acc clip count instead of acc weight --- src/main/navigation/navigation_pos_estimator.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 88a25c418b0..e78ffb445a0 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -618,7 +618,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) DEBUG_SET(DEBUG_ALTITUDE, 1, posEstimator.est.vel.z); // Vertical speed estimate DEBUG_SET(DEBUG_ALTITUDE, 3, posEstimator.imu.accelNEU.z); // Vertical acceleration on earth frame DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.gps.vel.z); // GPS vertical speed - DEBUG_SET(DEBUG_ALTITUDE, 7, navGetAccelerometerWeight()*1000); // Acceleromter weight in position/speed estimation + DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount()); // Clip count return false; }