From c416fc3fa00f389932f4dee4267033ab8b3b7ec3 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Fri, 30 Dec 2016 15:47:46 +0100 Subject: [PATCH] Tiltrotor: - added open-loop transition time for airspeed-less flying - added ramping down the back motors during forwards transition --- src/modules/vtol_att_control/tiltrotor.cpp | 91 ++++++++++++++----- src/modules/vtol_att_control/tiltrotor.h | 7 +- .../vtol_att_control_main.cpp | 12 +++ .../vtol_att_control/vtol_att_control_main.h | 2 + src/modules/vtol_att_control/vtol_type.h | 2 + 5 files changed, 87 insertions(+), 27 deletions(-) diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index d6666ab93f..655901bee5 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -35,6 +35,7 @@ * @file tiltrotor.cpp * * @author Roman Bapst + * @author Andreas Antener * */ @@ -68,6 +69,7 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : _params_handles_tiltrotor.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); _params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR"); _params_handles_tiltrotor.fw_motors_off = param_find("VT_FW_MOT_OFFID"); + _params_handles_tiltrotor.airspeed_mode = param_find("FW_ARSP_MODE"); } Tiltrotor::~Tiltrotor() @@ -128,6 +130,10 @@ Tiltrotor::parameters_update() if (_params_tiltrotor.airspeed_trans < _params_tiltrotor.airspeed_blend_start + 1.0f) { _params_tiltrotor.airspeed_trans = _params_tiltrotor.airspeed_blend_start + 1.0f; } + + /* airspeed mode */ + param_get(_params_handles_tiltrotor.airspeed_mode, &l); + _params_tiltrotor.airspeed_mode = math::constrain(l, 0, 2); } int Tiltrotor::get_motor_off_channels(int channels) @@ -201,17 +207,28 @@ void Tiltrotor::update_vtol_state() case FW_MODE: break; - case TRANSITION_FRONT_P1: + case TRANSITION_FRONT_P1: { + // allow switch if we are not armed for the sake of bench testing + bool transition_to_p2 = can_transition_on_ground(); - // check if we have reached airspeed to switch to fw mode - // also allow switch if we are not armed for the sake of bench testing - if (_airspeed->indicated_airspeed_m_s >= _params_tiltrotor.airspeed_trans || can_transition_on_ground()) { - _vtol_schedule.flight_mode = TRANSITION_FRONT_P2; - _vtol_schedule.transition_start = hrt_absolute_time(); + // check if we have reached airspeed to switch to fw mode + transition_to_p2 |= _params_tiltrotor.airspeed_mode != control_state_s::AIRSPD_MODE_DISABLED && + _airspeed->indicated_airspeed_m_s >= _params_tiltrotor.airspeed_trans && + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params->front_trans_time_min * 1e6f); + + // check if airspeed is invalid and transition by time + transition_to_p2 |= _params_tiltrotor.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED && + _tilt_control > _params_tiltrotor.tilt_transition && + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params->front_trans_time_openloop * 1e6f); + + if (transition_to_p2) { + _vtol_schedule.flight_mode = TRANSITION_FRONT_P2; + _vtol_schedule.transition_start = hrt_absolute_time(); + } + + break; } - break; - case TRANSITION_FRONT_P2: // if the rotors have been tilted completely we switch to fw mode @@ -290,6 +307,8 @@ void Tiltrotor::update_fw_state() void Tiltrotor::update_transition_state() { + VtolType::update_transition_state(); + if (!_flag_was_in_trans_mode) { // save desired heading for transition and last thrust value _flag_was_in_trans_mode = true; @@ -308,19 +327,25 @@ void Tiltrotor::update_transition_state() &_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur * 1000000.0f); } - // do blending of mc and fw controls - if (_airspeed->indicated_airspeed_m_s >= _params_tiltrotor.airspeed_blend_start) { - _mc_roll_weight = 0.0f; + bool use_airspeed = _params_tiltrotor.airspeed_mode != control_state_s::AIRSPD_MODE_DISABLED; - } else { - // at low speeds give full weight to mc - _mc_roll_weight = 1.0f; - } - - // disable mc yaw control once the plane has picked up speed + // at low speeds give full weight to MC + _mc_roll_weight = 1.0f; _mc_yaw_weight = 1.0f; - if (_airspeed->indicated_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) { + // reduce MC controls once the plane has picked up speed + if (use_airspeed && _airspeed->indicated_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) { + _mc_yaw_weight = 0.0f; + } + + if (use_airspeed && _airspeed->indicated_airspeed_m_s >= _params_tiltrotor.airspeed_blend_start) { + _mc_roll_weight = 0.0f; + } + + // without airspeed wait until half of open-loop time has passed + if (!use_airspeed + && (float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params->front_trans_time_openloop * 1e6f) / 2) { + _mc_roll_weight = 0.0f; _mc_yaw_weight = 0.0f; } @@ -331,7 +356,16 @@ void Tiltrotor::update_transition_state() _tilt_control = _params_tiltrotor.tilt_transition + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * (float)hrt_elapsed_time( &_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur_p2 * 1000000.0f); + _mc_roll_weight = 0.0f; + _mc_yaw_weight = 0.0f; + + // ramp down rear motors (setting MAX_PWM down scales the given output into the new range) + int rear_value = (1.0f - (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / + (_params_tiltrotor.front_trans_dur_p2 * + 1000000.0f)) * (float)(PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) + (float)PWM_DEFAULT_MIN; + + set_rear_motor_state(VALUE, rear_value); _thrust_transition = _mc_virtual_att_sp->thrust; @@ -395,12 +429,12 @@ void Tiltrotor::fill_actuator_outputs() } _actuators_out_1->timestamp = _actuators_fw_in->timestamp; - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] - * (1 - _mc_roll_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = + -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = - (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight); - _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] - * (1 - _mc_yaw_weight); // yaw + (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim); + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = + _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw _actuators_out_1->control[4] = _tilt_control; } @@ -409,7 +443,7 @@ void Tiltrotor::fill_actuator_outputs() * Set state of rear motors. */ -void Tiltrotor::set_rear_motor_state(rear_motor_state state) +void Tiltrotor::set_rear_motor_state(rear_motor_state state, int value) { int pwm_value = PWM_DEFAULT_MAX; @@ -429,6 +463,11 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state) pwm_value = _params->idle_pwm_mc; _rear_motors = IDLE; break; + + case VALUE: + pwm_value = value; + _rear_motors = VALUE; + break; } int ret; @@ -457,7 +496,9 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state) ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_max_values); - if (ret != OK) {PX4_WARN("failed setting max values");} + if (ret != OK) { + PX4_WARN("failed setting max values"); + } px4_close(fd); } diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index 91b938eaf3..39363a7744 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -72,6 +72,7 @@ private: int elevons_mc_lock; /**< lock elevons in multicopter mode */ float front_trans_dur_p2; int fw_motors_off; /**< bitmask of all motors that should be off in fixed wing mode */ + int airspeed_mode; } _params_tiltrotor; struct { @@ -85,6 +86,7 @@ private: param_t elevons_mc_lock; param_t front_trans_dur_p2; param_t fw_motors_off; + param_t airspeed_mode; } _params_handles_tiltrotor; enum vtol_mode { @@ -103,7 +105,8 @@ private: enum rear_motor_state { ENABLED = 0, DISABLED, - IDLE + IDLE, + VALUE } _rear_motors; struct { @@ -128,7 +131,7 @@ private: /** * Adjust the state of the rear motors. In fw mode they shouldn't spin. */ - void set_rear_motor_state(rear_motor_state state); + void set_rear_motor_state(rear_motor_state state, int value = 0); /** * Update parameters. 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 344b9247e0..b3dae9495c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -132,6 +132,8 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_handles.vtol_type = param_find("VT_TYPE"); _params_handles.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); _params_handles.fw_min_alt = param_find("VT_FW_MIN_ALT"); + _params_handles.front_trans_time_openloop = param_find("VT_F_TR_OL_TM"); + _params_handles.front_trans_time_min = param_find("VT_TRANS_MIN_TM"); /* fetch initial parameter values */ parameters_update(); @@ -560,6 +562,16 @@ VtolAttitudeControl::parameters_update() param_get(_params_handles.fw_min_alt, &v); _params.fw_min_alt = v; + param_get(_params_handles.front_trans_time_openloop, &_params.front_trans_time_openloop); + + param_get(_params_handles.front_trans_time_min, &_params.front_trans_time_min); + + /* + * Minimum transition time can be maximum 90 percent of the open loop transition time, + * anything else makes no sense and can potentially lead to numerical problems. + */ + _params.front_trans_time_min = math::min(_params.front_trans_time_openloop * 0.9f, + _params.front_trans_time_min); // update the parameters of the instances of base VtolType if (_vtol_type != nullptr) { 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 1858e7a26c..a43666c8c1 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -210,6 +210,8 @@ private: param_t vtol_type; param_t elevons_mc_lock; param_t fw_min_alt; + param_t front_trans_time_openloop; + param_t front_trans_time_min; } _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.h b/src/modules/vtol_att_control/vtol_type.h index 1ae3e2fe04..1a6e863302 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -60,6 +60,8 @@ struct Params { int vtol_type; int elevons_mc_lock; // lock elevons in multicopter mode float fw_min_alt; // minimum relative altitude for FW mode (QuadChute) + float front_trans_time_openloop; + float front_trans_time_min; }; // Has to match 1:1 msg/vtol_vehicle_status.msg