dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

VTOL: Make pusher assist generic to enable also for tiltrotor 

-move pusher assist functionality into vtol_type class and adapt it to also
work for tiltrotor VTOLs (pitch rotors down to accelerate forward)
-for tiltrotor: compensate for lost lift due to tilt by increasing the throttle
-enable pusher / tiltassist also in altitude mode

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2020-01-29 13:36:36 +01:00 committed by Beat Küng
parent ca0b5700ab
commit 5ae4ca8ff1
8 changed files with 126 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
src/modules/vtol_att_control/standard.cpp
View File

@ -62,8 +62,6 @@ Standard::Standard(VtolAttitudeControl *attc) :
_params_handles_standard.pusher_ramp_dt = param_find("VT_PSHER_RMP_DT");
_params_handles_standard.back_trans_ramp = param_find("VT_B_TRANS_RAMP");
_params_handles_standard.down_pitch_max = param_find("VT_DWN_PITCH_MAX");
_params_handles_standard.forward_thrust_scale = param_find("VT_FWD_THRUST_SC");
_params_handles_standard.pitch_setpoint_offset = param_find("FW_PSP_OFF");
_params_handles_standard.reverse_output = param_find("VT_B_REV_OUT");
_params_handles_standard.reverse_delay = param_find("VT_B_REV_DEL");
@ -84,13 +82,6 @@ Standard::parameters_update()
_airspeed_trans_blend_margin = _params->transition_airspeed - _params->airspeed_blend;
/* maximum down pitch allowed */
param_get(_params_handles_standard.down_pitch_max, &v);
_params_standard.down_pitch_max = math::radians(v);
/* scale for fixed wing thrust used for forward acceleration in multirotor mode */
param_get(_params_handles_standard.forward_thrust_scale, &_params_standard.forward_thrust_scale);
/* pitch setpoint offset */
param_get(_params_handles_standard.pitch_setpoint_offset, &v);
_params_standard.pitch_setpoint_offset = math::radians(v);
@ -326,76 +317,9 @@ void Standard::update_mc_state()
{
VtolType::update_mc_state();
// if the thrust scale param is zero or the drone is on manual mode,
// then the pusher-for-pitch strategy is disabled and we can return
if (_params_standard.forward_thrust_scale < FLT_EPSILON ||
!_v_control_mode->flag_control_position_enabled) {
return;
}
VtolType::pusher_assist();
// Do not engage pusher assist during a failsafe event
// There could be a problem with the fixed wing drive
if (_attc->get_vtol_vehicle_status()->vtol_transition_failsafe) {
return;
}
// 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;
}
const Dcmf R(Quatf(_v_att->q));
const Dcmf R_sp(Quatf(_v_att_sp->q_d));
const Eulerf euler(R);
const Eulerf euler_sp(R_sp);
_pusher_throttle = 0.0f;
// direction of desired body z axis represented in earth frame
Vector3f body_z_sp(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
// rotate desired body z axis into new frame which is rotated in z by the current
// heading of the vehicle. we refer to this as the heading frame.
Dcmf R_yaw = Eulerf(0.0f, 0.0f, -euler(2));
body_z_sp = R_yaw * body_z_sp;
body_z_sp.normalize();
// calculate the desired pitch seen in the heading frame
// this value corresponds to the amount the vehicle would try to pitch forward
float pitch_forward = atan2f(body_z_sp(0), body_z_sp(2));
// only allow pitching forward up to threshold, the rest of the desired
// forward acceleration will be compensated by the pusher
if (pitch_forward < -_params_standard.down_pitch_max) {
// desired roll angle in heading frame stays the same
float roll_new = -asinf(body_z_sp(1));
_pusher_throttle = (sinf(-pitch_forward) - sinf(_params_standard.down_pitch_max))
* _params_standard.forward_thrust_scale;
// return the vehicle to level position
float pitch_new = 0.0f;
// create corrected desired body z axis in heading frame
const Dcmf R_tmp = Eulerf(roll_new, pitch_new, 0.0f);
Vector3f tilt_new(R_tmp(0, 2), R_tmp(1, 2), R_tmp(2, 2));
// rotate the vector into a new frame which is rotated in z by the desired heading
// with respect to the earh frame.
const float yaw_error = wrap_pi(euler_sp(2) - euler(2));
const Dcmf R_yaw_correction = Eulerf(0.0f, 0.0f, -yaw_error);
tilt_new = R_yaw_correction * tilt_new;
// now extract roll and pitch setpoints
_v_att_sp->pitch_body = atan2f(tilt_new(0), tilt_new(2));
_v_att_sp->roll_body = -asinf(tilt_new(1));
const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2)));
q_sp.copyTo(_v_att_sp->q_d);
_v_att_sp->q_d_valid = true;
}
_pusher_throttle = _pusher_throttle < 0.0f ? 0.0f : _pusher_throttle;
_pusher_throttle = _forward_thrust;
}
void Standard::update_fw_state()

