diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6 index 52c6258c39b..d840f6c7e0b 100644 --- a/ROMFS/px4fmu_common/init.d/13002_firefly6 +++ b/ROMFS/px4fmu_common/init.d/13002_firefly6 @@ -46,5 +46,6 @@ set PWM_AUX_MAX 2000 set MAV_TYPE 21 param set VT_MOT_COUNT 6 +param set VT_FW_MOT_OFF 23 param set VT_IDLE_PWM_MC 1080 param set VT_TYPE 1 diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 492a9b84539..7bd58a4cc90 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -67,6 +67,7 @@ _min_front_trans_dur(0.5f) _params_handles_tiltrotor.airspeed_blend_start = param_find("VT_ARSP_BLEND"); _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_OFF"); } Tiltrotor::~Tiltrotor() @@ -80,6 +81,11 @@ Tiltrotor::parameters_update() float v; int l; + /* motors that must be turned off when in fixed wing mode */ + param_get(_params_handles_tiltrotor.fw_motors_off, &l); + _params_tiltrotor.fw_motors_off = get_motor_off_channels(l); + + /* vtol duration of a front transition */ param_get(_params_handles_tiltrotor.front_trans_dur, &v); _params_tiltrotor.front_trans_dur = math::constrain(v,1.0f,5.0f); @@ -126,6 +132,22 @@ Tiltrotor::parameters_update() return OK; } +int Tiltrotor::get_motor_off_channels(int channels) { + int channel_bitmap = 0; + + int channel; + for (int i = 0; i < _params->vtol_motor_count; ++i) { + channel = channels % 10; + if (channel == 0) { + break; + } + channel_bitmap |= 1 << channel; + channels = channels / 10; + } + + return channel_bitmap; +} + void Tiltrotor::update_vtol_state() { parameters_update(); @@ -357,7 +379,7 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state) { memset(&pwm_values, 0, sizeof(pwm_values)); for (int i = 0; i < _params->vtol_motor_count; i++) { - if (i == 2 || i == 3) { + if (is_motor_off_channel(i)) { pwm_values.values[i] = pwm_value; } else { pwm_values.values[i] = PWM_DEFAULT_MAX; @@ -370,5 +392,8 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state) { if (ret != OK) {errx(ret, "failed setting max values");} close(fd); - +} + +bool Tiltrotor::is_motor_off_channel(const int channel) { + return (_params_tiltrotor.fw_motors_off >> channel) & 1; } diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index e54e8cfc726..859731266aa 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -89,6 +89,7 @@ private: float airspeed_blend_start; /**< airspeed at which we start blending mc/fw controls */ 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 */ } _params_tiltrotor; struct { @@ -101,6 +102,7 @@ private: param_t airspeed_blend_start; param_t elevons_mc_lock; param_t front_trans_dur_p2; + param_t fw_motors_off; } _params_handles_tiltrotor; enum vtol_mode { @@ -133,6 +135,16 @@ private: const float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */ + /** + * Return a bitmap of channels that should be turned off in fixed wing mode. + */ + int get_motor_off_channels(const int channels); + + /** + * Return true if the motor channel is off in fixed wing mode. + */ + bool is_motor_off_channel(const int channel); + /** * Write control values to actuator output topics. */ diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c index 7a87fbd8f1f..d1b5b31081a 100644 --- a/src/modules/vtol_att_control/tiltrotor_params.c +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -84,3 +84,13 @@ PARAM_DEFINE_FLOAT(VT_TILT_FW, 1.0f); * @group VTOL Attitude Control */ PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f); + +/** + * The channel number of motors that must be turned off in fixed wing mode. + * + * + * @min 0 + * @max 123456 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_FW_MOT_OFF, 0);