From 2769c28fb0d0f54c6503e860300166cafc9fbd94 Mon Sep 17 00:00:00 2001
From: Vladimir Demidov <demvlad2@yandex.ru>
Date: Tue, 24 Dec 2024 01:28:00 +0300
Subject: [PATCH] Add AUTOPILOT_POSITION and OPTICALFLOW debug modes (#803)

* The DEBUG modes list is updated. The OPTICALFLOW, AUTOPILOT_POSITION debugs are added

* added converters for OPTICALFLOW debug

* added converters for AUTOPILOT_POSITION debug
---
 src/flightlog_fielddefs.js        |  2 +
 src/flightlog_fields_presenter.js | 70 +++++++++++++++++++++++++++++--
 2 files changed, 69 insertions(+), 3 deletions(-)

diff --git a/src/flightlog_fielddefs.js b/src/flightlog_fielddefs.js
index d1d52384..0e1af5d4 100644
--- a/src/flightlog_fielddefs.js
+++ b/src/flightlog_fielddefs.js
@@ -530,12 +530,14 @@ export function adjustFieldDefsList(firmwareType, firmwareVersion) {
     if (semver.gte(firmwareVersion, "4.6.0")) {
       DEBUG_MODE.splice(DEBUG_MODE.indexOf('GPS_RESCUE_THROTTLE_PID'), 1, 'AUTOPILOT_ALTITUDE');
       DEBUG_MODE.splice(DEBUG_MODE.indexOf("GYRO_SCALED"), 1);
+      DEBUG_MODE.splice(DEBUG_MODE.indexOf("RANGEFINDER_QUALITY") + 1, 0, "OPTICALFLOW");
       DEBUG_MODE.push('TPA');
       DEBUG_MODE.push('S_TERM');
       DEBUG_MODE.push('SPA');
       DEBUG_MODE.push('TASK');
       DEBUG_MODE.push('GIMBAL');
       DEBUG_MODE.push('WING_SETPOINT');
+      DEBUG_MODE.push('AUTOPILOT_POSITION');
     }
 
     DEBUG_MODE = makeReadOnly(DEBUG_MODE);
diff --git a/src/flightlog_fields_presenter.js b/src/flightlog_fields_presenter.js
index e833100b..debf5340 100644
--- a/src/flightlog_fields_presenter.js
+++ b/src/flightlog_fields_presenter.js
@@ -32,7 +32,7 @@ const FRIENDLY_FIELD_NAMES = {
   "axisF[0]": "PID Feedforward [roll]",
   "axisF[1]": "PID Feedforward [pitch]",
   "axisF[2]": "PID Feedforward [yaw]",
-  
+
   "axisS[all]": "PID S",
   "axisS[0]": "PID S [roll]",
   "axisS[1]": "PID S [pitch]",
@@ -129,7 +129,7 @@ const FRIENDLY_FIELD_NAMES = {
   GPS_altitude: "GPS Altitude ASL",
   GPS_speed: "GPS Speed",
   GPS_ground_course: "GPS Heading",
-  
+
   "gpsCartesianCoords[all]": "GPS Coords",
   "gpsCartesianCoords[0]": "GPS Coords [X]",
   "gpsCartesianCoords[1]": "GPS Coords [Y]",
@@ -1388,6 +1388,26 @@ FlightLogFieldPresenter.adjustDebugDefsList = function (
         'debug[4]': 'Current Setpoint [yaw]',
         'debug[5]': 'Adjusted Setpoint [yaw]',
       };
+      DEBUG_FRIENDLY_FIELD_NAMES.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_FRIENDLY_FIELD_NAMES.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',
+      };
     }
   }
 };
@@ -1728,7 +1748,7 @@ FlightLogFieldPresenter.decodeFieldToFriendly = function (
       }
     case "GPS_ground_course":
       return `${(value / 10).toFixed(1)} °`;
-    
+
     case "gpsCartesianCoords[0]":
     case "gpsCartesianCoords[1]":
     case "gpsCartesianCoords[2]":
@@ -2151,6 +2171,28 @@ FlightLogFieldPresenter.decodeDebugFieldToFriendly = function (
         return value.toFixed(0);
       case "EZLANDING":
         return `${(value / 100.0).toFixed(2)} %`;
+      case "OPTICALFLOW":
+        switch (fieldName) {
+          case "debug[1]":
+          case "debug[2]":
+          case "debug[3]":
+          case "debug[4]":
+            return `${(value / 1000).toFixed(1)}`;
+          default:
+            return value.toFixed(1);
+        }
+      case "AUTOPILOT_POSITION":
+        switch (fieldName) {
+          case "debug[2]":
+          case "debug[3]":
+          case "debug[4]":
+          case "debug[5]":
+          case "debug[6]":
+          case "debug[7]":
+            return `${(value / 10).toFixed(1)}`;
+          default:
+            return value.toFixed(1);
+        }
     }
     return value.toFixed(0);
   }
@@ -2816,6 +2858,28 @@ FlightLogFieldPresenter.ConvertDebugFieldValue = function (
         return value;
       case "DSHOT_TELEMETRY_COUNTS":
         return value;
+      case "OPTICALFLOW":
+        switch (fieldName) {
+          case "debug[1]":
+          case "debug[2]":
+          case "debug[3]":
+          case "debug[4]":
+            return toFriendly ? value / 1000 : value * 1000;
+          default:
+            return value;
+        }
+      case "AUTOPILOT_POSITION":
+        switch (fieldName) {
+          case "debug[2]":
+          case "debug[3]":
+          case "debug[4]":
+          case "debug[5]":
+          case "debug[6]":
+          case "debug[7]":
+            return toFriendly ? value / 10 : value * 10;
+          default:
+            return value;
+        }
     }
   }
   return value;