From cf6665625847e2ec237e78ce971b4ddd8098fa03 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Sat, 1 Dec 2018 08:54:12 +0100
Subject: [PATCH] mixer multirotor: add unit-tests
To run:
cd src/lib/mixer
make tests
This will validate the C++ implementation by taking the python
implementation as ground-truth. It runs through various actuator control
command values for all airmode variations and several mixer types.
The python script also allows to prototype new mixer algorithms.
It is not integrated into the existing build system, because it's easier
to use that way, with less dependencies, and faster testing workflow.
It could however be a bit more integrated.
Reference: https://github.com/Auterion/Flight_Control_Prototyping_Scripts/tree/master/control_allocation
---
src/lib/mixer/.gitignore | 2 +
src/lib/mixer/Makefile | 9 +
.../geometries/tools/px_generate_mixers.py | 0
src/lib/mixer/mixer.h | 14 +
src/lib/mixer/mixer_multirotor.cpp | 46 +++
src/lib/mixer/mixer_multirotor.py | 378 ++++++++++++++++++
src/lib/mixer/test_mixer_multirotor.cpp | 168 ++++++++
src/systemcmds/tests/test_mixer.cpp | 2 +-
8 files changed, 618 insertions(+), 1 deletion(-)
create mode 100644 src/lib/mixer/.gitignore
create mode 100644 src/lib/mixer/Makefile
mode change 100644 => 100755 src/lib/mixer/geometries/tools/px_generate_mixers.py
create mode 100755 src/lib/mixer/mixer_multirotor.py
create mode 100644 src/lib/mixer/test_mixer_multirotor.cpp
diff --git a/src/lib/mixer/.gitignore b/src/lib/mixer/.gitignore
new file mode 100644
index 0000000000..df7e57e698
--- /dev/null
+++ b/src/lib/mixer/.gitignore
@@ -0,0 +1,2 @@
+
+/test_mixer_multirotor
diff --git a/src/lib/mixer/Makefile b/src/lib/mixer/Makefile
new file mode 100644
index 0000000000..3ad4b70b97
--- /dev/null
+++ b/src/lib/mixer/Makefile
@@ -0,0 +1,9 @@
+
+.PHONY: all tests
+all: test_mixer_multirotor
+
+test_mixer_multirotor: test_mixer_multirotor.cpp mixer_multirotor.cpp mixer.cpp
+ g++ $^ -I .. -DMIXER_MULTIROTOR_USE_MOCK_GEOMETRY -o $@
+
+tests: test_mixer_multirotor
+ python mixer_multirotor.py --test --mixer-multirotor-binary ./$^
diff --git a/src/lib/mixer/geometries/tools/px_generate_mixers.py b/src/lib/mixer/geometries/tools/px_generate_mixers.py
old mode 100644
new mode 100755
diff --git a/src/lib/mixer/mixer.h b/src/lib/mixer/mixer.h
index 011db6b98d..c9220af5ac 100644
--- a/src/lib/mixer/mixer.h
+++ b/src/lib/mixer/mixer.h
@@ -641,6 +641,20 @@ public:
float pitch_scale,
float yaw_scale,
float idle_speed);
+
+ /**
+ * Constructor (for testing).
+ *
+ * @param control_cb Callback invoked to read inputs.
+ * @param cb_handle Passed to control_cb.
+ * @param rotors control allocation matrix
+ * @param rotor_count length of rotors
+ */
+ MultirotorMixer(ControlCallback control_cb,
+ uintptr_t cb_handle,
+ Rotor *rotors,
+ unsigned rotor_count);
+
~MultirotorMixer();
/**
diff --git a/src/lib/mixer/mixer_multirotor.cpp b/src/lib/mixer/mixer_multirotor.cpp
index 069bda4e6b..eaf6a6b5ee 100644
--- a/src/lib/mixer/mixer_multirotor.cpp
+++ b/src/lib/mixer/mixer_multirotor.cpp
@@ -45,10 +45,35 @@
#include <mathlib/mathlib.h>
+#ifdef MIXER_MULTIROTOR_USE_MOCK_GEOMETRY
+enum class MultirotorGeometry : MultirotorGeometryUnderlyingType {
+ QUAD_X,
+ MAX_GEOMETRY
+};
+namespace
+{
+const MultirotorMixer::Rotor _config_quad_x[] = {
+ { -0.707107, 0.707107, 1.000000, 1.000000 },
+ { 0.707107, -0.707107, 1.000000, 1.000000 },
+ { 0.707107, 0.707107, -1.000000, 1.000000 },
+ { -0.707107, -0.707107, -1.000000, 1.000000 },
+};
+const MultirotorMixer::Rotor *_config_index[] = {
+ &_config_quad_x[0]
+};
+const unsigned _config_rotor_count[] = {4};
+const char *_config_key[] = {"4x"};
+}
+
+#else
+
// This file is generated by the px_generate_mixers.py script which is invoked during the build process
// #include "mixer_multirotor.generated.h"
#include "mixer_multirotor_normalized.generated.h"
+#endif /* MIXER_MULTIROTOR_USE_MOCK_GEOMETRY */
+
+
#define debug(fmt, args...) do { } while(0)
//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
//#include <debug.h>
@@ -78,6 +103,27 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb,
_outputs_prev[i] = _idle_speed;
}
}
+MultirotorMixer::MultirotorMixer(ControlCallback control_cb,
+ uintptr_t cb_handle,
+ Rotor *rotors,
+ unsigned rotor_count) :
+ Mixer(control_cb, cb_handle),
+ _roll_scale(1.f),
+ _pitch_scale(1.f),
+ _yaw_scale(1.f),
+ _idle_speed(0.f),
+ _delta_out_max(0.0f),
+ _thrust_factor(0.0f),
+ _airmode(Airmode::disabled),
+ _rotor_count(rotor_count),
+ _rotors(rotors),
+ _outputs_prev(new float[_rotor_count]),
+ _tmp_array(new float[_rotor_count])
+{
+ for (unsigned i = 0; i < _rotor_count; ++i) {
+ _outputs_prev[i] = _idle_speed;
+ }
+}
MultirotorMixer::~MultirotorMixer()
{
diff --git a/src/lib/mixer/mixer_multirotor.py b/src/lib/mixer/mixer_multirotor.py
new file mode 100755
index 0000000000..d4095ba9b0
--- /dev/null
+++ b/src/lib/mixer/mixer_multirotor.py
@@ -0,0 +1,378 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+"""
+Mixer multirotor test and prototyping script.
+
+Author: Mathieu Bresciani <brescianimathieu@gmail.com>, Beat Kueng <beat-kueng@gmx.net>
+Description: This script can be used to prototype new mixer algorithms and test
+it against the C++ implementation.
+"""
+
+
+from __future__ import print_function
+
+from argparse import ArgumentParser
+import numpy as np
+import numpy.matlib
+import subprocess
+
+
+# --------------------------------------------------
+# mixing algorithms
+# --------------------------------------------------
+
+def compute_desaturation_gain(u, u_min, u_max, delta_u):
+ """
+ Computes the gain k by which delta_u has to be multiplied
+ in order to unsaturate the output that has the greatest saturation
+ """
+ d_u_sat_plus = u_max - u
+ d_u_sat_minus = u_min - u
+ k = np.zeros(u.size*2)
+ for i in range(u.size):
+ if abs(delta_u[i]) < 0.000001:
+ # avoid division by zero
+ continue
+
+ if d_u_sat_minus[i] > 0.0:
+ k[2*i] = d_u_sat_minus[i] / delta_u[i]
+ if d_u_sat_plus[i] < 0.0:
+ k[2*i+1] = d_u_sat_plus[i] / delta_u[i]
+
+ k_min = min(k)
+ k_max = max(k)
+
+ # Reduce the saturation as much as possible
+ k = k_min + k_max
+ return k
+
+
+def minimize_sat(u, u_min, u_max, delta_u):
+ """
+ Minimize the saturation of the actuators by
+ adding or substracting a fraction of delta_u.
+ Delta_u is the vector that added to the output u,
+ modifies the thrust or angular acceleration on a
+ specific axis.
+ For example, if delta_u is given
+ to slide along the vertical thrust axis, the saturation will
+ be minimized by shifting the vertical thrust setpoint,
+ without changing the roll/pitch/yaw accelerations.
+ """
+ k_1 = compute_desaturation_gain(u, u_min, u_max, delta_u)
+ u_1 = u + k_1 * delta_u # Try to unsaturate
+
+
+ # Compute the desaturation gain again based on the updated outputs.
+ # In most cases it will be zero. It won't be if max(outputs) - min(outputs)
+ # > max_output - min_output.
+ # In that case adding 0.5 of the gain will equilibrate saturations.
+ k_2 = compute_desaturation_gain(u_1, u_min, u_max, delta_u)
+
+ k_opt = k_1 + 0.5 * k_2
+
+ u_prime = u + k_opt * delta_u
+ return u_prime
+
+def mix_yaw(m_sp, u, P, u_min, u_max):
+ """
+ Mix yaw by adding it to an existing output vector u
+
+ Desaturation behavior: thrust is allowed to be decreased up to 15% in order to allow
+ some yaw control on the upper end. On the lower end thrust will never be increased,
+ but yaw is decreased as much as required.
+ """
+ m_sp_yaw_only = np.matlib.zeros(m_sp.size).T
+ m_sp_yaw_only[2, 0] = m_sp[2, 0]
+ u_p = u + P * m_sp_yaw_only
+
+ # Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch),
+ # and allow some yaw response at maximum thrust
+ u_r_dot = P[:,2]
+ u_pp = minimize_sat(u_p, u_min, u_max+0.15, u_r_dot)
+ u_T = P[:, 3]
+ u_ppp = minimize_sat(u_pp, -1000, u_max, u_T)
+ return u_ppp
+
+def airmode_rp(m_sp, P, u_min, u_max):
+ """
+ Mix roll, pitch, yaw and thrust.
+
+ Desaturation behavior: airmode for roll/pitch:
+ thrust is increased/decreased as much as required to meet the demanded roll/pitch.
+ Yaw is not allowed to increase the thrust, @see mix_yaw() for the exact behavior.
+ """
+ # Mix without yaw
+ m_sp_no_yaw = m_sp.copy()
+ m_sp_no_yaw[2, 0] = 0.0
+ u = P * m_sp_no_yaw
+
+ # Use thrust to unsaturate the outputs if needed
+ u_T = P[:, 3]
+ u_prime = minimize_sat(u, u_min, u_max, u_T)
+
+ # Mix yaw axis independently
+ u_final = mix_yaw(m_sp, u_prime, P, u_min, u_max)
+
+ return (u, u_final)
+
+
+def airmode_rpy(m_sp, P, u_min, u_max):
+ """
+ Mix roll, pitch, yaw and thrust.
+
+ Desaturation behavior: full airmode for roll/pitch/yaw:
+ thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw.
+ """
+ # Mix with yaw
+ u = P * m_sp
+
+ # Use thrust to unsaturate the outputs if needed
+ u_T = P[:, 3]
+ u_prime = minimize_sat(u, u_min, u_max, u_T)
+ return (u, u_prime)
+
+
+def normal_mode(m_sp, P, u_min, u_max):
+ """
+ Mix roll, pitch, yaw and thrust.
+
+ Desaturation behavior: no airmode, thrust is NEVER increased to meet the demanded
+ roll/pitch/yaw. Instead roll/pitch/yaw is reduced as much as needed.
+ Thrust can be reduced to unsaturate the upper side.
+ @see mix_yaw() for the exact yaw behavior.
+ """
+ # Mix without yaw
+ m_sp_no_yaw = m_sp.copy()
+ m_sp_no_yaw[2, 0] = 0.0
+ u = P * m_sp_no_yaw
+
+ # Use thrust to unsaturate the outputs if needed
+ # by reducing the thrust only
+ u_T = P[:, 3]
+ u_prime = minimize_sat(u, u_min, u_max, u_T)
+ if (u_prime > (u)).any():
+ u_prime = u
+
+ # Reduce roll/pitch acceleration if needed to unsaturate
+ u_p_dot = P[:, 0]
+ u_p2 = minimize_sat(u_prime, u_min, u_max, u_p_dot)
+ u_q_dot = P[:, 1]
+ u_p3 = minimize_sat(u_p2, u_min, u_max, u_q_dot)
+
+ # Mix yaw axis independently
+ u_final = mix_yaw(m_sp, u_p3, P, u_min, u_max)
+ return (u, u_final)
+
+
+# --------------------------------------------------
+# test cases
+# --------------------------------------------------
+
+# normalized control allocation test matrices (B_px from px_generate_mixers.py)
+# quad_x
+P1 = np.matrix([
+ [-0.71, 0.71, 1., 1. ],
+ [ 0.71, -0.71, 1., 1. ],
+ [ 0.71, 0.71, -1., 1. ],
+ [-0.71, -0.71, -1., 1. ]])
+# quad_wide
+P2 = np.matrix([
+ [-0.5, 0.71, 0.77, 1. ],
+ [ 0.5, -0.71, 1., 1. ],
+ [ 0.5, 0.71, -0.77, 1. ],
+ [-0.5, -0.71, -1., 1. ]])
+# hex_x
+P3 = np.matrix([
+ [-1., 0., -1., 1. ],
+ [ 1., -0., 1., 1. ],
+ [ 0.5, 0.87, -1., 1. ],
+ [-0.5, -0.87, 1., 1. ],
+ [-0.5, 0.87, 1., 1. ],
+ [ 0.5, -0.87, -1., 1. ]])
+# hex_cox
+P4 = np.matrix([
+ [-0.87, 0.5, -1., 1. ],
+ [-0.87, 0.5, 1., 1. ],
+ [ 0., -1., -1., 1. ],
+ [ 0., -1., 1., 1. ],
+ [ 0.87, 0.5, -1., 1. ],
+ [ 0.87, 0.5, 1., 1. ]])
+# octa_plus
+P5 = np.matrix([
+ [-0., 1., -1., 1. ],
+ [ 0., -1., -1., 1. ],
+ [-0.71, 0.71, 1., 1. ],
+ [-0.71, -0.71, 1., 1. ],
+ [ 0.71, 0.71, 1., 1. ],
+ [ 0.71, -0.71, 1., 1. ],
+ [ 1., 0., -1., 1. ],
+ [-1., -0., -1., 1. ]])
+
+P_tests = [ P1, P2, P3, P4, P5 ]
+
+test_cases_input = np.matrix([
+ # desired accelerations (must be within [-1, 1]):
+ #roll pitch yaw thrust
+ [ 0.0, 0.0, 0.0, 0.0],
+ [-0.05, 0.0, 0.0, 0.0],
+ [ 0.05, -0.05, 0.0, 0.0],
+ [ 0.05, 0.05, -0.025, 0.0],
+ [ 0.0, 0.2, -0.025, 0.0],
+ [ 0.2, 0.05, 0.09, 0.0],
+ [-0.125, 0.02, 0.04, 0.0],
+
+ # extreme cases
+ [ 1.0, 0.0, 0.0, 0.0],
+ [ 0.0, -1.0, 0.0, 0.0],
+ [ 0.0, 0.0, 1.0, 0.0],
+ [ 1.0, 1.0, -1.0, 0.0],
+ [-1.0, 0.9, -0.9, 0.0],
+ [-1.0, 0.9, 0.0, 0.0],
+ ])
+
+# use the following thrust values for all test cases (must be within [0, 1])
+thrust_values = [0, 0.1, 0.45, 0.9, 1.0]
+test_cases = np.zeros((test_cases_input.shape[0] * len(thrust_values), 4))
+for i in range(test_cases_input.shape[0]):
+ for k in range(len(thrust_values)):
+ test_case = test_cases_input[i]
+ test_case[0, 3] = thrust_values[k]
+ test_cases[i * len(thrust_values) + k, :] = test_case
+
+
+def run_tests(mixer_cb, P, test_mixer_binary, test_index=None):
+ """
+ Run all (or a specific) tests for a certain mixer method an control
+ allocation matrix P
+ """
+ B = np.linalg.pinv(P)
+ proc = subprocess.Popen(
+ test_mixer_binary,
+ #'cat > /tmp/test_'+str(mode_idx), shell=True, # just to test the output
+ stdout=subprocess.PIPE,
+ stdin=subprocess.PIPE)
+ proc.stdin.write("{:}\n".format(mode_idx)) # airmode
+ motor_count = P.shape[0]
+ proc.stdin.write("{:}\n".format(motor_count)) # motor count
+ # control allocation matrix
+ for row in P.getA():
+ for col in row:
+ proc.stdin.write("{:.8f} ".format(col))
+ proc.stdin.write("\n")
+ proc.stdin.write("\n")
+
+ failed = False
+ try:
+ if test_index is None:
+ # go through all test cases
+ test_indices = range(test_cases.shape[0])
+ else:
+ test_indices = [test_index]
+ for i in test_indices:
+ actuator_controls = test_cases[[i], :].T
+ proc.stdin.write("{:.8f} {:.8f} {:.8f} {:.8f}\n"
+ .format(actuator_controls[0, 0], actuator_controls[1, 0],
+ actuator_controls[2, 0], actuator_controls[3, 0]))
+
+ (u, u_new) = mixer_cb(actuator_controls, P, 0.0, 1.0)
+ # Saturate the outputs between 0 and 1
+ u_new_sat = np.maximum(u_new, np.matlib.zeros(u.size).T)
+ u_new_sat = np.minimum(u_new_sat, np.matlib.ones(u.size).T)
+ # write expected outputs
+ for j in range(motor_count):
+ proc.stdin.write("{:.8f} ".format(u_new_sat[j, 0]))
+ proc.stdin.write("\n")
+
+ proc.stdin.close()
+ except IOError as e:
+ failed = True
+ result = proc.stdout.read()
+ proc.wait()
+ if proc.returncode != 0: failed = True
+ if failed:
+ print("Error: test failed")
+ print("B:\n{}".format(B))
+ print("P:\n{}".format(P))
+ print(result)
+ raise Exception('Test failed')
+
+parser = ArgumentParser(description=__doc__)
+parser.add_argument('--test', action='store_true', default=False, help='Run tests')
+parser.add_argument("--mixer-multirotor-binary",
+ help="select test_mixer_multirotor binary file name",
+ default='./test_mixer_multirotor')
+parser.add_argument("--mode", "-m", dest="mode",
+ help="mixer mode: none, rp, rpy", default=None)
+parser.add_argument("-i", dest="index", type=int,
+ help="Select a single test to run (starting at 1)", default=None)
+
+args = parser.parse_args()
+mixer_mode = args.mode
+
+if args.test:
+ mixer_binary = args.mixer_multirotor_binary
+ test_index = args.index
+ if test_index is not None: test_index -= 1
+ for mode_idx, (airmode, mixer_cb) in enumerate([
+ ('none', normal_mode),
+ ('rp', airmode_rp),
+ ('rpy', airmode_rpy)]):
+ if mixer_mode is not None and mixer_mode != airmode:
+ continue
+ print('Testing mode: '+airmode)
+ for P in P_tests:
+ run_tests(mixer_cb, P, mixer_binary, test_index)
+ exit(0)
+
+# --------------------------------------------------
+# Prototyping and corner case testing playground
+# --------------------------------------------------
+
+# Compute the control allocation matrix
+# u = P * m
+P = P1 # normal quad
+#P = P2 # wide quad
+
+# Normalized actuator effectiveness matrix using the pseudo inverse of P
+# m = B * u
+B = np.linalg.pinv(P)
+
+# Desired accelerations (actuator controls, in [-1, 1])
+p_dot_sp = 0.0 # roll acceleration (p is the roll rate)
+q_dot_sp = 0.1 # pitch acceleration
+r_dot_sp = 0.1 # yaw acceleration
+T_sp = 0.0 # vertical thrust
+m_sp = np.matrix([p_dot_sp, q_dot_sp, r_dot_sp, T_sp]).T # Vector of desired "accelerations"
+
+# Airmode type (none/rp/rpy)
+airmode = mixer_mode
+if airmode is None: airmode = "none"
+
+# Actuators output saturations
+u_max = 1.0
+u_min = 0.0
+
+if airmode == "none":
+ (u, u_new) = normal_mode(m_sp, P, u_min, u_max)
+elif airmode == "rp":
+ (u, u_new) = airmode_rp(m_sp, P, u_min, u_max)
+elif airmode == "rpy":
+ (u, u_new) = airmode_rpy(m_sp, P, u_min, u_max)
+else:
+ u = 0.0
+ u_new = 0.0
+
+# Saturate the outputs between 0 and 1
+u_new_sat = np.maximum(u_new, np.matlib.zeros(u.size).T)
+u_new_sat = np.minimum(u_new_sat, np.matlib.ones(u.size).T)
+
+# Display some results
+print("u = \n{}\n".format(u))
+print("u_new = \n{}\n".format(u_new))
+print("u_new_sat = \n{}\n".format(u_new_sat))
+print("Desired accelerations = \n{}\n".format(m_sp))
+# Compute back the allocated accelerations
+m_new = B * u_new_sat
+print("Allocated accelerations = \n{}\n".format(m_new))
diff --git a/src/lib/mixer/test_mixer_multirotor.cpp b/src/lib/mixer/test_mixer_multirotor.cpp
new file mode 100644
index 0000000000..533a1e82d2
--- /dev/null
+++ b/src/lib/mixer/test_mixer_multirotor.cpp
@@ -0,0 +1,168 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * testing binary that runs the multirotor mixer through test cases given
+ * via file or stdin and compares the mixer output against expected values.
+ */
+
+#include "mixer.h"
+#include <cstdio>
+#include <cmath>
+
+static const unsigned output_max = 16;
+static float actuator_controls[output_max] {};
+
+static int mixer_callback(uintptr_t handle,
+ uint8_t control_group,
+ uint8_t control_index,
+ float &control);
+
+static int
+mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control)
+{
+ control = actuator_controls[control_index];
+ return 0;
+}
+
+int main(int argc, char *argv[])
+{
+ FILE *file_in = stdin;
+ FILE *file_out = stdout;
+
+ if (argc > 1) {
+ file_in = fopen(argv[1], "r");
+ }
+
+ unsigned rotor_count = 0;
+ MultirotorMixer::Rotor rotors[output_max];
+ float actuator_outputs[output_max];
+
+ // read airmode
+ int airmode;
+ fscanf(file_in, "%i", &airmode);
+
+ // read the motor count & control allocation matrix
+ fscanf(file_in, "%i", &rotor_count);
+
+ if (rotor_count > output_max) {
+ return -1;
+ }
+
+ for (int i = 0; i < rotor_count; ++i) {
+ fscanf(file_in, "%f %f %f %f", &rotors[i].roll_scale, &rotors[i].pitch_scale,
+ &rotors[i].yaw_scale, &rotors[i].thrust_scale);
+ }
+
+ MultirotorMixer mixer(mixer_callback, 0, rotors, rotor_count);
+ mixer.set_airmode((Mixer::Airmode)airmode);
+
+ int test_counter = 0;
+ int num_failed = 0;
+
+ while (!feof(file_in)) {
+
+ // read actuator controls
+ int count = 0;
+
+ while (count < 4 && fscanf(file_in, "%f", &actuator_controls[count]) == 1) {
+ ++count;
+ }
+
+ if (count < 4) {
+ break;
+ }
+
+ // do the mixing
+ if (mixer.mix(actuator_outputs, output_max) != rotor_count) {
+ return -1;
+ }
+
+ // read expected outputs
+ count = 0;
+ float expected_output[output_max];
+ bool failed = false;
+
+ while (count < rotor_count && fscanf(file_in, "%f", &expected_output[count]) == 1) {
+ if (fabsf(expected_output[count] - actuator_outputs[count]) > 0.00001f) {
+ failed = true;
+ }
+
+ ++count;
+ }
+
+ if (count < rotor_count) {
+ break;
+ }
+
+ if (failed) {
+ printf("test %i failed:\n", test_counter + 1);
+ printf("control input : %.3f %.3f %.3f %.3f\n", actuator_controls[0], actuator_controls[1],
+ actuator_controls[2], actuator_controls[3]);
+ printf("mixer output : ");
+
+ for (int i = 0; i < rotor_count; ++i) {
+ printf("%.3f ", actuator_outputs[i]);
+ }
+
+ printf("\n");
+ printf("expected output: ");
+
+ for (int i = 0; i < rotor_count; ++i) {
+ printf("%.3f ", expected_output[i]);
+ }
+
+ printf("\n");
+ printf("diff : ");
+
+ for (int i = 0; i < rotor_count; ++i) {
+ printf("%.3f ", expected_output[i] - actuator_outputs[i]);
+ }
+
+ printf("\n");
+ ++num_failed;
+ }
+
+ ++test_counter;
+ }
+
+ printf("tested %i cases: %i success, %i failed\n", test_counter,
+ test_counter - num_failed, num_failed);
+
+
+ if (file_in != stdin) {
+ fclose(file_in);
+ }
+
+ return num_failed > 0 ? -1 : 0;
+}
diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp
index b06b2d62d2..609b4a58f5 100644
--- a/src/systemcmds/tests/test_mixer.cpp
+++ b/src/systemcmds/tests/test_mixer.cpp
@@ -62,7 +62,7 @@ static int mixer_callback(uintptr_t handle,
uint8_t control_index,
float &control);
-const unsigned output_max = 8;
+static const unsigned output_max = 8;
static float actuator_controls[output_max];
static bool should_prearm = false;
--
GitLab