From a878d6823d435f4b00607b8712410965bda8a4a6 Mon Sep 17 00:00:00 2001 From: Azarakhsh Keipour Date: Mon, 26 Aug 2019 18:00:54 -0400 Subject: [PATCH] Mixer: Added 3D thrust capability to the multirotor and 6-dof mixers - Added 3D thrust definitions to actuator controls and multirotor motor limits messages --- msg/actuator_controls.msg | 5 +++- msg/multirotor_motor_limits.msg | 26 +++++++++++-------- .../mixer/MultirotorMixer/MultirotorMixer.cpp | 15 ++++++++--- .../mixer/MultirotorMixer/MultirotorMixer.hpp | 20 ++++++++------ .../MultirotorMixer/MultirotorMixer6dof.cpp | 24 ++++++++--------- 5 files changed, 54 insertions(+), 36 deletions(-) diff --git a/msg/actuator_controls.msg b/msg/actuator_controls.msg index d266f25bea..edda89dca3 100644 --- a/msg/actuator_controls.msg +++ b/msg/actuator_controls.msg @@ -9,10 +9,13 @@ uint8 INDEX_FLAPS = 4 uint8 INDEX_SPOILERS = 5 uint8 INDEX_AIRBRAKES = 6 uint8 INDEX_LANDING_GEAR = 7 +uint8 INDEX_X_THRUST = 8 +uint8 INDEX_Y_THRUST = 9 +uint8 INDEX_Z_THRUST = 10 uint8 GROUP_INDEX_ATTITUDE = 0 uint8 GROUP_INDEX_ATTITUDE_ALTERNATE = 1 uint64 timestamp_sample # the timestamp the data this control response is based on was sampled -float32[8] control +float32[11] control # TOPICS actuator_controls actuator_controls_0 actuator_controls_1 actuator_controls_2 actuator_controls_3 # TOPICS actuator_controls_virtual_fw actuator_controls_virtual_mc diff --git a/msg/multirotor_motor_limits.msg b/msg/multirotor_motor_limits.msg index 2cd692fd36..36520e90e8 100644 --- a/msg/multirotor_motor_limits.msg +++ b/msg/multirotor_motor_limits.msg @@ -1,14 +1,18 @@ uint64 timestamp # time since system start (microseconds) uint16 saturation_status # Integer bit mask indicating which axes in the control mixer are saturated -# 0 - True if the saturation status is valid -# 1 - True if any motor is saturated at the upper limit -# 2 - True if any motor is saturated at the lower limit -# 3 - True if a positive roll increment will increase motor saturation -# 4 - True if negative roll increment will increase motor saturation -# 5 - True if positive pitch increment will increase motor saturation -# 6 - True if negative pitch increment will increase motor saturation -# 7 - True if positive yaw increment will increase motor saturation -# 8 - True if negative yaw increment will increase motor saturation -# 9 - True if positive thrust increment will increase motor saturation -# 10 - True if negative thrust increment will increase motor saturation +uint8 INDEX_VALID = 0 # True if the saturation status is valid +uint8 INDEX_MOTOR_POS = 1 # True if any motor is saturated at the upper limit +uint8 INDEX_MOTOR_NEG = 2 # True if any motor is saturated at the lower limit +uint8 INDEX_ROLL_POS = 3 # True if a positive roll increment will increase motor saturation +uint8 INDEX_ROLL_NEG = 4 # True if negative roll increment will increase motor saturation +uint8 INDEX_PITCH_POS = 5 # True if positive pitch increment will increase motor saturation +uint8 INDEX_PITCH_NEG = 6 # True if negative pitch increment will increase motor saturation +uint8 INDEX_YAW_POS = 7 # True if positive yaw increment will increase motor saturation +uint8 INDEX_YAW_NEG = 8 # True if negative yaw increment will increase motor saturation +uint8 INDEX_X_THRUST_POS = 9 # True if positive X thrust increment will increase motor saturation +uint8 INDEX_X_THRUST_NEG = 10 # True if negative X thrust increment will increase motor saturation +uint8 INDEX_Y_THRUST_POS = 11 # True if positive Y thrust increment will increase motor saturation +uint8 INDEX_Y_THRUST_NEG = 12 # True if negative Y thrust increment will increase motor saturation +uint8 INDEX_Z_THRUST_POS = 13 # True if positive Z thrust increment will increase motor saturation +uint8 INDEX_Z_THRUST_NEG = 14 # True if negative Z thrust increment will increase motor saturation diff --git a/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp b/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp index 47b41a58b0..17e2a839a3 100644 --- a/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp +++ b/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp @@ -470,8 +470,9 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo _saturation_status.flags.yaw_neg = true; } - // A positive change in thrust will increase saturation - _saturation_status.flags.thrust_pos = true; + // A negative change in Z thrust will increase saturation + _saturation_status.flags.z_thrust_neg = true; + } // The motor is saturated at the lower limit @@ -497,8 +498,8 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo _saturation_status.flags.pitch_pos = true; } - // A negative change in thrust will increase saturation - _saturation_status.flags.thrust_neg = true; + // A positive change in Z thrust will increase saturation + _saturation_status.flags.z_thrust_pos = true; } if (clipping_low_yaw) { @@ -513,5 +514,11 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo } } + // X and Y thrusts are not controlled + _saturation_status.flags.x_thrust_pos = true; + _saturation_status.flags.x_thrust_neg = true; + _saturation_status.flags.y_thrust_pos = true; + _saturation_status.flags.y_thrust_neg = true; + _saturation_status.flags.valid = true; } diff --git a/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp b/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp index 42f38fc04f..319e72f797 100644 --- a/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp +++ b/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp @@ -161,8 +161,12 @@ public: uint16_t pitch_neg : 1; // 6 - true when a negative pitch demand change will increase saturation uint16_t yaw_pos : 1; // 7 - true when a positive yaw demand change will increase saturation uint16_t yaw_neg : 1; // 8 - true when a negative yaw demand change will increase saturation - uint16_t thrust_pos : 1; // 9 - true when a positive thrust demand change will increase saturation - uint16_t thrust_neg : 1; //10 - true when a negative thrust demand change will increase saturation + uint16_t x_thrust_pos : 1; // 9 - true when a positive x thrust demand change will increase saturation + uint16_t x_thrust_neg : 1; //10 - true when a negative x thrust demand change will increase saturation + uint16_t y_thrust_pos : 1; //11 - true when a positive y thrust demand change will increase saturation + uint16_t y_thrust_neg : 1; //12 - true when a negative y thrust demand change will increase saturation + uint16_t z_thrust_pos : 1; //13 - true when a positive z thrust demand change will increase saturation + uint16_t z_thrust_neg : 1; //14 - true when a negative z thrust demand change will increase saturation } flags; uint16_t value; }; @@ -386,12 +390,12 @@ public: uint16_t pitch_neg : 1; // 6 - true when a negative pitch demand change will increase saturation uint16_t yaw_pos : 1; // 7 - true when a positive yaw demand change will increase saturation uint16_t yaw_neg : 1; // 8 - true when a negative yaw demand change will increase saturation - uint16_t x_pos : 1; // 9 - true when a positive x thrust demand change will increase saturation - uint16_t x_neg : 1; //10 - true when a negative x thrust demand change will increase saturation - uint16_t y_pos : 1; //11 - true when a positive y thrust demand change will increase saturation - uint16_t y_neg : 1; //12 - true when a negative y thrust demand change will increase saturation - uint16_t z_pos : 1; //13 - true when a positive z thrust demand change will increase saturation - uint16_t z_neg : 1; //14 - true when a negative z thrust demand change will increase saturation + uint16_t x_thrust_pos : 1; // 9 - true when a positive x thrust demand change will increase saturation + uint16_t x_thrust_neg : 1; //10 - true when a negative x thrust demand change will increase saturation + uint16_t y_thrust_pos : 1; //11 - true when a positive y thrust demand change will increase saturation + uint16_t y_thrust_neg : 1; //12 - true when a negative y thrust demand change will increase saturation + uint16_t z_thrust_pos : 1; //13 - true when a positive z thrust demand change will increase saturation + uint16_t z_thrust_neg : 1; //14 - true when a negative z thrust demand change will increase saturation } flags; uint16_t value; }; diff --git a/src/lib/mixer/MultirotorMixer/MultirotorMixer6dof.cpp b/src/lib/mixer/MultirotorMixer/MultirotorMixer6dof.cpp index a4db2a1d20..b43a438bd7 100644 --- a/src/lib/mixer/MultirotorMixer/MultirotorMixer6dof.cpp +++ b/src/lib/mixer/MultirotorMixer/MultirotorMixer6dof.cpp @@ -467,31 +467,31 @@ MultirotorMixer6dof::update_saturation_status(unsigned index, bool clipping_high // check if the x input is saturating if (_rotors[index].x_scale > 0.0f) { // A positive change in x will increase saturation - _saturation_status.flags.x_pos = true; + _saturation_status.flags.x_thrust_pos = true; } else if (_rotors[index].x_scale < 0.0f) { // A negative change in x will increase saturation - _saturation_status.flags.x_neg = true; + _saturation_status.flags.x_thrust_neg = true; } // check if the y input is saturating if (_rotors[index].y_scale > 0.0f) { // A positive change in y will increase saturation - _saturation_status.flags.y_pos = true; + _saturation_status.flags.y_thrust_pos = true; } else if (_rotors[index].y_scale < 0.0f) { // A negative change in y will increase saturation - _saturation_status.flags.y_neg = true; + _saturation_status.flags.y_thrust_neg = true; } // check if the z input is saturating if (_rotors[index].z_scale > 0.0f) { // A positive change in z will increase saturation - _saturation_status.flags.z_pos = true; + _saturation_status.flags.z_thrust_pos = true; } else if (_rotors[index].z_scale < 0.0f) { // A negative change in z will increase saturation - _saturation_status.flags.z_neg = true; + _saturation_status.flags.z_thrust_neg = true; } } @@ -521,31 +521,31 @@ MultirotorMixer6dof::update_saturation_status(unsigned index, bool clipping_high // check if the x input is saturating if (_rotors[index].x_scale > 0.0f) { // A negative change in x will increase saturation - _saturation_status.flags.x_neg = true; + _saturation_status.flags.x_thrust_neg = true; } else if (_rotors[index].x_scale < 0.0f) { // A positive change in x will increase saturation - _saturation_status.flags.x_pos = true; + _saturation_status.flags.x_thrust_pos = true; } // check if the y input is saturating if (_rotors[index].y_scale > 0.0f) { // A negative change in y will increase saturation - _saturation_status.flags.y_neg = true; + _saturation_status.flags.y_thrust_neg = true; } else if (_rotors[index].y_scale < 0.0f) { // A positive change in y will increase saturation - _saturation_status.flags.y_pos = true; + _saturation_status.flags.y_thrust_pos = true; } // check if the z input is saturating if (_rotors[index].z_scale > 0.0f) { // A negative change in z will increase saturation - _saturation_status.flags.z_neg = true; + _saturation_status.flags.z_thrust_neg = true; } else if (_rotors[index].z_scale < 0.0f) { // A positive change in z will increase saturation - _saturation_status.flags.z_pos = true; + _saturation_status.flags.z_thrust_pos = true; } }