From 57b54a4263b43fb697455209727fc7bd4821254a Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 2 Jun 2023 14:48:02 +0900
Subject: [PATCH] AR_PosControl: add singleton and get_srate

---
 libraries/APM_Control/AR_PosControl.cpp | 14 ++++++++++++--
 libraries/APM_Control/AR_PosControl.h   | 10 ++++++++++
 2 files changed, 22 insertions(+), 2 deletions(-)

diff --git a/libraries/APM_Control/AR_PosControl.cpp b/libraries/APM_Control/AR_PosControl.cpp
index 6e64514014..9cf414831a 100644
--- a/libraries/APM_Control/AR_PosControl.cpp
+++ b/libraries/APM_Control/AR_PosControl.cpp
@@ -22,8 +22,6 @@
 #include <GCS_MAVLink/GCS.h>
 #include <AC_Avoidance/AC_Avoid.h>
 
-extern const AP_HAL::HAL& hal;
-
 #define AR_POSCON_TIMEOUT_MS            100     // timeout after 0.1 sec
 #define AR_POSCON_POS_P                 0.2f    // default position P gain
 #define AR_POSCON_VEL_P                 1.0f    // default velocity P gain
@@ -35,6 +33,10 @@ extern const AP_HAL::HAL& hal;
 #define AR_POSCON_VEL_FILT_D            5.0f    // default velocity D term filter
 #define AR_POSCON_DT                    0.02f   // default dt for PID controllers
 
+extern const AP_HAL::HAL& hal;
+
+AR_PosControl *AR_PosControl::_singleton;
+
 const AP_Param::GroupInfo AR_PosControl::var_info[] = {
 
     // @Param: _POS_P
@@ -103,6 +105,7 @@ AR_PosControl::AR_PosControl(AR_AttitudeControl& atc) :
     _p_pos(AR_POSCON_POS_P),
     _pid_vel(AR_POSCON_VEL_P, AR_POSCON_VEL_I, AR_POSCON_VEL_D, AR_POSCON_VEL_FF, AR_POSCON_VEL_IMAX, AR_POSCON_VEL_FILT, AR_POSCON_VEL_FILT_D)
 {
+    _singleton = this;
     AP_Param::setup_object_defaults(this, var_info);
 }
 
@@ -349,6 +352,13 @@ Vector2p AR_PosControl::get_pos_error() const
     return (_pos_target - curr_pos_NE.topostype());
 }
 
+// get the slew rate value for velocity.  used for oscillation detection in lua scripts
+void AR_PosControl::get_srate(float &velocity_srate)
+{
+    // slew rate is the same for x and y axis
+    velocity_srate = _pid_vel.get_pid_info_x().slew_rate;
+}
+
 // write PSC logs
 void AR_PosControl::write_log()
 {
diff --git a/libraries/APM_Control/AR_PosControl.h b/libraries/APM_Control/AR_PosControl.h
index d06d852fc4..40748fce30 100644
--- a/libraries/APM_Control/AR_PosControl.h
+++ b/libraries/APM_Control/AR_PosControl.h
@@ -11,6 +11,11 @@ public:
     // constructor
     AR_PosControl(AR_AttitudeControl& atc);
 
+    // do not allow copying
+    CLASS_NO_COPY(AR_PosControl);
+
+    static AR_PosControl *get_singleton() { return _singleton; }
+
     // update navigation
     void update(float dt);
 
@@ -73,6 +78,9 @@ public:
     AC_P_2D& get_pos_p() { return _p_pos; }
     AC_PID_2D& get_vel_pid() { return _pid_vel; }
 
+    // get the slew rate value for velocity.  used for oscillation detection in lua scripts
+    void get_srate(float &velocity_srate);
+
     // write PSC logs
     void write_log();
 
@@ -81,6 +89,8 @@ public:
 
 private:
 
+    static AR_PosControl *_singleton;
+
     // initialise and check for ekf position resets
     void init_ekf_xy_reset();
     void handle_ekf_xy_reset();