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 <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2020-01-30 17:09:55 +01:00 committed by Beat Küng
parent 5ae4ca8ff1
commit a26e914ef4
5 changed files with 19 additions and 25 deletions

View File

@ -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()

View File

@ -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;
}

View File

@ -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:

View File

@ -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;
}

View File

@ -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)
/**