From 0f40eb953d1a828ebd4407cd83c0906df754ae0d Mon Sep 17 00:00:00 2001 From: Azarakhsh Keipour Date: Thu, 26 Sep 2019 13:24:10 -0400 Subject: [PATCH] Bug Fix: The absolute value of Z thrust is used in the 6-Dof mixer now - Tilt-Hex flies like a normal hex but with the new 6-dof mixer now (with some jitters) - Some minor parameter changes in the hexa_x_tilt definition to make the simulated flight smoother --- .../init.d-posix/6101_hexa_x_tilt | 3 +- .../mixer/MultirotorMixer/MultirotorMixer.hpp | 9 ++++-- .../MultirotorMixer/MultirotorMixer6dof.cpp | 29 ++++++++++--------- 3 files changed, 24 insertions(+), 17 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d-posix/6101_hexa_x_tilt b/ROMFS/px4fmu_common/init.d-posix/6101_hexa_x_tilt index 324b3b4d2e..28e374786f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/6101_hexa_x_tilt +++ b/ROMFS/px4fmu_common/init.d-posix/6101_hexa_x_tilt @@ -20,8 +20,9 @@ if [ $AUTOCNF = yes ] then param set MC_PITCHRATE_P 0.2 param set MC_ROLLRATE_P 0.2 + param set MC_YAWRATE_P 0.1 param set MPC_Z_VEL_I 0.15 - param set MPC_Z_VEL_P 0.6 + param set MPC_Z_VEL_P 0.3 param set RTL_DESCEND_ALT 5.0 param set RTL_LAND_DELAY 5 diff --git a/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp b/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp index 319e72f797..eb01db373f 100644 --- a/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp +++ b/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp @@ -439,7 +439,8 @@ private: * 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. */ - inline void mix_airmode_rp(float roll, float pitch, float yaw, float x_thrust, float y_thrust, float z_thrust, float *outputs); + inline void mix_airmode_rp(float roll, float pitch, float yaw, float x_thrust, float y_thrust, float z_thrust, + float *outputs); /** * Mix roll, pitch, yaw, thrust and set the outputs vector. @@ -448,7 +449,8 @@ private: * thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw, * while giving priority to roll and pitch over yaw. */ - inline void mix_airmode_rpy(float roll, float pitch, float yaw, float x_thrust, float y_thrust, float z_thrust, float *outputs); + inline void mix_airmode_rpy(float roll, float pitch, float yaw, float x_thrust, float y_thrust, float z_thrust, + float *outputs); /** * Mix roll, pitch, yaw, thrust and set the outputs vector. @@ -458,7 +460,8 @@ private: * Thrust can be reduced to unsaturate the upper side. * @see mix_yaw() for the exact yaw behavior. */ - inline void mix_airmode_disabled(float roll, float pitch, float yaw, float x_thrust, float y_thrust, float z_thrust, float *outputs); + inline void mix_airmode_disabled(float roll, float pitch, float yaw, float x_thrust, float y_thrust, float z_thrust, + float *outputs); /** * Mix yaw by updating an existing output vector (that already contains roll/pitch/thrust). diff --git a/src/lib/mixer/MultirotorMixer/MultirotorMixer6dof.cpp b/src/lib/mixer/MultirotorMixer/MultirotorMixer6dof.cpp index b43a438bd7..8df8851564 100644 --- a/src/lib/mixer/MultirotorMixer/MultirotorMixer6dof.cpp +++ b/src/lib/mixer/MultirotorMixer/MultirotorMixer6dof.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2019 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 @@ -213,7 +213,8 @@ MultirotorMixer6dof::minimize_saturation(const float *desaturation_vector, float } void -MultirotorMixer6dof::mix_airmode_rp(float roll, float pitch, float yaw, float x_thrust, float y_thrust, float z_thrust, float *outputs) +MultirotorMixer6dof::mix_airmode_rp(float roll, float pitch, float yaw, float x_thrust, float y_thrust, float z_thrust, + float *outputs) { // Airmode for roll and pitch, but not yaw @@ -226,7 +227,7 @@ MultirotorMixer6dof::mix_airmode_rp(float roll, float pitch, float yaw, float x_ z_thrust * _rotors[i].z_scale; // Thrust will be used to unsaturate if needed - _tmp_array[i] = _rotors[i].z_scale; + _tmp_array[i] = math::abs_t(_rotors[i].z_scale); } minimize_saturation(_tmp_array, outputs, _saturation_status); @@ -236,7 +237,8 @@ MultirotorMixer6dof::mix_airmode_rp(float roll, float pitch, float yaw, float x_ } void -MultirotorMixer6dof::mix_airmode_rpy(float roll, float pitch, float yaw, float x_thrust, float y_thrust, float z_thrust, float *outputs) +MultirotorMixer6dof::mix_airmode_rpy(float roll, float pitch, float yaw, float x_thrust, float y_thrust, float z_thrust, + float *outputs) { // Airmode for roll, pitch and yaw @@ -250,7 +252,7 @@ MultirotorMixer6dof::mix_airmode_rpy(float roll, float pitch, float yaw, float x z_thrust * _rotors[i].z_scale; // Z thrust will be used to unsaturate if needed - _tmp_array[i] = _rotors[i].z_scale; + _tmp_array[i] = math::abs_t(_rotors[i].z_scale); } minimize_saturation(_tmp_array, outputs, _saturation_status); @@ -265,20 +267,21 @@ MultirotorMixer6dof::mix_airmode_rpy(float roll, float pitch, float yaw, float x } void -MultirotorMixer6dof::mix_airmode_disabled(float roll, float pitch, float yaw, float x_thrust, float y_thrust, float z_thrust, float *outputs) +MultirotorMixer6dof::mix_airmode_disabled(float roll, float pitch, float yaw, float x_thrust, float y_thrust, + float z_thrust, float *outputs) { // Airmode disabled: never allow to increase the thrust to unsaturate a motor // Mix without yaw for (unsigned i = 0; i < _rotor_count; i++) { outputs[i] = roll * _rotors[i].roll_scale + - pitch * _rotors[i].pitch_scale + - x_thrust * _rotors[i].x_scale + - y_thrust * _rotors[i].y_scale + - z_thrust * _rotors[i].z_scale; + pitch * _rotors[i].pitch_scale + + x_thrust * _rotors[i].x_scale + + y_thrust * _rotors[i].y_scale + + z_thrust * _rotors[i].z_scale; // Z thrust will be used to unsaturate if needed - _tmp_array[i] = _rotors[i].z_scale; + _tmp_array[i] = math::abs_t(_rotors[i].z_scale); } // only reduce thrust @@ -316,7 +319,7 @@ void MultirotorMixer6dof::mix_yaw(float yaw, float *outputs) minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.15f); for (unsigned i = 0; i < _rotor_count; i++) { - _tmp_array[i] = _rotors[i].z_scale; + _tmp_array[i] = math::abs_t(_rotors[i].z_scale); } // reduce thrust only @@ -335,7 +338,7 @@ MultirotorMixer6dof::mix(float *outputs, unsigned space) float yaw = math::constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f); float x_thrust = math::constrain(get_control(0, 4), 0.0f, 1.0f); float y_thrust = math::constrain(get_control(0, 5), 0.0f, 1.0f); - // TODO remove the - sign + // TODO remove the - sign float z_thrust = - math::constrain(get_control(0, 3), 0.0f, 1.0f); // clean out class variable used to capture saturation