4
src/modules/vtol_att_control/standard.h
View File

@ -69,8 +69,6 @@ private:
struct {
float pusher_ramp_dt;
float back_trans_ramp;
float down_pitch_max;
float forward_thrust_scale;
float pitch_setpoint_offset;
float reverse_output;
float reverse_delay;
@ -79,8 +77,6 @@ private:
struct {
param_t pusher_ramp_dt;
param_t back_trans_ramp;
param_t down_pitch_max;
param_t forward_thrust_scale;
param_t pitch_setpoint_offset;
param_t reverse_output;
param_t reverse_delay;

24
src/modules/vtol_att_control/tiltrotor.cpp
View File

@ -207,8 +207,11 @@ void Tiltrotor::update_mc_state()
{
VtolType::update_mc_state();
// make sure motors are not tilted
_tilt_control = _params_tiltrotor.tilt_mc;
VtolType::pusher_assist();
_tilt_control = _forward_thrust;
Tiltrotor::thrust_compensation_for_tilt();
}
void Tiltrotor::update_fw_state()
@ -387,3 +390,20 @@ void Tiltrotor::fill_actuator_outputs()
_actuators_fw_in->control[actuator_controls_s::INDEX_YAW];
}
}
/*
* Increase combined thrust of MC propellers if motors are tilted. Assumes that all MC motors are tilted equally.
*/
void Tiltrotor::thrust_compensation_for_tilt()
{
// only compensate for tilt angle up to 0.5 * max tilt
float compensated_tilt = _tilt_control;
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);
_v_att_sp->thrust_body[2] = thrust_new;
}

1
src/modules/vtol_att_control/tiltrotor.h
View File

@ -58,6 +58,7 @@ public:
void update_mc_state() override;
void update_fw_state() override;
void waiting_on_tecs() override;
void thrust_compensation_for_tilt();
private:

10
src/modules/vtol_att_control/vtol_att_control_main.cpp
View File

@ -86,6 +86,9 @@ VtolAttitudeControl::VtolAttitudeControl() :
_params_handles.diff_thrust = param_find("VT_FW_DIFTHR_EN");
_params_handles.diff_thrust_scale = param_find("VT_FW_DIFTHR_SC");
_params_handles.down_pitch_max = param_find("VT_DWN_PITCH_MAX");
_params_handles.forward_thrust_scale = param_find("VT_FWD_THRUST_SC");
/* fetch initial parameter values */
parameters_update();
@ -272,6 +275,13 @@ VtolAttitudeControl::parameters_update()
param_get(_params_handles.diff_thrust_scale, &v);
_params.diff_thrust_scale = math::constrain(v, -1.0f, 1.0f);
/* maximum down pitch allowed */
param_get(_params_handles.down_pitch_max, &v);
_params.down_pitch_max = math::radians(v);
/* scale for fixed wing thrust used for forward acceleration in multirotor mode */
param_get(_params_handles.forward_thrust_scale, &_params.forward_thrust_scale);
// make sure parameters are feasible, require at least 1 m/s difference between transition and blend airspeed
_params.airspeed_blend = math::min(_params.airspeed_blend, _params.transition_airspeed - 1.0f);

