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 +#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 @@ -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 , Beat Kueng +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 +#include + +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;