From ef6db75d5a5c949362bd35081404393f91c1c502 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 15 Mar 2022 09:25:18 +1100
Subject: [PATCH] AP_Math: added angle_to_accel() and accel_to_angle()

---
 libraries/AP_Math/control.cpp | 16 ++++++++++++++++
 libraries/AP_Math/control.h   | 10 ++++++++++
 2 files changed, 26 insertions(+)

diff --git a/libraries/AP_Math/control.cpp b/libraries/AP_Math/control.cpp
index 54431e5d6c..03adce568b 100644
--- a/libraries/AP_Math/control.cpp
+++ b/libraries/AP_Math/control.cpp
@@ -507,3 +507,19 @@ float input_expo(float input, float expo)
     }
     return input;
 }
+
+/*
+  convert a maximum lean angle in degrees to an accel limit in m/s/s
+ */
+float angle_to_accel(float angle_deg)
+{
+    return GRAVITY_MSS * tanf(radians(angle_deg));
+}
+
+/*
+  convert a maximum accel in m/s/s to a lean angle in degrees
+ */
+float accel_to_angle(float accel)
+{
+    return degrees(atanf((accel/GRAVITY_MSS)));
+}
diff --git a/libraries/AP_Math/control.h b/libraries/AP_Math/control.h
index 3ef5b0321b..ecec883200 100644
--- a/libraries/AP_Math/control.h
+++ b/libraries/AP_Math/control.h
@@ -145,3 +145,13 @@ float kinematic_limit(Vector3f direction, float max_xy, float max_z_pos, float m
 // The input must be in the range of -1 to 1.
 // The expo should be less than 1.0 but limited to be less than 0.95.
 float input_expo(float input, float expo);
+
+/*
+  convert a maximum lean angle in degrees to an accel limit in m/s/s
+ */
+float angle_to_accel(float angle_deg);
+
+/*
+  convert a maximum accel in m/s/s to a lean angle in degrees
+ */
+float accel_to_angle(float accel);