From 4c61b05ef3c9ec47ea819bc5c7121eed88d29598 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 18 Feb 2016 09:48:30 +0100 Subject: [PATCH] tailsitter, tiltrotor: - use the transition trust when waiting for TECS to kick in after transition --- src/modules/vtol_att_control/tailsitter.cpp | 23 +++++++++++++++------ src/modules/vtol_att_control/tailsitter.h | 5 +++++ src/modules/vtol_att_control/tiltrotor.cpp | 12 +++++++++-- src/modules/vtol_att_control/tiltrotor.h | 1 + src/modules/vtol_att_control/vtol_type.h | 22 +++++++++----------- 5 files changed, 43 insertions(+), 20 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 170625343f..5d1676f37a 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -52,7 +52,9 @@ Tailsitter::Tailsitter(VtolAttitudeControl *attc) : VtolType(attc), _airspeed_tot(0.0f), _min_front_trans_dur(0.5f), - + _thrust_transition_start(0.0f), + _yaw_transition(0.0f), + _pitch_transition_start(0.0f), _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")), _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input")) { @@ -233,7 +235,7 @@ void Tailsitter::update_transition_state() // save desired heading for transition and last thrust value _yaw_transition = _v_att_sp->yaw_body; _pitch_transition_start = _v_att_sp->pitch_body; - _throttle_transition = _v_att_sp->thrust; + _thrust_transition_start = _v_att_sp->thrust; _flag_was_in_trans_mode = true; } @@ -247,10 +249,11 @@ void Tailsitter::update_transition_state() /** create time dependant throttle signal higher than in MC and growing untill P2 switch speed reached */ if (_airspeed->true_airspeed_m_s <= _params_tailsitter.airspeed_trans) { - _v_att_sp->thrust = _throttle_transition + (fabsf(THROTTLE_TRANSITION_MAX * _throttle_transition) * + _thrust_transition = _thrust_transition_start + (fabsf(THROTTLE_TRANSITION_MAX * _thrust_transition_start) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f)); - _v_att_sp->thrust = math::constrain(_v_att_sp->thrust , _throttle_transition , - (1.0f + THROTTLE_TRANSITION_MAX) * _throttle_transition); + _thrust_transition = math::constrain(_thrust_transition , _thrust_transition_start , + (1.0f + THROTTLE_TRANSITION_MAX) * _thrust_transition_start); + _v_att_sp->thrust = _thrust_transition; } // disable mc yaw control once the plane has picked up speed @@ -286,6 +289,8 @@ void Tailsitter::update_transition_state() } + _v_att_sp->thrust = _thrust_transition; + /** start blending MC and FW controls from pitch -45 to pitch -70 for smooth control takeover*/ //_mc_roll_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); @@ -312,7 +317,7 @@ void Tailsitter::update_transition_state() _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , -2.0f , PITCH_TRANSITION_BACK + 0.2f); // throttle value is decreesed - _v_att_sp->thrust = _throttle_transition * 0.9f; + _v_att_sp->thrust = _thrust_transition_start * 0.9f; /** keep yaw disabled */ _mc_yaw_weight = 0.0f; @@ -348,6 +353,12 @@ void Tailsitter::update_transition_state() memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d)); } +void Tailsitter::waiting_on_tecs() +{ + // copy the last trust value from the front transition + _v_att_sp->thrust = _thrust_transition; +} + void Tailsitter::calc_tot_airspeed() { float airspeed = math::max(1.0f, _airspeed->true_airspeed_m_s); // prevent numerical drama diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index 8bbcca50b2..17fdcae965 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -59,6 +59,7 @@ public: virtual void update_mc_state(); virtual void update_fw_state(); virtual void fill_actuator_outputs(); + virtual void waiting_on_tecs(); private: @@ -100,6 +101,10 @@ private: /** not sure about it yet ?! **/ float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */ + float _thrust_transition_start; // throttle value when we start the front transition + float _yaw_transition; // yaw angle in which transition will take place + float _pitch_transition_start; // pitch angle at the start of transition (tailsitter) + /** should this anouncement stay? **/ perf_counter_t _loop_perf; /**< loop performance counter */ diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 1f5df2639b..a106a7496f 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -292,8 +292,6 @@ void Tiltrotor::update_transition_state() { if (!_flag_was_in_trans_mode) { // save desired heading for transition and last thrust value - _yaw_transition = _v_att->yaw; - _throttle_transition = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; _flag_was_in_trans_mode = true; } @@ -326,6 +324,8 @@ void Tiltrotor::update_transition_state() _mc_yaw_weight = 0.0f; } + _thrust_transition = _mc_virtual_att_sp->thrust; + } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) { // the plane is ready to go into fixed wing mode, tilt the rotors forward completely _tilt_control = _params_tiltrotor.tilt_transition + @@ -333,6 +333,8 @@ void Tiltrotor::update_transition_state() &_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur_p2 * 1000000.0f); _mc_roll_weight = 0.0f; + _thrust_transition = _mc_virtual_att_sp->thrust; + } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { if (_rear_motors != IDLE) { set_rear_motor_state(IDLE); @@ -364,6 +366,12 @@ void Tiltrotor::update_transition_state() memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); } +void Tiltrotor::waiting_on_tecs() +{ + // keep multicopter thrust until we get data from TECS + _v_att_sp->thrust = _thrust_transition; +} + /** * Write data to actuator output topic. */ diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index e18d2ca0f6..041cbf536f 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -57,6 +57,7 @@ public: virtual void fill_actuator_outputs(); virtual void update_mc_state(); virtual void update_fw_state(); + virtual void waiting_on_tecs(); private: diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 7fed24cb8a..8df24880a8 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -146,21 +146,19 @@ protected: struct Params *_params; - bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode" + bool flag_idle_mc = true; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode" - float _mc_roll_weight; // weight for multicopter attitude controller roll output - float _mc_pitch_weight; // weight for multicopter attitude controller pitch output - float _mc_yaw_weight; // weight for multicopter attitude controller yaw output - float _mc_throttle_weight; // weight for multicopter throttle command. Used to avoid + float _mc_roll_weight = 1.0f; // weight for multicopter attitude controller roll output + float _mc_pitch_weight = 1.0f; // weight for multicopter attitude controller pitch output + float _mc_yaw_weight = 1.0f; // weight for multicopter attitude controller yaw output + float _mc_throttle_weight = 1.0f; // weight for multicopter throttle command. Used to avoid // motors spinning up or cutting too fast whend doing transitions. + float _thrust_transition = 0.0f; // thrust value applied during a front transition (tailsitter & tiltrotor only) - float _yaw_transition; // yaw angle in which transition will take place - float _pitch_transition_start; // pitch angle at the start of transition (tailsitter) - float _throttle_transition; // throttle value used for the transition phase - bool _flag_was_in_trans_mode; // true if mode has just switched to transition - hrt_abstime _trans_finished_ts; - bool _tecs_running; - hrt_abstime _tecs_running_ts; + bool _flag_was_in_trans_mode = false; // true if mode has just switched to transition + hrt_abstime _trans_finished_ts = 0; + bool _tecs_running = false; + hrt_abstime _tecs_running_ts = 0; };