diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 1ccf54c11f..1d9d7b1155 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -46,12 +46,15 @@ Standard::Standard(VtolAttitudeControl *attc) : VtolType(attc), _flag_enable_mc_motors(true), _pusher_throttle(0.0f), - _mc_att_ctl_weight(1.0f), _airspeed_trans_blend_margin(0.0f) { _vtol_schedule.flight_mode = MC_MODE; _vtol_schedule.transition_start = 0; + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; + _params_handles_standard.front_trans_dur = param_find("VT_F_TRANS_DUR"); _params_handles_standard.back_trans_dur = param_find("VT_B_TRANS_DUR"); _params_handles_standard.pusher_trans = param_find("VT_TRANS_THR"); @@ -107,7 +110,9 @@ void Standard::update_vtol_state() if (_vtol_schedule.flight_mode == MC_MODE) { // in mc mode _vtol_schedule.flight_mode = MC_MODE; - _mc_att_ctl_weight = 1.0f; + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; } else if (_vtol_schedule.flight_mode == FW_MODE) { // transition to mc mode @@ -118,7 +123,9 @@ void Standard::update_vtol_state() } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { // failsafe back to mc mode _vtol_schedule.flight_mode = MC_MODE; - _mc_att_ctl_weight = 1.0f; + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { // keep transitioning to mc mode @@ -138,7 +145,9 @@ void Standard::update_vtol_state() } else if (_vtol_schedule.flight_mode == FW_MODE) { // in fw mode _vtol_schedule.flight_mode = FW_MODE; - _mc_att_ctl_weight = 0.0f; + _mc_roll_weight = 0.0f; + _mc_pitch_weight = 0.0f; + _mc_yaw_weight = 0.0f; } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { // continue the transition to fw mode while monitoring airspeed for a final switch to fw mode @@ -184,18 +193,28 @@ void Standard::update_transition_state() // do blending of mc and fw controls if a blending airspeed has been provided if (_airspeed_trans_blend_margin > 0.0f && _airspeed->true_airspeed_m_s >= _params_standard.airspeed_blend) { - _mc_att_ctl_weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) / _airspeed_trans_blend_margin; + float weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) / _airspeed_trans_blend_margin; + _mc_roll_weight = weight; + _mc_pitch_weight = weight; + _mc_yaw_weight = weight; } else { // at low speeds give full weight to mc - _mc_att_ctl_weight = 1.0f; + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; } } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { // continually increase mc attitude control as we transition back to mc mode if (_params_standard.back_trans_dur > 0.0f) { - _mc_att_ctl_weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * 1000000.0f); + float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * 1000000.0f); + _mc_roll_weight = weight; + _mc_pitch_weight = weight; + _mc_yaw_weight = weight; } else { - _mc_att_ctl_weight = 1.0f; + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; } // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again @@ -206,7 +225,9 @@ void Standard::update_transition_state() } } - _mc_att_ctl_weight = math::constrain(_mc_att_ctl_weight, 0.0f, 1.0f); + _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); + _mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f); + _mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f); } void Standard::update_mc_state() @@ -235,24 +256,23 @@ void Standard::update_external_state() void Standard::fill_actuator_outputs() { /* multirotor controls */ - _actuators_out_0->control[0] = _actuators_mc_in->control[0] * _mc_att_ctl_weight; // roll - _actuators_out_0->control[1] = _actuators_mc_in->control[1] * _mc_att_ctl_weight; // pitch - _actuators_out_0->control[2] = _actuators_mc_in->control[2] * _mc_att_ctl_weight; // yaw - _actuators_out_0->control[3] = _actuators_mc_in->control[3]; // throttle + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; // roll + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; // pitch + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; // yaw + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle /* fixed wing controls */ - const float fw_att_ctl_weight = 1.0f - _mc_att_ctl_weight; - _actuators_out_1->control[0] = -_actuators_fw_in->control[0] * fw_att_ctl_weight; //roll - _actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim) * fw_att_ctl_weight; //pitch - _actuators_out_1->control[2] = _actuators_fw_in->control[2] * fw_att_ctl_weight; // yaw + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); //roll + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight); //pitch + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight); // yaw // set the fixed wing throttle control if (_vtol_schedule.flight_mode == FW_MODE) { // take the throttle value commanded by the fw controller - _actuators_out_1->control[3] = _actuators_fw_in->control[3]; + _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; } else { // otherwise we may be ramping up the throttle during the transition to fw mode - _actuators_out_1->control[3] = _pusher_throttle; + _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle; } } diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h index bdc150e7a8..7bce82071a 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -93,7 +93,6 @@ private: bool _flag_enable_mc_motors; float _pusher_throttle; - float _mc_att_ctl_weight; // the amount of multicopter attitude control that should be applied in fixed wing mode while transitioning float _airspeed_trans_blend_margin; void fill_actuator_outputs(); diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index fd7943a5a9..46fd7b9c3d 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -146,31 +146,31 @@ void Tailsitter::fill_actuator_outputs() { switch(_vtol_mode) { case ROTARY_WING: - _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[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; if (_params->elevons_mc_lock == 1) { _actuators_out_1->control[0] = 0; _actuators_out_1->control[1] = 0; } else { - _actuators_out_1->control[0] = _actuators_mc_in->control[2]; //roll elevon - _actuators_out_1->control[1] = _actuators_mc_in->control[1]; //pitch elevon + // NOTE: There is no mistake in the line below, multicopter yaw axis is controlled by elevon roll actuation! + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW]; //roll elevon + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH]; //pitch elevon } break; case FIXED_WING: - /*For the first test in fw mode, only use engines for thrust!!!*/ - _actuators_out_0->control[0] = 0; - _actuators_out_0->control[1] = 0; - _actuators_out_0->control[2] = 0; - _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 + // in fixed wing mode we use engines only for providing thrust, no moments are generated + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = 0; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; // roll elevon + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim; // pitch elevon + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw + _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; // throttle break; case TRANSITION: case EXTERNAL: diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 87989cf469..73fa1037ab 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -42,17 +42,22 @@ #include "vtol_att_control_main.h" #define ARSP_BLEND_START 8.0f // airspeed at which we start blending mc/fw controls +#define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition +#define DELTA 0.5f // the time it should take to tilt the rotors forward completly after the airspeed + // for transitioning into fixed wing mode has been reached Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : VtolType(attc), _rear_motors(ENABLED), -_tilt_control(0.0f), -_roll_weight_mc(1.0f), -_yaw_weight_mc(1.0f) +_tilt_control(0.0f) { _vtol_schedule.flight_mode = MC_MODE; _vtol_schedule.transition_start = 0; + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; + _params_handles_tiltrotor.front_trans_dur = param_find("VT_F_TRANS_DUR"); _params_handles_tiltrotor.back_trans_dur = param_find("VT_B_TRANS_DUR"); _params_handles_tiltrotor.tilt_mc = param_find("VT_TILT_MC"); @@ -60,7 +65,7 @@ _yaw_weight_mc(1.0f) _params_handles_tiltrotor.tilt_fw = param_find("VT_TILT_FW"); _params_handles_tiltrotor.airspeed_trans = param_find("VT_ARSP_TRANS"); _params_handles_tiltrotor.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); - } +} Tiltrotor::~Tiltrotor() { @@ -101,6 +106,13 @@ Tiltrotor::parameters_update() param_get(_params_handles_tiltrotor.elevons_mc_lock, &l); _params_tiltrotor.elevons_mc_lock = l; + /* avoid parameters which will lead to zero division in the transition code */ + _params.front_trans_dur = math::constrain(_params.front_trans_dur, 0.5f, 10.0f); + + if (fabsf(_params.airspeed_trans - ARSP_BLEND_START) < 1.0f ) { + _params.airspeed_trans = ARSP_BLEND_START + 1.0f; + } + return OK; } @@ -115,10 +127,10 @@ void Tiltrotor::update_vtol_state() */ if (_manual_control_sp->aux1 < 0.0f) { + // plane is in multicopter mode - switch (_vtol_schedule.flight_mode) { + switch(_vtol_schedule.flight_mode) { case MC_MODE: - _tilt_control = _params_tiltrotor.tilt_mc; break; case FW_MODE: _vtol_schedule.flight_mode = TRANSITION_BACK; @@ -135,20 +147,18 @@ void Tiltrotor::update_vtol_state() case TRANSITION_BACK: if (_tilt_control <= _params_tiltrotor.tilt_mc) { _vtol_schedule.flight_mode = MC_MODE; - _tilt_control = _params_tiltrotor.tilt_mc; } break; } - } else { - switch (_vtol_schedule.flight_mode) { + + switch(_vtol_schedule.flight_mode) { case MC_MODE: // initialise a front transition _vtol_schedule.flight_mode = TRANSITION_FRONT_P1; _vtol_schedule.transition_start = hrt_absolute_time(); break; case FW_MODE: - _tilt_control = _params_tiltrotor.tilt_fw; break; case TRANSITION_FRONT_P1: // check if we have reached airspeed to switch to fw mode @@ -165,9 +175,10 @@ void Tiltrotor::update_vtol_state() } break; case TRANSITION_BACK: + // failsafe into fixed wing mode + _vtol_schedule.flight_mode = FW_MODE; break; } - } // map tiltrotor specific control phases to simple control modes @@ -188,7 +199,10 @@ void Tiltrotor::update_vtol_state() void Tiltrotor::update_mc_state() { - // adjust max pwm for rear motors to spin up + // make sure motors are not tilted + _tilt_control = _params_tiltrotor.tilt_mc; + + // enable rear motors if (_rear_motors != ENABLED) { set_rear_motor_state(ENABLED); } @@ -198,13 +212,18 @@ void Tiltrotor::update_mc_state() set_idle_mc(); flag_idle_mc = true; } + + _mc_roll_weight = 1.0f; + _mc_pitch_weight = 1.0f; + _mc_yaw_weight = 1.0f; } void Tiltrotor::update_fw_state() { - /* in fw mode we need the rear motors to stop spinning, in backtransition - * mode we let them spin in idle - */ + // make sure motors are tilted forward + _tilt_control = _params_tiltrotor.tilt_fw; + + // disable rear motors if (_rear_motors != DISABLED) { set_rear_motor_state(DISABLED); } @@ -214,7 +233,11 @@ void Tiltrotor::update_mc_state() set_idle_fw(); flag_idle_mc = false; } - } + + _mc_roll_weight = 0.0f; + _mc_pitch_weight = 0.0f; + _mc_yaw_weight = 0.0f; +} void Tiltrotor::update_transition_state() { @@ -224,42 +247,46 @@ void Tiltrotor::update_transition_state() set_rear_motor_state(ENABLED); } // 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 (_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) { - _roll_weight_mc = 1.0f - (_airspeed->true_airspeed_m_s - ARSP_BLEND_START) / (_params_tiltrotor.airspeed_trans - ARSP_BLEND_START); + _mc_roll_weight = 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 - _roll_weight_mc = 1.0f; + _mc_roll_weight = 1.0f; } // disable mc yaw control once the plane has picked up speed - _yaw_weight_mc = 1.0f; - if (_airspeed->true_airspeed_m_s > 5.0f) { - _yaw_weight_mc = 0.0f; + _mc_yaw_weight = 1.0f; + if (_airspeed->true_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) { + _mc_yaw_weight = 0.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); - _roll_weight_mc = 0.0f; + // the plane is ready to go into fixed wing mode, tilt the rotors forward completely + _tilt_control = _params_tiltrotor.tilt_transition + + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(DELTA*1000000.0f); + _mc_roll_weight = 0.0f; } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { if (_rear_motors != IDLE) { set_rear_motor_state(IDLE); } // tilt rotors back if (_tilt_control > _params_tiltrotor.tilt_mc) { - _tilt_control = _params_tiltrotor.tilt_fw - fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur*1000000.0f); + _tilt_control = _params_tiltrotor.tilt_fw - + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur*1000000.0f); } - _roll_weight_mc = 0.0f; + _mc_roll_weight = 0.0f; } - _roll_weight_mc = math::constrain(_roll_weight_mc, 0.0f, 1.0f); - _yaw_weight_mc = math::constrain(_yaw_weight_mc, 0.0f, 1.0f); + _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); + _mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f); } void Tiltrotor::update_external_state() @@ -272,63 +299,16 @@ void Tiltrotor::update_external_state() */ void Tiltrotor::fill_actuator_outputs() { - switch(_vtol_schedule.flight_mode) { - case MC_MODE: - _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_1->control[0] = 0; - _actuators_out_1->control[1] = 0; - _actuators_out_1->control[4] = _tilt_control; - break; - case FW_MODE: - _actuators_out_0->control[0] = 0; - _actuators_out_0->control[1] = 0; - _actuators_out_0->control[2] = 0; - _actuators_out_0->control[3] = _actuators_fw_in->control[3]; + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; - _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 - _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; - break; - case TRANSITION_FRONT_P1: - _actuators_out_0->control[0] = _actuators_mc_in->control[0] * _roll_weight_mc; - _actuators_out_0->control[1] = _actuators_mc_in->control[1]; - _actuators_out_0->control[2] = _actuators_mc_in->control[2] * _yaw_weight_mc; - _actuators_out_0->control[3] = _actuators_mc_in->control[3]; - _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); //pitch elevon - _actuators_out_1->control[4] = _tilt_control; // for tilt-rotor control - break; - case TRANSITION_FRONT_P2: - _actuators_out_0->control[0] = 0; - _actuators_out_0->control[1] = 0; - _actuators_out_0->control[2] = 0; - _actuators_out_0->control[3] = _actuators_fw_in->control[3]; - - _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 - _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; // tilt - - break; - case TRANSITION_BACK: - _actuators_out_0->control[0] = _actuators_mc_in->control[0] * _roll_weight_mc; - _actuators_out_0->control[1] = _actuators_mc_in->control[1]; - _actuators_out_0->control[2] = _actuators_mc_in->control[2] * _yaw_weight_mc; - _actuators_out_0->control[3] = _actuators_fw_in->control[3]; - - _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 - _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; // tilt - } + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) *(1 - _mc_pitch_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight); // yaw + _actuators_out_1->control[4] = _tilt_control; } @@ -337,19 +317,22 @@ void Tiltrotor::fill_actuator_outputs() */ void Tiltrotor::set_rear_motor_state(rear_motor_state state) { - int pwm_value; + int pwm_value = PWM_DEFAULT_MAX; // map desired rear rotor state to max allowed pwm signal switch (state) { case ENABLED: - pwm_value = 2000; + pwm_value = PWM_DEFAULT_MAX; _rear_motors = ENABLED; + break; case DISABLED: - pwm_value = 950; + pwm_value = PWM_LOWEST_MAX; _rear_motors = DISABLED; + break; case IDLE: - pwm_value = 1250; + pwm_value = _params->idle_pwm_mc; _rear_motors = IDLE; + break; } int ret; @@ -367,7 +350,7 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state) { if (i == 2 || i == 3) { pwm_values.values[i] = pwm_value; } else { - pwm_values.values[i] = 2000; + pwm_values.values[i] = PWM_DEFAULT_MAX; } pwm_values.channel_count = _params->vtol_motor_count; } diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 90b024def4..a797201e01 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -41,6 +41,8 @@ #ifndef VTOL_TYPE_H #define VTOL_TYPE_H +#include + struct Params { int idle_pwm_mc; // pwm value for idle in mc mode int vtol_motor_count; // number of motors @@ -110,6 +112,10 @@ protected: bool flag_idle_mc; //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 + }; #endif