Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Math: add dignostic message for SCurve internal error

This commit is contained in:
Peter Barker 2022-08-09 15:50:27 +10:00 committed by Peter Barker
parent e29374fa04
commit f345c16fda
1 changed files with 46 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
libraries/AP_Math/SCurve.cpp
View File

@ -16,6 +16,10 @@
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_InternalError/AP_InternalError.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" #include "SCurve.h"
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #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"); ::printf("SCurve::calculate_path invalid outputs\n");
#endif #endif
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result); 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; Jm_out = 0.0f;
t2_out = 0.0f; t2_out = 0.0f;
t4_out = 0.0f; t4_out = 0.0f;