From 809b6591c309d4a9941268f04d7368fd69fcd06d Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 5 Aug 2015 16:16:03 +0200 Subject: [PATCH] Bring back the ability to transition plus a partial cleanup of tiltrotor support and firefly6 config updates. --- ROMFS/px4fmu_common/init.d/13002_firefly6 | 14 +++-- ROMFS/px4fmu_common/mixers/firefly6.aux.mix | 6 +- .../fw_att_control/fw_att_control_main.cpp | 6 +- src/modules/vtol_att_control/tiltrotor.cpp | 58 ++++++++----------- src/modules/vtol_att_control/tiltrotor.h | 3 +- .../vtol_att_control_main.cpp | 3 + 6 files changed, 42 insertions(+), 48 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6 index 1ec65dff3e..52c6258c39 100644 --- a/ROMFS/px4fmu_common/init.d/13002_firefly6 +++ b/ROMFS/px4fmu_common/init.d/13002_firefly6 @@ -12,21 +12,25 @@ sh /etc/init.d/rc.vtol_defaults if [ $AUTOCNF == yes ] then param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.15 + param set MC_ROLLRATE_P 0.17 param set MC_ROLLRATE_I 0.002 - param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_D 0.004 param set MC_ROLLRATE_FF 0.0 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.12 + param set MC_PITCHRATE_P 0.14 param set MC_PITCHRATE_I 0.002 - param set MC_PITCHRATE_D 0.003 + param set MC_PITCHRATE_D 0.004 param set MC_PITCHRATE_FF 0.0 - param set MC_YAW_P 2.8 + param set MC_YAW_P 3.8 param set MC_YAW_FF 0.5 param set MC_YAWRATE_P 0.22 param set MC_YAWRATE_I 0.02 param set MC_YAWRATE_D 0.0 param set MC_YAWRATE_FF 0.0 + + param set VT_TILT_MC 0.08 + param set VT_TILT_TRANS 0.5 + param set VT_TILT_FW 0.9 fi set MIXER firefly6 diff --git a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix index 22dc2a69ce..8e31007ff1 100644 --- a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix +++ b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix @@ -5,18 +5,18 @@ Tilt mechanism servo mixer --------------------------- M: 1 O: 10000 10000 0 -10000 10000 -S: 1 4 10000 10000 0 -10000 10000 +S: 1 4 0 20000 -10000 -10000 10000 Elevon mixers ------------- M: 2 O: 10000 10000 0 -10000 10000 -S: 1 0 -7500 -7500 0 -10000 10000 +S: 1 0 -7500 -7500 0 -10000 10000 S: 1 1 8000 8000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 -S: 1 0 -7500 -7500 0 -10000 10000 +S: 1 0 -7500 -7500 0 -10000 10000 S: 1 1 -8000 -8000 0 -10000 10000 Landing gear mixer diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 442aaa1702..ee78ef3110 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -695,7 +695,6 @@ FixedwingAttitudeControl::task_main() /* only run controller if attitude changed */ if (fds[1].revents & POLLIN) { - static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); @@ -800,7 +799,7 @@ FixedwingAttitudeControl::task_main() } /* if we are in rotary wing mode, do nothing */ - if (_vehicle_status.is_rotary_wing) { + if (_vehicle_status.is_rotary_wing && !_vehicle_status.is_vtol) { continue; } @@ -813,11 +812,8 @@ FixedwingAttitudeControl::task_main() } /* decide if in stabilized or full manual control */ - if (_vcontrol_mode.flag_control_attitude_enabled) { - /* scale around tuning airspeed */ - float airspeed; /* if airspeed is not updating, we assume the normal average speed */ diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 2cf074fe42..e7ebab58e9 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -194,7 +194,7 @@ void Tiltrotor::update_mc_state() void Tiltrotor::process_mc_data() { - fill_mc_att_control_output(); + fill_att_control_output(); } void Tiltrotor::update_fw_state() @@ -222,19 +222,22 @@ void Tiltrotor::process_mc_data() void Tiltrotor::process_fw_data() { - fill_fw_att_control_output(); + fill_att_control_output(); } void Tiltrotor::update_transition_state() { if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { // tilt rotors forward up to certain angle - if (_tilt_control <= _params_tiltrotor.tilt_transition) { - _tilt_control = _params_tiltrotor.tilt_mc + fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur*1000000.0f); + if (_params_tiltrotor.front_trans_dur <= 0.0f) { + _tilt_control = _params_tiltrotor.tilt_transition; + } else if (_tilt_control <= _params_tiltrotor.tilt_transition) { + _tilt_control = _params_tiltrotor.tilt_mc + fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) * + (float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.front_trans_dur * 1000000.0f); } // do blending of mc and fw controls - if (_airspeed->true_airspeed_m_s >= ARSP_BLEND_START) { + if (_airspeed->true_airspeed_m_s >= ARSP_BLEND_START && _params_tiltrotor.airspeed_trans - ARSP_BLEND_START > 0.0f) { _roll_weight_mc = 1.0f - (_airspeed->true_airspeed_m_s - ARSP_BLEND_START) / (_params_tiltrotor.airspeed_trans - ARSP_BLEND_START); } else { // at low speeds give full weight to mc @@ -244,13 +247,14 @@ void Tiltrotor::update_transition_state() _roll_weight_mc = math::constrain(_roll_weight_mc, 0.0f, 1.0f); } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) { - _tilt_control = _params_tiltrotor.tilt_transition + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(0.5f*1000000.0f); + _tilt_control = _params_tiltrotor.tilt_transition + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * + (float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (0.5f * 1000000.0f); _roll_weight_mc = 0.0f; } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { // tilt rotors forward up to certain angle - float progress = (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur*1000000.0f); + float progress = (float) hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tiltrotor.back_trans_dur * 1000000.0f); if (_tilt_control > _params_tiltrotor.tilt_mc) { - _tilt_control = _params_tiltrotor.tilt_fw - fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc)*progress; + _tilt_control = _params_tiltrotor.tilt_fw - fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc) * progress; } _roll_weight_mc = progress; @@ -263,38 +267,26 @@ void Tiltrotor::update_external_state() } /** -* Prepare message to acutators with data from mc attitude controller. +* Prepare message to acutators with data from the attitude controllers. */ -void Tiltrotor::fill_mc_att_control_output() +void Tiltrotor::fill_att_control_output() { - _actuators_out_0->control[0] = _actuators_mc_in->control[0]; - _actuators_out_0->control[1] = _actuators_mc_in->control[1]; - _actuators_out_0->control[2] = _actuators_mc_in->control[2]; - _actuators_out_0->control[3] = _actuators_mc_in->control[3]; + _actuators_out_0->control[0] = _actuators_mc_in->control[0] * _roll_weight_mc; // roll + _actuators_out_0->control[1] = _actuators_mc_in->control[1] * _roll_weight_mc; // pitch + _actuators_out_0->control[2] = _actuators_mc_in->control[2] * _roll_weight_mc; // yaw + + if (_vtol_schedule.flight_mode == FW_MODE) { + _actuators_out_1->control[3] = _actuators_fw_in->control[3]; // fw throttle + } else { + _actuators_out_0->control[3] = _actuators_mc_in->control[3]; // mc throttle + } _actuators_out_1->control[0] = -_actuators_fw_in->control[0] * (1.0f - _roll_weight_mc); //roll elevon - _actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim)* (1.0f -_roll_weight_mc); //pitch elevon - + _actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim)* (1.0f -_roll_weight_mc); //pitch elevon _actuators_out_1->control[4] = _tilt_control; // for tilt-rotor control -} -/** -* Prepare message to acutators with data from fw attitude controller. -*/ -void Tiltrotor::fill_fw_att_control_output() -{ - /*For the first test in fw mode, only use engines for thrust!!!*/ - _actuators_out_0->control[0] = _actuators_mc_in->control[0] * _roll_weight_mc; - _actuators_out_0->control[1] = _actuators_mc_in->control[1] * _roll_weight_mc; - _actuators_out_0->control[2] = _actuators_mc_in->control[2] * _roll_weight_mc; - _actuators_out_0->control[3] = _actuators_fw_in->control[3]; - /*controls for the elevons */ - _actuators_out_1->control[0] = -_actuators_fw_in->control[0]; // roll elevon - _actuators_out_1->control[1] = _actuators_fw_in->control[1] + _params->fw_pitch_trim; // pitch elevon // unused now but still logged - _actuators_out_1->control[2] = _actuators_fw_in->control[2]; // yaw - _actuators_out_1->control[3] = _actuators_fw_in->control[3]; // throttle - _actuators_out_1->control[4] = _tilt_control; + _actuators_out_1->control[2] = _actuators_fw_in->control[2]; // fw yaw } /** diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index 3ef1362d00..07f3562027 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -99,8 +99,7 @@ private: float _tilt_control; float _roll_weight_mc; - void fill_mc_att_control_output(); - void fill_fw_att_control_output(); + void fill_att_control_output(); void set_max_mc(); void set_max_fw(unsigned pwm_value); 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 589a08d8cd..7fbb43f769 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -531,6 +531,9 @@ void VtolAttitudeControl::task_main() // update transition state if got any new data if (got_new_data) { _vtol_type->update_transition_state(); + + _vtol_type->process_mc_data(); + fill_mc_att_rates_sp(); } } else if (_vtol_type->get_mode() == EXTERNAL) {