diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 9822a2cd25..1c2bbeab22 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -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() diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h index da623cfc30..065b634296 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -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; diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 6c5999bc08..ba4ead672d 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -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; +} diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index 7ee36d7615..1dc38dd1bf 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -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: diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index dcb0aa6d93..68c7de9851 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -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); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index a88b4a7321..cb77aa8431 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -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 diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 7401a2ad16..46e50859da 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -46,6 +46,8 @@ #include #include +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; + } + +} diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 2117bf8f53..069cd2a7a2 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -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) + /**