From 7127f6a92fb6060c365c1032c2609ad9d4f60c94 Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 28 Oct 2015 22:48:53 +0100 Subject: [PATCH] fixed code style --- src/modules/vtol_att_control/standard.cpp | 2 +- src/modules/vtol_att_control/tailsitter.cpp | 380 ++++++++++-------- src/modules/vtol_att_control/tailsitter.h | 18 +- .../vtol_att_control/tailsitter_params.c | 2 +- src/modules/vtol_att_control/tiltrotor.cpp | 1 + .../vtol_att_control_main.cpp | 5 + .../vtol_att_control/vtol_att_control_main.h | 37 +- 7 files changed, 250 insertions(+), 195 deletions(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 5a329d52f9..8a5896999e 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -254,7 +254,7 @@ void Standard::update_fw_state() // copy virtual attitude setpoint to real attitude setpoint memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); - // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again + // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again if (!_flag_enable_mc_motors) { set_max_mc(950); set_idle_fw(); // force them to stop, not just idle diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 827e3ec617..1776dc141d 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -31,33 +31,33 @@ * ****************************************************************************/ - /** - * @file tailsitter.cpp - * - * @author Roman Bapst - * @author David Vorsin - * - */ +/** +* @file tailsitter.cpp +* +* @author Roman Bapst +* @author David Vorsin +* +*/ #include "tailsitter.h" #include "vtol_att_control_main.h" - #define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition - #define THROTTLE_TRANSITION_MAX 0.25f // maximum added thrust above last value in transition - #define PITCH_TRANSITION_FRONT_P1 -1.1f // pitch angle to switch to TRANSITION_P2 - #define PITCH_TRANSITION_FRONT_P2 -1.2f // pitch angle to switch to FW - #define PITCH_TRANSITION_BACK -0.25f // pitch angle to switch to MC +#define ARSP_YAW_CTRL_DISABLE 7.0f // airspeed at which we stop controlling yaw during a front transition +#define THROTTLE_TRANSITION_MAX 0.25f // maximum added thrust above last value in transition +#define PITCH_TRANSITION_FRONT_P1 -1.1f // pitch angle to switch to TRANSITION_P2 +#define PITCH_TRANSITION_FRONT_P2 -1.2f // pitch angle to switch to FW +#define PITCH_TRANSITION_BACK -0.25f // pitch angle to switch to MC -Tailsitter::Tailsitter (VtolAttitudeControl *attc) : -VtolType(attc), -_airspeed_tot(0.0f), -_roll_weight_mc(1.0f), -_pitch_weight_mc(1.0f), -_yaw_weight_mc(1.0f), -_min_front_trans_dur(0.5f), +Tailsitter::Tailsitter(VtolAttitudeControl *attc) : + VtolType(attc), + _airspeed_tot(0.0f), + _roll_weight_mc(1.0f), + _pitch_weight_mc(1.0f), + _yaw_weight_mc(1.0f), + _min_front_trans_dur(0.5f), -_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")), -_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input")) + _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input")) { _vtol_schedule.flight_mode = MC_MODE; _vtol_schedule.transition_start = 0; @@ -74,7 +74,7 @@ _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinit _params_handles_tailsitter.airspeed_trans = param_find("VT_ARSP_TRANS"); _params_handles_tailsitter.airspeed_blend_start = param_find("VT_ARSP_BLEND"); _params_handles_tailsitter.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); - + } Tailsitter::~Tailsitter() @@ -90,7 +90,7 @@ Tailsitter::parameters_update() /* vtol duration of a front transition */ param_get(_params_handles_tailsitter.front_trans_dur, &v); - _params_tailsitter.front_trans_dur = math::constrain(v,1.0f,5.0f); + _params_tailsitter.front_trans_dur = math::constrain(v, 1.0f, 5.0f); /* vtol front transition phase 2 duration */ param_get(_params_handles_tailsitter.front_trans_dur_p2, &v); @@ -98,7 +98,7 @@ Tailsitter::parameters_update() /* vtol duration of a back transition */ param_get(_params_handles_tailsitter.back_trans_dur, &v); - _params_tailsitter.back_trans_dur = math::constrain(v,0.0f,5.0f); + _params_tailsitter.back_trans_dur = math::constrain(v, 0.0f, 5.0f); /* vtol airspeed at which it is ok to switch to fw mode */ param_get(_params_handles_tailsitter.airspeed_trans, &v); @@ -115,7 +115,7 @@ Tailsitter::parameters_update() /* avoid parameters which will lead to zero division in the transition code */ _params_tailsitter.front_trans_dur = math::max(_params_tailsitter.front_trans_dur, _min_front_trans_dur); - if ( _params_tailsitter.airspeed_trans < _params_tailsitter.airspeed_blend_start + 1.0f ) { + if (_params_tailsitter.airspeed_trans < _params_tailsitter.airspeed_blend_start + 1.0f) { _params_tailsitter.airspeed_trans = _params_tailsitter.airspeed_blend_start + 1.0f; } @@ -124,97 +124,115 @@ Tailsitter::parameters_update() void Tailsitter::update_vtol_state() { - parameters_update(); + parameters_update(); - /* simple logic using a two way switch to perform transitions. + /* simple logic using a two way switch to perform transitions. * after flipping the switch the vehicle will start tilting in MC control mode, picking up * forward speed. After the vehicle has picked up enough and sufficient pitch angle the uav will go into FW mode. * For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle. - */ + */ - if (_manual_control_sp->aux1 < 0.0f) { + if (_manual_control_sp->aux1 < 0.0f) { - - switch(_vtol_schedule.flight_mode) { // user switchig to MC mode - case MC_MODE: - break; - case FW_MODE: - _vtol_schedule.flight_mode = TRANSITION_BACK; - _vtol_schedule.transition_start = hrt_absolute_time(); - break; - case TRANSITION_FRONT_P1: - // failsafe into multicopter mode + + switch (_vtol_schedule.flight_mode) { // user switchig to MC mode + case MC_MODE: + break; + + case FW_MODE: + _vtol_schedule.flight_mode = TRANSITION_BACK; + _vtol_schedule.transition_start = hrt_absolute_time(); + break; + + case TRANSITION_FRONT_P1: + // failsafe into multicopter mode + _vtol_schedule.flight_mode = MC_MODE; + break; + + case TRANSITION_FRONT_P2: + // NOT USED + // failsafe into multicopter mode + //_vtol_schedule.flight_mode = MC_MODE; + break; + + case TRANSITION_BACK: + + // check if we have reached pitch angle to switch to MC mode + if (_v_att->pitch >= PITCH_TRANSITION_BACK) { _vtol_schedule.flight_mode = MC_MODE; - break; - case TRANSITION_FRONT_P2: - // NOT USED - // failsafe into multicopter mode - //_vtol_schedule.flight_mode = MC_MODE; - break; - case TRANSITION_BACK: - // check if we have reached pitch angle to switch to MC mode - if (_v_att->pitch >= PITCH_TRANSITION_BACK) { - _vtol_schedule.flight_mode = MC_MODE; - } - break; + } + + break; } + } else { // user switchig to FW 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: - break; - case TRANSITION_FRONT_P1: - // check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode - if ((_airspeed->true_airspeed_m_s >= _params_tailsitter.airspeed_trans) && (_v_att->pitch <= PITCH_TRANSITION_FRONT_P1)) { - _vtol_schedule.flight_mode = FW_MODE; - //_vtol_schedule.transition_start = hrt_absolute_time(); - } - break; - case TRANSITION_FRONT_P2: - // NOT USED - // check if we have reached pitch angle to switch to FW mode - //if (_v_att->pitch <= PITCH_TRANSITION_FRONT_P2) { - // _vtol_schedule.flight_mode = FW_MODE; - //} - break; - case TRANSITION_BACK: - // failsafe into fixed wing mode - _vtol_schedule.flight_mode = FW_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; - /* **LATER*** if pitch is closer to mc (-45>) - * go to transition P1 - */ - break; + case FW_MODE: + break; + + case TRANSITION_FRONT_P1: + + // check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode + if ((_airspeed->true_airspeed_m_s >= _params_tailsitter.airspeed_trans) + && (_v_att->pitch <= PITCH_TRANSITION_FRONT_P1)) { + _vtol_schedule.flight_mode = FW_MODE; + //_vtol_schedule.transition_start = hrt_absolute_time(); + } + + break; + + case TRANSITION_FRONT_P2: + // NOT USED + // check if we have reached pitch angle to switch to FW mode + //if (_v_att->pitch <= PITCH_TRANSITION_FRONT_P2) { + // _vtol_schedule.flight_mode = FW_MODE; + //} + break; + + case TRANSITION_BACK: + // failsafe into fixed wing mode + _vtol_schedule.flight_mode = FW_MODE; + + /* **LATER*** if pitch is closer to mc (-45>) + * go to transition P1 + */ + break; } } // map tailsitter specific control phases to simple control modes - switch(_vtol_schedule.flight_mode) { - case MC_MODE: - _vtol_mode = ROTARY_WING; - _vtol_vehicle_status->vtol_in_trans_mode = false; - _flag_was_in_trans_mode = false; - break; - case FW_MODE: - _vtol_mode = FIXED_WING; - _vtol_vehicle_status->vtol_in_trans_mode = false; - _flag_was_in_trans_mode = false; - break; - case TRANSITION_FRONT_P1: - _vtol_mode = TRANSITION; - _vtol_vehicle_status->vtol_in_trans_mode = true; - case TRANSITION_FRONT_P2: - _vtol_mode = TRANSITION; - _vtol_vehicle_status->vtol_in_trans_mode = true; - case TRANSITION_BACK: - _vtol_mode = TRANSITION; - _vtol_vehicle_status->vtol_in_trans_mode = true; - break; + switch (_vtol_schedule.flight_mode) { + case MC_MODE: + _vtol_mode = ROTARY_WING; + _vtol_vehicle_status->vtol_in_trans_mode = false; + _flag_was_in_trans_mode = false; + break; + + case FW_MODE: + _vtol_mode = FIXED_WING; + _vtol_vehicle_status->vtol_in_trans_mode = false; + _flag_was_in_trans_mode = false; + break; + + case TRANSITION_FRONT_P1: + _vtol_mode = TRANSITION; + _vtol_vehicle_status->vtol_in_trans_mode = true; + + case TRANSITION_FRONT_P2: + _vtol_mode = TRANSITION; + _vtol_vehicle_status->vtol_in_trans_mode = true; + + case TRANSITION_BACK: + _vtol_mode = TRANSITION; + _vtol_vehicle_status->vtol_in_trans_mode = true; + break; } } @@ -223,7 +241,7 @@ void Tailsitter::update_vtol_state() void Tailsitter::update_mc_state() { // set idle speed for rotary wing mode - if (!flag_idle_mc) { + if (!flag_idle_mc) { set_idle_mc(); flag_idle_mc = true; } @@ -264,19 +282,24 @@ void Tailsitter::update_transition_state() if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ - _v_att_sp->pitch_body = _pitch_transition_start - (fabsf(PITCH_TRANSITION_FRONT_P1 -_pitch_transition_start) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.front_trans_dur * 1000000.0f)); - _v_att_sp->pitch_body =math::constrain( _v_att_sp->pitch_body , PITCH_TRANSITION_FRONT_P1-0.2f , _pitch_transition_start ); + _v_att_sp->pitch_body = _pitch_transition_start - (fabsf(PITCH_TRANSITION_FRONT_P1 - _pitch_transition_start) * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f)); + _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , PITCH_TRANSITION_FRONT_P1 - 0.2f , + _pitch_transition_start); /** create time dependant throttle signal higher than in MC and growing untill P2 switch speed reached */ if (_airspeed->true_airspeed_m_s <= _params_tailsitter.airspeed_trans) { - _v_att_sp->thrust = _throttle_transition + (fabsf(THROTTLE_TRANSITION_MAX * _throttle_transition) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.front_trans_dur * 1000000.0f)); - _v_att_sp->thrust = math::constrain(_v_att_sp->thrust , _throttle_transition , (1.0f + THROTTLE_TRANSITION_MAX) * _throttle_transition); + _v_att_sp->thrust = _throttle_transition + (fabsf(THROTTLE_TRANSITION_MAX * _throttle_transition) * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur * 1000000.0f)); + _v_att_sp->thrust = math::constrain(_v_att_sp->thrust , _throttle_transition , + (1.0f + THROTTLE_TRANSITION_MAX) * _throttle_transition); } // disable mc yaw control once the plane has picked up speed if (_airspeed->true_airspeed_m_s > ARSP_YAW_CTRL_DISABLE) { _mc_yaw_weight = 0.0f; - }else { + + } else { _mc_yaw_weight = 1.0f; } @@ -294,27 +317,29 @@ void Tailsitter::update_transition_state() } /** create time dependant pitch angle set point + 0.2 rad overlap over the switch value*/ - if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P2-0.2f) ) { - _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1 - - (fabsf(PITCH_TRANSITION_FRONT_P2 - PITCH_TRANSITION_FRONT_P1) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); - if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_FRONT_P2-0.2f) ) { - _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P2-0.2f; + if (_v_att_sp->pitch_body >= (PITCH_TRANSITION_FRONT_P2 - 0.2f)) { + _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P1 - + (fabsf(PITCH_TRANSITION_FRONT_P2 - PITCH_TRANSITION_FRONT_P1) * (float)hrt_elapsed_time( + &_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); + + if (_v_att_sp->pitch_body <= (PITCH_TRANSITION_FRONT_P2 - 0.2f)) { + _v_att_sp->pitch_body = PITCH_TRANSITION_FRONT_P2 - 0.2f; } } /** start blending MC and FW controls from pitch -45 to pitch -70 for smooth control takeover*/ - + //_mc_roll_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); //_mc_pitch_weight = 1.0f - 1.0f * ((float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.front_trans_dur_p2 * 1000000.0f)); - + _mc_roll_weight = 0.0f; _mc_pitch_weight = 0.0f; /** keep yaw disabled */ _mc_yaw_weight = 0.0f; - + } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { @@ -323,20 +348,23 @@ void Tailsitter::update_transition_state() flag_idle_mc = true; } - /** create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value*/ - _v_att_sp->pitch_body = M_PI_2_F + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f ) * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); - _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , -2.0f , PITCH_TRANSITION_BACK+0.2f); + /** create time dependant pitch angle set point stating at -pi/2 + 0.2 rad overlap over the switch value*/ + _v_att_sp->pitch_body = M_PI_2_F + _pitch_transition_start + fabsf(PITCH_TRANSITION_BACK + 1.57f) * + (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_tailsitter.back_trans_dur * 1000000.0f); + _v_att_sp->pitch_body = math::constrain(_v_att_sp->pitch_body , -2.0f , PITCH_TRANSITION_BACK + 0.2f); // throttle value is decreesed - _v_att_sp->thrust = _throttle_transition*0.9f; - + _v_att_sp->thrust = _throttle_transition * 0.9f; + /** keep yaw disabled */ _mc_yaw_weight = 0.0f; /** smoothly move control weight to MC */ - _mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); - _mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tailsitter.back_trans_dur * 1000000.0f); - + _mc_roll_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / + (_params_tailsitter.back_trans_dur * 1000000.0f); + _mc_pitch_weight = 1.0f * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / + (_params_tailsitter.back_trans_dur * 1000000.0f); + } @@ -353,8 +381,8 @@ void Tailsitter::update_transition_state() _v_att_sp->yaw_body = _yaw_transition; _v_att_sp->R_valid = true; - math::Matrix<3,3> R_sp; - R_sp.from_euler(_v_att_sp->roll_body,_v_att_sp->pitch_body,_v_att_sp->yaw_body); + math::Matrix<3, 3> R_sp; + R_sp.from_euler(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body); memcpy(&_v_att_sp->R_body[0], R_sp.data, sizeof(_v_att_sp->R_body)); math::Quaternion q_sp; @@ -425,49 +453,69 @@ void Tailsitter::scale_mc_output() */ void Tailsitter::fill_actuator_outputs() { - switch(_vtol_mode) { - case ROTARY_WING: - _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]; + switch (_vtol_mode) { + case ROTARY_WING: + _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 { - // 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] =0; //pitch elevon - } - break; - case FIXED_WING: - // 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: - // in transition engines are mixed by weight (BACK TRANSITION ONLY) - _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]; + if (_params->elevons_mc_lock == 1) { + _actuators_out_1->control[0] = 0; + _actuators_out_1->control[1] = 0; + } else { // 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_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight); - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; - // **LATER** + (_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_THROTTLE] = _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; - break; - case EXTERNAL: - // not yet implemented, we are switching brute force at the moment - break; + _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] = 0; //pitch elevon + } + + break; + + case FIXED_WING: + // 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: + // in transition engines are mixed by weight (BACK TRANSITION ONLY) + _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]; + + // 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_fw_in->control[actuator_controls_s::INDEX_ROLL] + * (1 - _mc_roll_weight); + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = + _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; + // **LATER** + (_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_THROTTLE] = + _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; + break; + + case EXTERNAL: + // not yet implemented, we are switching brute force at the moment + break; } } diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index 1b2caca676..de5ab0ba3e 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -31,13 +31,13 @@ * ****************************************************************************/ - /** - * @file tiltrotor.h - * - * @author Roman Bapst - * @author David Vorsin - * - */ +/** +* @file tiltrotor.h +* +* @author Roman Bapst +* @author David Vorsin +* +*/ #ifndef TAILSITTER_H #define TAILSITTER_H @@ -114,7 +114,7 @@ private: struct { vtol_mode flight_mode; /**< vtol flight mode, defined by enum vtol_mode */ hrt_abstime transition_start; /**< absoulte time at which front transition started */ - }_vtol_schedule; + } _vtol_schedule; float _airspeed_tot; /** speed estimation for propwash controlled surfaces */ float _roll_weight_mc; /**< multicopter desired roll moment weight */ @@ -125,7 +125,7 @@ private: float _min_front_trans_dur; /**< min possible time in which rotors are rotated into the first position */ - /** should this anouncement stay? **/ + /** should this anouncement stay? **/ perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ diff --git a/src/modules/vtol_att_control/tailsitter_params.c b/src/modules/vtol_att_control/tailsitter_params.c index 52801d9e8c..af38b55693 100644 --- a/src/modules/vtol_att_control/tailsitter_params.c +++ b/src/modules/vtol_att_control/tailsitter_params.c @@ -50,7 +50,7 @@ * @min 0.1 * @max 2 * @group VTOL Attitude Control - + PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f);*/ diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 24ff7ece2a..3890a71843 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -306,6 +306,7 @@ void Tiltrotor::update_transition_state() _throttle_transition = _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; _flag_was_in_trans_mode = true; } + if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { // for the first part of the transition the rear rotors are enabled if (_rear_motors != ENABLED) { 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 80bb62eccd..5ab0d01b1c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -512,6 +512,7 @@ void VtolAttitudeControl::publish_att_sp() if (_v_att_sp_pub != nullptr) { /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_pub, &_v_att_sp); + } else { /* advertise and publish */ _v_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); @@ -740,7 +741,11 @@ VtolAttitudeControl::start() SCHED_DEFAULT, SCHED_PRIORITY_MAX - 10, 2048, +<<<<<<< HEAD (px4_main_t)&VtolAttitudeControl::task_main_trampoline, +======= + (main_t)&VtolAttitudeControl::task_main_trampoline, +>>>>>>> fixed code style nullptr); if (_control_task < 0) { 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 c7c51247ae..ccf644142c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -107,27 +107,28 @@ public: int start(); /* start the task and return OK on success */ bool is_fixed_wing_requested(); - struct vehicle_attitude_s *get_att() {return &_v_att;} - struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;} - struct mc_virtual_attitude_setpoint_s *get_mc_virtual_att_sp () {return &_mc_virtual_att_sp;} - struct fw_virtual_attitude_setpoint_s *get_fw_virtual_att_sp () {return &_fw_virtual_att_sp;} - struct vehicle_rates_setpoint_s *get_rates_sp() {return &_v_rates_sp;} - struct vehicle_rates_setpoint_s *get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;} - struct vehicle_rates_setpoint_s *get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;} - struct manual_control_setpoint_s *get_manual_control_sp() {return &_manual_control_sp;} - struct vehicle_control_mode_s *get_control_mode() {return &_v_control_mode;} - struct vtol_vehicle_status_s *get_vehicle_status() {return &_vtol_vehicle_status;} - struct actuator_controls_s *get_actuators_out0() {return &_actuators_out_0;} - struct actuator_controls_s *get_actuators_out1() {return &_actuators_out_1;} - struct actuator_controls_s *get_actuators_mc_in() {return &_actuators_mc_in;} - struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;} - struct actuator_armed_s *get_armed() {return &_armed;} - struct vehicle_local_position_s *get_local_pos() {return &_local_pos;} - struct airspeed_s *get_airspeed() {return &_airspeed;} - struct battery_status_s *get_batt_status() {return &_batt_status;} + struct vehicle_attitude_s *get_att() {return &_v_att;} + struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;} + struct mc_virtual_attitude_setpoint_s *get_mc_virtual_att_sp() {return &_mc_virtual_att_sp;} + struct fw_virtual_attitude_setpoint_s *get_fw_virtual_att_sp() {return &_fw_virtual_att_sp;} + struct vehicle_rates_setpoint_s *get_rates_sp() {return &_v_rates_sp;} + struct vehicle_rates_setpoint_s *get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;} + struct vehicle_rates_setpoint_s *get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;} + struct manual_control_setpoint_s *get_manual_control_sp() {return &_manual_control_sp;} + struct vehicle_control_mode_s *get_control_mode() {return &_v_control_mode;} + struct vtol_vehicle_status_s *get_vehicle_status() {return &_vtol_vehicle_status;} + struct actuator_controls_s *get_actuators_out0() {return &_actuators_out_0;} + struct actuator_controls_s *get_actuators_out1() {return &_actuators_out_1;} + struct actuator_controls_s *get_actuators_mc_in() {return &_actuators_mc_in;} + struct actuator_controls_s *get_actuators_fw_in() {return &_actuators_fw_in;} + struct actuator_armed_s *get_armed() {return &_armed;} + struct vehicle_local_position_s *get_local_pos() {return &_local_pos;} + struct airspeed_s *get_airspeed() {return &_airspeed;} + struct battery_status_s *get_batt_status() {return &_batt_status;} struct Params *get_params() {return &_params;} + private: //******************flags & handlers****************************************************** bool _task_should_exit;