From 400f8ea4be4d954cfd158dd138899586b770deef Mon Sep 17 00:00:00 2001 From: Azarakhsh Keipour Date: Mon, 30 Sep 2019 18:25:11 -0400 Subject: [PATCH] Bug Fix(es): 3D-thrust ranges fixed to -1..1 and manual z maps to negative z-thrust now - The range for 3D-thrust in the 6-DOF multirotor mixer is changed to -1 to 1 now (fixed from 0 to 1) - The Z thrust in the 6-DOF multirotor mixer is mapped to the Z-thrust command now (fixed from thrust command) - The manual Z command in the rate controller maps to the negative Z-thrust (fixed from positive Z) - The variable _thrust_body_sp in the rate controller renamed to _thrust_sp to be compatible with the older variable removed in the last commit - The code tested in TakeOff, Manual, Hold, Position and Land modes on both tilted hex and iris Accepting and working with 3D thrust commands now --- .../MultirotorMixer/MultirotorMixer6dof.cpp | 6 ++--- .../MulticopterRateControl.cpp | 26 +++++++++---------- .../MulticopterRateControl.hpp | 2 +- 3 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/lib/mixer/MultirotorMixer/MultirotorMixer6dof.cpp b/src/lib/mixer/MultirotorMixer/MultirotorMixer6dof.cpp index c6548a299c..4c4491f3d3 100644 --- a/src/lib/mixer/MultirotorMixer/MultirotorMixer6dof.cpp +++ b/src/lib/mixer/MultirotorMixer/MultirotorMixer6dof.cpp @@ -336,9 +336,9 @@ MultirotorMixer6dof::mix(float *outputs, unsigned space) float roll = math::constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f); float pitch = math::constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f); float yaw = math::constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f); - float x_thrust = math::constrain(get_control(0, 8), 0.0f, 1.0f); - float y_thrust = math::constrain(get_control(0, 9), 0.0f, 1.0f); - float z_thrust = - math::constrain(get_control(0, 3), 0.0f, 1.0f); + float x_thrust = math::constrain(get_control(0, 8), -1.0f, 1.0f); + float y_thrust = math::constrain(get_control(0, 9), -1.0f, 1.0f); + float z_thrust = math::constrain(get_control(0, 10), -1.0f, 1.0f); // clean out class variable used to capture saturation _saturation_status.value = 0; diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index 955a50a4a5..80291e46b2 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp @@ -213,18 +213,18 @@ MulticopterRateControl::Run() math::superexpo(_manual_control_sp.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())}; _rates_sp = man_rate_sp.emult(_acro_rate_max); - _thrust_body_sp(0) = _manual_control_sp.x; - _thrust_body_sp(1) = _manual_control_sp.y; - _thrust_body_sp(2) = _manual_control_sp.z; + _thrust_sp(0) = _manual_control_sp.x; + _thrust_sp(1) = _manual_control_sp.y; + _thrust_sp(2) = -_manual_control_sp.z; // publish rate setpoint vehicle_rates_setpoint_s v_rates_sp{}; v_rates_sp.roll = _rates_sp(0); v_rates_sp.pitch = _rates_sp(1); v_rates_sp.yaw = _rates_sp(2); - v_rates_sp.thrust_body[0] = _thrust_body_sp(0); - v_rates_sp.thrust_body[1] = _thrust_body_sp(1); - v_rates_sp.thrust_body[2] = _thrust_body_sp(2); + v_rates_sp.thrust_body[0] = _thrust_sp(0); + v_rates_sp.thrust_body[1] = _thrust_sp(1); + v_rates_sp.thrust_body[2] = _thrust_sp(2); v_rates_sp.timestamp = hrt_absolute_time(); _v_rates_sp_pub.publish(v_rates_sp); @@ -238,9 +238,9 @@ MulticopterRateControl::Run() _rates_sp(0) = v_rates_sp.roll; _rates_sp(1) = v_rates_sp.pitch; _rates_sp(2) = v_rates_sp.yaw; - _thrust_body_sp(0) = v_rates_sp.thrust_body[0]; - _thrust_body_sp(1) = v_rates_sp.thrust_body[1]; - _thrust_body_sp(2) = v_rates_sp.thrust_body[2]; + _thrust_sp(0) = v_rates_sp.thrust_body[0]; + _thrust_sp(1) = v_rates_sp.thrust_body[1]; + _thrust_sp(2) = v_rates_sp.thrust_body[2]; } } @@ -292,11 +292,11 @@ MulticopterRateControl::Run() actuators.control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.0f; actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.0f; actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f; - actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_body_sp(2)) ? -_thrust_body_sp(2) : 0.0f; + actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_sp(2)) ? -_thrust_sp(2) : 0.0f; actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = (float)_landing_gear.landing_gear; - actuators.control[actuator_controls_s::INDEX_X_THRUST] = (PX4_ISFINITE(_thrust_body_sp(0))) ? _thrust_body_sp(0) : 0.0f; - actuators.control[actuator_controls_s::INDEX_Y_THRUST] = (PX4_ISFINITE(_thrust_body_sp(1))) ? _thrust_body_sp(1) : 0.0f; - actuators.control[actuator_controls_s::INDEX_Z_THRUST] = (PX4_ISFINITE(_thrust_body_sp(2))) ? _thrust_body_sp(2) : 0.0f; + actuators.control[actuator_controls_s::INDEX_X_THRUST] = (PX4_ISFINITE(_thrust_sp(0))) ? _thrust_sp(0) : 0.0f; + actuators.control[actuator_controls_s::INDEX_Y_THRUST] = (PX4_ISFINITE(_thrust_sp(1))) ? _thrust_sp(1) : 0.0f; + actuators.control[actuator_controls_s::INDEX_Z_THRUST] = (PX4_ISFINITE(_thrust_sp(2))) ? _thrust_sp(2) : 0.0f; actuators.timestamp_sample = angular_velocity.timestamp_sample; // scale effort by battery status if enabled diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index ee095fb562..f9eb9b47cc 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -129,7 +129,7 @@ private: matrix::Vector3f _rates_sp; /**< angular rates setpoint */ - matrix::Vector3f _thrust_body_sp; /**< 3-D thrust setpoint */ + matrix::Vector3f _thrust_sp; /**< 3-D thrust setpoint */ bool _gear_state_initialized{false}; /**< true if the gear state has been initialized */