From 174534a45d7cd335d99fca290d78e29e957f0ef3 Mon Sep 17 00:00:00 2001 From: Vladimir Demidov Date: Mon, 23 Dec 2024 23:24:07 +0300 Subject: [PATCH] Add AUTOPILOT_POSITION and OPTICALFLOW debug modes (#4284) * Updated debug modes list. The OPTICALFLOW, AUTOPILOT_POSITION debugs are added * Added debug fields names for OPTICALFLOW and AUTOPILOT_POSITION debugs * Optical flow debug caption is edited Co-authored-by: Mark Haslinghuis * Autopilot debug fields caption is edited Co-authored-by: Mark Haslinghuis --------- Co-authored-by: Mark Haslinghuis --- src/js/debug.js | 25 ++++++++++++++++++++++++- 1 file changed, 24 insertions(+), 1 deletion(-) diff --git a/src/js/debug.js b/src/js/debug.js index f342b95ba6..fd986676e2 100644 --- a/src/js/debug.js +++ b/src/js/debug.js @@ -821,7 +821,8 @@ function update() { if (semver.gte(FC.CONFIG.apiVersion, API_VERSION_1_47)) { DEBUG.modes.splice(DEBUG.modes.indexOf('GPS_RESCUE_THROTTLE_PID'), 1, 'AUTOPILOT_ALTITUDE'); DEBUG.modes.splice(DEBUG.modes.indexOf('GYRO_SCALED'), 1); - + DEBUG.modes.splice(DEBUG.modes.indexOf('RANGEFINDER_QUALITY') + 1, 0, 'OPTICALFLOW'); + DEBUG.modes.push('AUTOPILOT_POSITION'); delete DEBUG.fieldNames.GPS_RESCUE_THROTTLE_PID; delete DEBUG.fieldNames.GYRO_SCALED; @@ -859,6 +860,28 @@ function update() { 'debug[5]': 'TPA Argument (Wing)', }; + DEBUG.fieldNames.OPTICALFLOW = { + 'debug[all]': 'Optical Flow', + 'debug[0]': 'Quality', + 'debug[1]': 'Raw flow rates X', + 'debug[2]': 'Raw flow rates Y', + 'debug[3]': 'Processed flow rates X', + 'debug[4]': 'Processed flow rates Y', + 'debug[5]': 'Delta time', + }; + + DEBUG.fieldNames.AUTOPILOT_POSITION = { + 'debug[all]': 'Autopilot Position', + 'debug[0]': 'Distance', + 'debug[1]': 'GPS Distance', + 'debug[2]': 'PID Sum EF', + 'debug[3]': 'Angle', + 'debug[4]': 'pidP', + 'debug[5]': 'pidI', + 'debug[6]': 'pidD', + 'debug[7]': 'pidA', + }; + DEBUG.enableFields.splice(DEBUG.enableFields.indexOf("Gyro") + 1, 0, "Attitude"); DEBUG.enableFields.push("Servo"); }