From a26e914ef4d347e0bf6f54bd40ae17cabf087fbb Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 30 Jan 2020 17:09:55 +0100 Subject: [PATCH] VTOL pusher/tilt support: addressed review comments: - remove gloabl variable and instead give back forward_thrust from function - give back increased vertical thrust from fct thrust_compensation_for_tilt() Signed-off-by: Silvan Fuhrer --- src/modules/vtol_att_control/standard.cpp | 4 +--- src/modules/vtol_att_control/tiltrotor.cpp | 11 ++++------- src/modules/vtol_att_control/tiltrotor.h | 2 +- src/modules/vtol_att_control/vtol_type.cpp | 23 +++++++++++----------- src/modules/vtol_att_control/vtol_type.h | 4 +--- 5 files changed, 19 insertions(+), 25 deletions(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 1c2bbeab22..d5c7200dbe 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -317,9 +317,7 @@ void Standard::update_mc_state() { VtolType::update_mc_state(); - VtolType::pusher_assist(); - - _pusher_throttle = _forward_thrust; + _pusher_throttle = VtolType::pusher_assist(); } void Standard::update_fw_state() diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index ba4ead672d..3e0b069f2c 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -207,11 +207,9 @@ void Tiltrotor::update_mc_state() { VtolType::update_mc_state(); - VtolType::pusher_assist(); + _tilt_control = VtolType::pusher_assist(); - _tilt_control = _forward_thrust; - - Tiltrotor::thrust_compensation_for_tilt(); + _v_att_sp->thrust_body[2] = Tiltrotor::thrust_compensation_for_tilt(); } void Tiltrotor::update_fw_state() @@ -395,7 +393,7 @@ void Tiltrotor::fill_actuator_outputs() * Increase combined thrust of MC propellers if motors are tilted. Assumes that all MC motors are tilted equally. */ -void Tiltrotor::thrust_compensation_for_tilt() +float Tiltrotor::thrust_compensation_for_tilt() { // only compensate for tilt angle up to 0.5 * max tilt @@ -403,7 +401,6 @@ void Tiltrotor::thrust_compensation_for_tilt() compensated_tilt = compensated_tilt < 0.0f ? 0.0f : compensated_tilt; compensated_tilt = compensated_tilt > 0.5f ? 0.5f : compensated_tilt; - float thrust_new = _v_att_sp->thrust_body[2] / cosf(compensated_tilt * M_PI_2_F); + return _v_att_sp->thrust_body[2] / cosf(compensated_tilt * M_PI_2_F); - _v_att_sp->thrust_body[2] = thrust_new; } diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index 1dc38dd1bf..e102edb35d 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -58,7 +58,7 @@ public: void update_mc_state() override; void update_fw_state() override; void waiting_on_tecs() override; - void thrust_compensation_for_tilt(); + float thrust_compensation_for_tilt(); private: diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 46e50859da..62211d1648 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -385,28 +385,24 @@ bool VtolType::is_channel_set(const int channel, const int target) } -void VtolType::pusher_assist() +float VtolType::pusher_assist() { - - // normalized pusher support throttle (standard VTOL) or tilt (tiltrotor), initialize to 0 - _forward_thrust = 0.0f; - // if the thrust scale param is zero or the drone is not in some position or altitude control mode, // then the pusher-for-pitch strategy is disabled and we can return if (_params->forward_thrust_scale < FLT_EPSILON || !(_v_control_mode->flag_control_position_enabled || _v_control_mode->flag_control_altitude_enabled)) { - return; + return 0.0f; } // Do not engage pusher assist during a failsafe event (could be a problem with the fixed wing drive) if (_attc->get_vtol_vehicle_status()->vtol_transition_failsafe) { - return; + return 0.0f; } // disable pusher assist during landing if (_attc->get_pos_sp_triplet()->current.valid && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { - return; + return 0.0f; } const Dcmf R(Quatf(_v_att->q)); @@ -427,16 +423,19 @@ void VtolType::pusher_assist() // this value corresponds to the amount the vehicle would try to pitch down float pitch_down = atan2f(body_z_sp(0), body_z_sp(2)); + // normalized pusher support throttle (standard VTOL) or tilt (tiltrotor), initialize to 0 + float forward_thrust = 0.0f; + // only allow pitching down up to threshold, the rest of the desired // forward acceleration will be compensated by the pusher/tilt if (pitch_down < -_params->down_pitch_max) { // desired roll angle in heading frame stays the same float roll_new = -asinf(body_z_sp(1)); - _forward_thrust = (sinf(-pitch_down) - sinf(_params->down_pitch_max)) * _params->forward_thrust_scale; + forward_thrust = (sinf(-pitch_down) - sinf(_params->down_pitch_max)) * _params->forward_thrust_scale; // limit forward actuation to [0, 0.9] - _forward_thrust = _forward_thrust < 0.0f ? 0.0f : _forward_thrust; - _forward_thrust = _forward_thrust > 0.9f ? 0.9f : _forward_thrust; + forward_thrust = forward_thrust < 0.0f ? 0.0f : forward_thrust; + forward_thrust = forward_thrust > 0.9f ? 0.9f : forward_thrust; // return the vehicle to level position float pitch_new = 0.0f; @@ -460,4 +459,6 @@ void VtolType::pusher_assist() _v_att_sp->q_d_valid = true; } + return forward_thrust; + } diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 069cd2a7a2..047bd2b84d 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -168,7 +168,7 @@ public: /** * Pusher assist in hover (pusher/pull for standard VTOL, motor tilt for tiltrotor) */ - void pusher_assist(); + float pusher_assist(); mode get_mode() {return _vtol_mode;} @@ -222,8 +222,6 @@ protected: motor_state _motor_state = motor_state::DISABLED; - float _forward_thrust = 0.0f; // normalized pusher throttle (standard VTOL) or tilt (tiltrotor) - /**