From e884e4c5ac40e1826100b04048ad28354cad7f4d Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 27 Feb 2021 13:36:36 +1100
Subject: [PATCH] AP_Math: added fixedwing_turn_rate() helper

---
 libraries/AP_Math/AP_Math.cpp | 10 ++++++++++
 libraries/AP_Math/AP_Math.h   |  5 +++++
 2 files changed, 15 insertions(+)

diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp
index e2323b38d0..0800e10f1b 100644
--- a/libraries/AP_Math/AP_Math.cpp
+++ b/libraries/AP_Math/AP_Math.cpp
@@ -404,3 +404,13 @@ void fill_nanf(float *f, uint16_t count)
     }
 }
 #endif
+
+/*
+  calculate turn rate in deg/sec given a bank angle and airspeed for a
+  fixed wing aircraft
+ */
+float fixedwing_turn_rate(float bank_angle_deg, float airspeed)
+{
+    bank_angle_deg = constrain_float(bank_angle_deg, -80, 80);
+    return degrees(GRAVITY_MSS*tanf(radians(bank_angle_deg))/MAX(airspeed,1));
+}
diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h
index 63ee3dd405..3b142ad1e8 100644
--- a/libraries/AP_Math/AP_Math.h
+++ b/libraries/AP_Math/AP_Math.h
@@ -297,3 +297,8 @@ float calc_lowpass_alpha_dt(float dt, float cutoff_freq);
 void fill_nanf(float *f, uint16_t count);
 #endif
 
+/*
+  calculate turn rate in deg/sec given a bank angle and airspeed for a
+  fixed wing aircraft
+ */
+float fixedwing_turn_rate(float bank_angle_deg, float airspeed);