From 484a0b989f92b69b113e33dd566a86bcef700ffa Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Thu, 30 Sep 2021 22:16:15 +0100
Subject: [PATCH] AP_Motors: add expo inverse test example

---
 .../expo_inverse_test/expo_inverse_test.cpp   | 97 +++++++++++++++++++
 .../examples/expo_inverse_test/wscript        |  7 ++
 2 files changed, 104 insertions(+)
 create mode 100644 libraries/AP_Motors/examples/expo_inverse_test/expo_inverse_test.cpp
 create mode 100644 libraries/AP_Motors/examples/expo_inverse_test/wscript

diff --git a/libraries/AP_Motors/examples/expo_inverse_test/expo_inverse_test.cpp b/libraries/AP_Motors/examples/expo_inverse_test/expo_inverse_test.cpp
new file mode 100644
index 0000000000..00da1a15f6
--- /dev/null
+++ b/libraries/AP_Motors/examples/expo_inverse_test/expo_inverse_test.cpp
@@ -0,0 +1,97 @@
+/*
+   This program 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 program 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/>.
+ */
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Math/AP_Math.h>
+#include <AP_Motors/AP_Motors.h>
+
+/* run with:
+    ./waf configure --board linux
+    ./waf build --targets examples/expo_inverse_test
+    ./build/linux/examples/expo_inverse_test
+*/
+
+void setup();
+void loop();
+
+const AP_HAL::HAL& hal = AP_HAL::get_HAL();
+
+class AP_MotorsMulticopter_test : public AP_MotorsMulticopter {
+public:
+
+    AP_MotorsMulticopter_test(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
+        AP_MotorsMulticopter(loop_rate, speed_hz)
+    {
+    };
+
+    // have to have these functions as they are pure virtual
+    void init(motor_frame_class frame_class, motor_frame_type frame_type) override {};
+    void set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type) override {};
+    void output_test_seq(uint8_t motor_seq, int16_t pwm) override {};
+    const char* get_frame_string() const override { return "TEST"; }
+    void output_armed_stabilizing() override {};
+    void output_to_motors() override {};
+
+    // helper function to allow setting of expo
+    void set_expo(float v) { _thrust_curve_expo.set(v); }
+
+};
+
+AP_MotorsMulticopter_test motors{1};
+
+/*
+ *  rotation tests
+ */
+void setup(void)
+{
+    hal.console->begin(115200);
+    hal.console->printf("\n\nMotors expo inverse test\n\n");
+
+    const float expo_step = 0.01;
+    const float throttle_step = 0.01;
+
+    double max_diff = 0.0;
+    float max_diff_throttle = 0;
+    float max_diff_expo = 0;
+
+    float expo = -1.0;
+    while (expo < 1.0+expo_step*0.5) {
+        hal.console->printf("expo: %0.4f\n",expo);
+        motors.set_expo(expo);
+
+        float throttle = 0.0;
+        while (throttle < 1.0+throttle_step*0.5) {
+
+            const float throttle_out = motors.actuator_to_thrust(motors.thrust_to_actuator(throttle));
+            const double diff = fabsf(throttle_out - throttle);
+            if (diff > max_diff) {
+                max_diff_throttle = throttle;
+                max_diff_expo = expo;
+                max_diff = diff;
+            }
+
+            hal.console->printf("\tthrottle: %0.4f, error %0.8f\n", throttle, diff);
+
+            throttle += throttle_step;
+        }
+        hal.console->printf("\n");
+        expo += expo_step;
+    }
+
+    hal.console->printf("\nMotors expo inverse done, max error of %0.8f at expo %0.4f, throttle %0.4f\n\n", max_diff, max_diff_expo, max_diff_throttle);
+}
+
+void loop(void) {}
+
+AP_HAL_MAIN();
diff --git a/libraries/AP_Motors/examples/expo_inverse_test/wscript b/libraries/AP_Motors/examples/expo_inverse_test/wscript
new file mode 100644
index 0000000000..719ec151ba
--- /dev/null
+++ b/libraries/AP_Motors/examples/expo_inverse_test/wscript
@@ -0,0 +1,7 @@
+#!/usr/bin/env python
+# encoding: utf-8
+
+def build(bld):
+    bld.ap_example(
+        use='ap',
+    )