From a3aeef880aec187c68582c0d7775593cb26a4219 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 9 Aug 2022 15:50:27 +1000
Subject: [PATCH] AP_Math: add dignostic message for SCurve internal error

---
 libraries/AP_Math/SCurve.cpp | 46 ++++++++++++++++++++++++++++++++++++
 1 file changed, 46 insertions(+)

diff --git a/libraries/AP_Math/SCurve.cpp b/libraries/AP_Math/SCurve.cpp
index 5d60eacd03..074b3e0603 100644
--- a/libraries/AP_Math/SCurve.cpp
+++ b/libraries/AP_Math/SCurve.cpp
@@ -16,6 +16,10 @@
 #include <AP_Math/AP_Math.h>
 #include <AP_HAL/AP_HAL.h>
 #include <AP_InternalError/AP_InternalError.h>
+#include <AP_Vehicle/AP_Vehicle_Type.h>
+#if APM_BUILD_COPTER_OR_HELI
+#include <AP_Logger/AP_Logger.h>
+#endif
 #include "SCurve.h"
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
@@ -862,6 +866,48 @@ void SCurve::calculate_path(float Sm, float Jm, float V0, float Am, float Vm, fl
         ::printf("SCurve::calculate_path invalid outputs\n");
 #endif
         INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
+
+#if APM_BUILD_COPTER_OR_HELI
+        // @LoggerMessage: SCVE
+        // @Description: Debug message for SCurve internal error
+        // @Field: TimeUS: Time since system startup
+        // @Field: Sm: duration of the raised cosine jerk profile
+        // @Field: Jm: maximum value of the raised cosine jerk profile
+        // @Field: V0: initial velocity magnitude
+        // @Field: Am: maximum constant acceleration
+        // @Field: Vm: maximum constant velocity
+        // @Field: L: Length of the path
+        // @Field: Jm_out: maximum value of the raised cosine jerk profile
+        // @Field: tj_out: segment duration
+        // @Field: t2_out: segment duration
+        // @Field: t4_out: segment duration
+        // @Field: t6_out: segment duration
+
+        static bool logged_scve;  // only log once
+        if (!logged_scve) {
+            logged_scve = true;
+            AP::logger().Write(
+                "SCVE",
+                "TimeUS,Sm,Jm,V0,Am,Vm,L,Jm_out,tj_out,t2_out,t4_out,t6_out",
+                "s-----------",
+                "F-----------",
+                "Qfffffffffff",
+                AP_HAL::micros64(),
+                (double)Sm,
+                (double)Jm,
+                (double)V0,
+                (double)Am,
+                (double)Vm,
+                (double)L,
+                (double)Jm_out,
+                (double)tj_out,
+                (double)t2_out,
+                (double)t4_out,
+                (double)t6_out
+                );
+        }
+#endif  // APM_BUILD_COPTER_OR_HELI
+
         Jm_out = 0.0f;
         t2_out = 0.0f;
         t4_out = 0.0f;