2
src/modules/vtol_att_control/vtol_att_control_main.h
View File

@ -201,6 +201,8 @@ private:
param_t fw_motors_off;
param_t diff_thrust;
param_t diff_thrust_scale;
param_t down_pitch_max;
param_t forward_thrust_scale;
} _params_handles{};
/* for multicopters it is usual to have a non-zero idle speed of the engines

82
src/modules/vtol_att_control/vtol_type.cpp
View File

@ -46,6 +46,8 @@
#include <px4_platform_common/defines.h>
#include <matrix/math.hpp>
using namespace matrix;
VtolType::VtolType(VtolAttitudeControl *att_controller) :
_attc(att_controller),
@ -187,7 +189,7 @@ void VtolType::check_quadchute_condition()
{
if (_v_control_mode->flag_armed && !_land_detected->landed) {
matrix::Eulerf euler = matrix::Quatf(_v_att->q);
Eulerf euler = Quatf(_v_att->q);
// fixed-wing minimum altitude
if (_params->fw_min_alt > FLT_EPSILON) {
@ -381,3 +383,81 @@ bool VtolType::is_channel_set(const int channel, const int target)
return (channel_bitmap >> channel) & 1;
}
void 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;
}
// 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;
}
// 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;
}
const Dcmf R(Quatf(_v_att->q));
const Dcmf R_sp(Quatf(_v_att_sp->q_d));
const Eulerf euler(R);
const Eulerf euler_sp(R_sp);
// direction of desired body z axis represented in earth frame
Vector3f body_z_sp(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
// rotate desired body z axis into new frame which is rotated in z by the current
// heading of the vehicle. we refer to this as the heading frame.
Dcmf R_yaw = Eulerf(0.0f, 0.0f, -euler(2));
body_z_sp = R_yaw * body_z_sp;
body_z_sp.normalize();
// calculate the desired pitch seen in the heading frame
// 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));
// 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;
// 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;
// return the vehicle to level position
float pitch_new = 0.0f;
// create corrected desired body z axis in heading frame
const Dcmf R_tmp = Eulerf(roll_new, pitch_new, 0.0f);
Vector3f tilt_new(R_tmp(0, 2), R_tmp(1, 2), R_tmp(2, 2));
// rotate the vector into a new frame which is rotated in z by the desired heading
// with respect to the earh frame.
const float yaw_error = wrap_pi(euler_sp(2) - euler(2));
const Dcmf R_yaw_correction = Eulerf(0.0f, 0.0f, -yaw_error);
tilt_new = R_yaw_correction * tilt_new;
// now extract roll and pitch setpoints
_v_att_sp->pitch_body = atan2f(tilt_new(0), tilt_new(2));
_v_att_sp->roll_body = -asinf(tilt_new(1));
const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2)));
q_sp.copyTo(_v_att_sp->q_d);
_v_att_sp->q_d_valid = true;
}
}

8
src/modules/vtol_att_control/vtol_type.h
View File

@ -70,6 +70,8 @@ struct Params {
int32_t fw_motors_off; /**< bitmask of all motors that should be off in fixed wing mode */
int32_t diff_thrust;
float diff_thrust_scale;
float down_pitch_max;
float forward_thrust_scale;
};
// Has to match 1:1 msg/vtol_vehicle_status.msg
@ -163,6 +165,10 @@ public:
*/
bool can_transition_on_ground();
/**
* Pusher assist in hover (pusher/pull for standard VTOL, motor tilt for tiltrotor)
*/
void pusher_assist();
mode get_mode() {return _vtol_mode;}
@ -216,6 +222,8 @@ protected:
motor_state _motor_state = motor_state::DISABLED;
float _forward_thrust = 0.0f; // normalized pusher throttle (standard VTOL) or tilt (tiltrotor)
/**