AP_Math: add control common functions

libraries/AP_Math/AP_Math.h

@@ -18,6 +18,7 @@
 #include "vector3.h"
 #include "spline5.h"
 #include "location.h"
+#include "control.h"
 
 // define AP_Param types AP_Vector3f and Ap_Matrix3f
 AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F);

libraries/AP_Math/control.cpp

@@ -0,0 +1,71 @@
+/*
+ * control.cpp
+ * Copyright (C) Leonard Hall 2020
+ *
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ *  this module provides common controller functions
+ */
+#include "AP_Math.h"
+#include "vector2.h"
+#include "vector3.h"
+
+// Proportional controller with piecewise sqrt sections to constrain second derivative
+float sqrt_controller(float error, float p, float second_ord_lim, float dt)
+{
+    float correction_rate;
+    if (is_negative(second_ord_lim) || is_zero(second_ord_lim)) {
+        // second order limit is zero or negative.
+        correction_rate = error * p;
+    } else if (is_zero(p)) {
+        // P term is zero but we have a second order limit.
+        if (is_positive(error)) {
+            correction_rate = safe_sqrt(2.0f * second_ord_lim * (error));
+        } else if (is_negative(error)) {
+            correction_rate = -safe_sqrt(2.0f * second_ord_lim * (-error));
+        } else {
+            correction_rate = 0.0f;
+        }
+    } else {
+        // Both the P and second order limit have been defined.
+        float linear_dist = second_ord_lim / sq(p);
+        if (error > linear_dist) {
+            correction_rate = safe_sqrt(2.0f * second_ord_lim * (error - (linear_dist / 2.0f)));
+        } else if (error < -linear_dist) {
+            correction_rate = -safe_sqrt(2.0f * second_ord_lim * (-error - (linear_dist / 2.0f)));
+        } else {
+            correction_rate = error * p;
+        }
+    }
+    if (!is_zero(dt)) {
+        // this ensures we do not get small oscillations by over shooting the error correction in the last time step.
+        return constrain_float(correction_rate, -fabsf(error) / dt, fabsf(error) / dt);
+    } else {
+        return correction_rate;
+    }
+}
+
+// limit vector to a given length, returns true if vector was limited
+bool limit_vector_length(float &vector_x, float &vector_y, float max_length)
+{
+    const float vector_length = norm(vector_x, vector_y);
+    if ((vector_length > max_length) && is_positive(vector_length)) {
+        vector_x *= (max_length / vector_length);
+        vector_y *= (max_length / vector_length);
+        return true;
+    }
+    return false;
+}

libraries/AP_Math/control.h

@@ -0,0 +1,11 @@
+#pragma once
+
+/*
+  common controller helper functions
+ */
+
+// Proportional controller with piecewise sqrt sections to constrain second derivative
+float sqrt_controller(float error, float p, float second_ord_lim, float dt);
+
+// limit vector to a given length, returns true if vector was limited
+bool limit_vector_length(float &vector_x, float &vector_y, float max_length);