From 74c702674b2f5a5762524f42fa450f037a88f78c Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 16 Dec 2024 20:45:50 +0900
Subject: [PATCH] AC_PosControl: add get_vel_target and get_accel_target

---
 .../AC_AttitudeControl/AC_PosControl.cpp      | 22 +++++++++++++++++++
 libraries/AC_AttitudeControl/AC_PosControl.h  |  6 +++++
 2 files changed, 28 insertions(+)

diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp
index c50c7581e8..5b768c99f9 100644
--- a/libraries/AC_AttitudeControl/AC_PosControl.cpp
+++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp
@@ -1208,6 +1208,28 @@ bool AC_PosControl::get_posvelaccel_offset(Vector3f &pos_offset_NED, Vector3f &v
     accel_offset_NED.z = -_accel_offset_target.z * 0.01;
     return true;
 }
+
+// get target velocity in m/s in NED frame
+bool AC_PosControl::get_vel_target(Vector3f &vel_target_NED)
+{
+    if (!is_active_xy() || !is_active_z()) {
+        return false;
+    }
+    vel_target_NED.xy() = _vel_target.xy() * 0.01;
+    vel_target_NED.z = -_vel_target.z * 0.01;
+    return true;
+}
+
+// get target acceleration in m/s/s in NED frame
+bool AC_PosControl::get_accel_target(Vector3f &accel_target_NED)
+{
+    if (!is_active_xy() || !is_active_z()) {
+        return false;
+    }
+    accel_target_NED.xy() = _accel_target.xy() * 0.01;
+    accel_target_NED.z = -_accel_target.z * 0.01;
+    return true;
+}
 #endif
 
 /// set the horizontal position, velocity and acceleration offset targets in cm, cms and cm/s/s from EKF origin in NE frame
diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h
index f52c7a9c5c..ff5de98498 100644
--- a/libraries/AC_AttitudeControl/AC_PosControl.h
+++ b/libraries/AC_AttitudeControl/AC_PosControl.h
@@ -339,6 +339,12 @@ public:
     // units are m, m/s and m/s/s in NED frame
     bool set_posvelaccel_offset(const Vector3f &pos_offset_NED, const Vector3f &vel_offset_NED, const Vector3f &accel_offset_NED);
     bool get_posvelaccel_offset(Vector3f &pos_offset_NED, Vector3f &vel_offset_NED, Vector3f &accel_offset_NED);
+
+    // get target velocity in m/s in NED frame
+    bool get_vel_target(Vector3f &vel_target_NED);
+
+    // get target acceleration in m/s/s in NED frame
+    bool get_accel_target(Vector3f &accel_target_NED);
 #endif
 
     /// set the horizontal position, velocity and acceleration offset targets in cm, cms and cm/s/s from EKF origin in NE frame