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/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index dfa270b048..d45cb7c97a 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -626,6 +626,10 @@ then then set MAV_TYPE 21 fi + if [ $MIXER == quad_x_pusher_vtol ] + then + set MAV_TYPE 22 + fi fi # Still no MAV_TYPE found 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 43c44aaeac..9a92f5c70e 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(); @@ -799,6 +798,11 @@ FixedwingAttitudeControl::task_main() //warnx("_actuators_airframe.control[1] = -1.0f;"); } + /* if we are in rotary wing mode, do nothing */ + if (_vehicle_status.is_rotary_wing && !_vehicle_status.is_vtol) { + continue; + } + /* default flaps to center */ float flaps_control = 0.0f; @@ -808,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/module.mk b/src/modules/vtol_att_control/module.mk index ad6efd2b27..d3f9326b04 100644 --- a/src/modules/vtol_att_control/module.mk +++ b/src/modules/vtol_att_control/module.mk @@ -42,6 +42,8 @@ SRCS = vtol_att_control_main.cpp \ tiltrotor_params.c \ tiltrotor.cpp \ vtol_type.cpp \ - tailsitter.cpp + tailsitter.cpp \ + standard_params.c \ + standard.cpp EXTRACXXFLAGS = -Wno-write-strings 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/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c index 7d233f6f50..d4e925f072 100644 --- a/src/modules/vtol_att_control/tiltrotor_params.c +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -39,28 +39,6 @@ */ #include - -/** - * Duration of a front transition - * - * Time in seconds used for a transition - * - * @min 0.0 - * @max 5 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR,3.0f); - -/** - * Duration of a back transition - * - * Time in seconds used for a back transition - * - * @min 0.0 - * @max 5 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR,2.0f); /** * Position of tilt servo in mc mode @@ -71,7 +49,7 @@ PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR,2.0f); * @max 1 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_TILT_MC,0.0f); +PARAM_DEFINE_FLOAT(VT_TILT_MC, 0.0f); /** * Position of tilt servo in transition mode @@ -82,7 +60,7 @@ PARAM_DEFINE_FLOAT(VT_TILT_MC,0.0f); * @max 1 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_TILT_TRANS,0.3f); +PARAM_DEFINE_FLOAT(VT_TILT_TRANS, 0.3f); /** * Position of tilt servo in fw mode @@ -93,15 +71,4 @@ PARAM_DEFINE_FLOAT(VT_TILT_TRANS,0.3f); * @max 1 * @group VTOL Attitude Control */ -PARAM_DEFINE_FLOAT(VT_TILT_FW,1.0f); - -/** - * Transition airspeed - * - * Airspeed at which we can switch to fw mode - * - * @min 0.0 - * @max 20 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_ARSP_TRANS,10.0f); +PARAM_DEFINE_FLOAT(VT_TILT_FW, 1.0f); 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 b70cd19dd8..0d462423b6 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -122,6 +122,9 @@ VtolAttitudeControl::VtolAttitudeControl() : } else if (_params.vtol_type == 1) { _tiltrotor = new Tiltrotor(this); _vtol_type = _tiltrotor; + } else if (_params.vtol_type == 2) { + _standard = new Standard(this); + _vtol_type = _standard; } else { _task_should_exit = true; } @@ -530,6 +533,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) { 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 43e8969929..bf9a69d5c1 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -84,6 +84,7 @@ #include "tiltrotor.h" #include "tailsitter.h" +#include "standard.h" extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]); @@ -188,6 +189,7 @@ private: VtolType * _vtol_type; // base class for different vtol types Tiltrotor * _tiltrotor; // tailsitter vtol type Tailsitter * _tailsitter; // tiltrotor vtol type + Standard * _standard; // standard vtol type //*****************Member functions*********************************************************************** diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 33c095036c..56779ca8f9 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -143,10 +143,10 @@ PARAM_DEFINE_FLOAT(VT_PROP_EFF, 0.0f); PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN, 0.3f); /** - * VTOL Type (Tailsitter=0, Tiltrotor=1) + * VTOL Type (Tailsitter=0, Tiltrotor=1, Standard=2) * * @min 0 - * @max 1 + * @max 2 * @group VTOL Attitude Control */ PARAM_DEFINE_INT32(VT_TYPE, 0); @@ -161,3 +161,47 @@ PARAM_DEFINE_INT32(VT_TYPE, 0); * @group VTOL Attitude Control */ PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK, 0); + +/** + * Duration of a front transition + * + * Time in seconds used for a transition + * + * @min 0.0 + * @max 5 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR, 3.0f); + +/** + * Duration of a back transition + * + * Time in seconds used for a back transition + * + * @min 0.0 + * @max 5 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR, 2.0f); + +/** + * Transition blending airspeed + * + * Airspeed at which we can start blending both fw and mc controls. Set to 0 to disable. + * + * @min 0.0 + * @max 20.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_ARSP_BLEND, 8.0f); + +/** + * Transition airspeed + * + * Airspeed at which we can switch to fw mode + * + * @min 1.0 + * @max 20 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_ARSP_TRANS, 10.0f); diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index bbe6a8642e..a7fdbdde1a 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -38,8 +38,8 @@ * */ -#ifndef VTOL_YYPE_H -#define VTOL_YYPE_H +#ifndef VTOL_TYPE_H +#define VTOL_TYPE_H struct Params { int idle_pwm_mc; // pwm value for idle in mc mode