forked from Archive/PX4-Autopilot
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:
parent
5ae4ca8ff1
commit
a26e914ef4
|
@ -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()
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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:
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
||||
}
|
||||
|
|
|
@ -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)
|
||||
|
||||
|
||||
|
||||
/**
|
||||
|
|
Loading…
Reference in New Issue