fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 19:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #2640 from UAVenture/vtol_fixes

Browse Source 
VTOL: Bring back the ability to transition, blend and cleanup tiltrotor a little
This commit is contained in:
Lorenz Meier
2015-08-05 18:31:24 +02:00
parent da4d8a5c2b 809b6591c3
commit d4e9b72a01
6 changed files with 42 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/13002_firefly6
+9 -5
View File
@@ -12,21 +12,25 @@ sh /etc/init.d/rc.vtol_defaults
if [ $AUTOCNF == yes ] if [ $AUTOCNF == yes ]
then then
param set MC_ROLL_P 7.0 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_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_ROLLRATE_FF 0.0
param set MC_PITCH_P 7.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_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_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_YAW_FF 0.5
param set MC_YAWRATE_P 0.22 param set MC_YAWRATE_P 0.22
param set MC_YAWRATE_I 0.02 param set MC_YAWRATE_I 0.02
param set MC_YAWRATE_D 0.0 param set MC_YAWRATE_D 0.0
param set MC_YAWRATE_FF 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 fi
set MIXER firefly6 set MIXER firefly6
ROMFS/px4fmu_common/mixers/firefly6.aux.mix
+3 -3
View File
@@ -5,18 +5,18 @@ Tilt mechanism servo mixer
--------------------------- ---------------------------
M: 1 M: 1
O: 10000 10000 0 -10000 10000 O: 10000 10000 0 -10000 10000
S: 1 4 10000 10000 0 -10000 10000 S: 1 4 0 20000 -10000 -10000 10000
Elevon mixers Elevon mixers
------------- -------------
M: 2 M: 2
O: 10000 10000 0 -10000 10000 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 S: 1 1 8000 8000 0 -10000 10000
M: 2 M: 2
O: 10000 10000 0 -10000 10000 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 S: 1 1 -8000 -8000 0 -10000 10000
Landing gear mixer Landing gear mixer
src/modules/fw_att_control/fw_att_control_main.cpp
+1 -5
View File
@@ -695,7 +695,6 @@ FixedwingAttitudeControl::task_main()
/* only run controller if attitude changed */ /* only run controller if attitude changed */
if (fds[1].revents & POLLIN) { if (fds[1].revents & POLLIN) {
static uint64_t last_run = 0; static uint64_t last_run = 0;
float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
last_run = hrt_absolute_time(); last_run = hrt_absolute_time();
@@ -800,7 +799,7 @@ FixedwingAttitudeControl::task_main()
} }
/* if we are in rotary wing mode, do nothing */ /* 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; continue;
} }
@@ -813,11 +812,8 @@ FixedwingAttitudeControl::task_main()
} }
/* decide if in stabilized or full manual control */ /* decide if in stabilized or full manual control */
if (_vcontrol_mode.flag_control_attitude_enabled) { if (_vcontrol_mode.flag_control_attitude_enabled) {
/* scale around tuning airspeed */ /* scale around tuning airspeed */
float airspeed; float airspeed;
/* if airspeed is not updating, we assume the normal average speed */ /* if airspeed is not updating, we assume the normal average speed */
src/modules/vtol_att_control/tiltrotor.cpp
+24 -32
View File
@@ -194,7 +194,7 @@ void Tiltrotor::update_mc_state()
void Tiltrotor::process_mc_data() void Tiltrotor::process_mc_data()
{ {
fill_mc_att_control_output(); fill_att_control_output();
} }
void Tiltrotor::update_fw_state() void Tiltrotor::update_fw_state()
@@ -222,19 +222,22 @@ void Tiltrotor::process_mc_data()
void Tiltrotor::process_fw_data() void Tiltrotor::process_fw_data()
{ {
fill_fw_att_control_output(); fill_att_control_output();
} }
void Tiltrotor::update_transition_state() void Tiltrotor::update_transition_state()
{ {
if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
// tilt rotors forward up to certain angle // tilt rotors forward up to certain angle
if (_tilt_control <= _params_tiltrotor.tilt_transition) { if (_params_tiltrotor.front_trans_dur <= 0.0f) {
_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); _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 // 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); _roll_weight_mc = 1.0f - (_airspeed->true_airspeed_m_s - ARSP_BLEND_START) / (_params_tiltrotor.airspeed_trans - ARSP_BLEND_START);
} else { } else {
// at low speeds give full weight to mc // 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); _roll_weight_mc = math::constrain(_roll_weight_mc, 0.0f, 1.0f);
} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) { } 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; _roll_weight_mc = 0.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
// tilt rotors forward up to certain angle // 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) { 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; _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[0] = _actuators_mc_in->control[0] * _roll_weight_mc; // roll
_actuators_out_0->control[1] = _actuators_mc_in->control[1]; _actuators_out_0->control[1] = _actuators_mc_in->control[1] * _roll_weight_mc; // pitch
_actuators_out_0->control[2] = _actuators_mc_in->control[2]; _actuators_out_0->control[2] = _actuators_mc_in->control[2] * _roll_weight_mc; // yaw
_actuators_out_0->control[3] = _actuators_mc_in->control[3];
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[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 _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 // unused now but still logged
_actuators_out_1->control[2] = _actuators_fw_in->control[2]; // yaw _actuators_out_1->control[2] = _actuators_fw_in->control[2]; // fw yaw
_actuators_out_1->control[3] = _actuators_fw_in->control[3]; // throttle
_actuators_out_1->control[4] = _tilt_control;
} }
/** /**
src/modules/vtol_att_control/tiltrotor.h
+1 -2
View File
@@ -99,8 +99,7 @@ private:
float _tilt_control; float _tilt_control;
float _roll_weight_mc; float _roll_weight_mc;
void fill_mc_att_control_output(); void fill_att_control_output();
void fill_fw_att_control_output();
void set_max_mc(); void set_max_mc();
void set_max_fw(unsigned pwm_value); void set_max_fw(unsigned pwm_value);
src/modules/vtol_att_control/vtol_att_control_main.cpp
+3
View File
@@ -531,6 +531,9 @@ void VtolAttitudeControl::task_main()
// update transition state if got any new data // update transition state if got any new data
if (got_new_data) { if (got_new_data) {
_vtol_type->update_transition_state(); _vtol_type->update_transition_state();
_vtol_type->process_mc_data();
fill_mc_att_rates_sp();
} }
} else if (_vtol_type->get_mode() == EXTERNAL) { } else if (_vtol_type->get_mode() == EXTERNAL) {