From 137a2444a8824e5da1eabd59a7a7e78b218824f3 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 11 Jan 2018 15:44:35 -0500 Subject: [PATCH] VTOL standard respect VT_ELEV_MC_LOCK --- src/modules/vtol_att_control/standard.cpp | 31 +++++++++++++------ src/modules/vtol_att_control/tailsitter.cpp | 9 +----- src/modules/vtol_att_control/tailsitter.h | 3 -- src/modules/vtol_att_control/tiltrotor.cpp | 5 --- src/modules/vtol_att_control/tiltrotor.h | 2 -- .../vtol_att_control_main.cpp | 2 +- .../vtol_att_control_params.c | 2 +- src/modules/vtol_att_control/vtol_type.h | 2 +- 8 files changed, 26 insertions(+), 30 deletions(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 998c4bb269..54000ad79f 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -517,13 +517,12 @@ void Standard::fill_actuator_outputs() // fixed wing controls _actuators_out_1->timestamp = _actuators_fw_in->timestamp; - if (_vtol_schedule.flight_mode != MC_MODE) { - - //roll + // roll _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; - //pitch + + // pitch _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim; // yaw @@ -534,11 +533,25 @@ void Standard::fill_actuator_outputs() } else { - // zero outputs when inactive - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = 0.0f; - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _params->fw_pitch_trim; - _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = 0.0f; - _actuators_out_1->control[actuator_controls_s::INDEX_AIRBRAKES] = 0.0f; + if (_params->elevons_mc_lock) { + // zero outputs when inactive + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = 0.0f; + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _params->fw_pitch_trim; + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = 0.0f; + _actuators_out_1->control[actuator_controls_s::INDEX_AIRBRAKES] = 0.0f; + + } else { + // roll + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = + -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; + + // pitch + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = + _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim; + + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = 0.0f; + _actuators_out_1->control[actuator_controls_s::INDEX_AIRBRAKES] = 0.0f; + } } // set the fixed wing throttle control diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 11b6953441..df3014cad4 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -69,8 +69,6 @@ Tailsitter::Tailsitter(VtolAttitudeControl *attc) : _params_handles_tailsitter.back_trans_dur = param_find("VT_B_TRANS_DUR"); _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() @@ -82,7 +80,6 @@ void Tailsitter::parameters_update() { float v; - int l; /* vtol duration of a front transition */ param_get(_params_handles_tailsitter.front_trans_dur, &v); @@ -104,10 +101,6 @@ Tailsitter::parameters_update() param_get(_params_handles_tailsitter.airspeed_blend_start, &v); _params_tailsitter.airspeed_blend_start = v; - /* vtol lock elevons in multicopter */ - param_get(_params_handles_tailsitter.elevons_mc_lock, &l); - _params_tailsitter.elevons_mc_lock = l; - /* 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); @@ -389,7 +382,7 @@ void Tailsitter::fill_actuator_outputs() _actuators_out_1->timestamp = _actuators_mc_in->timestamp; - if (_params->elevons_mc_lock == 1) { + if (_params->elevons_mc_lock) { _actuators_out_1->control[0] = 0; _actuators_out_1->control[1] = 0; diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index 6c6146eb37..35aa0ee8d0 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -69,8 +69,6 @@ private: float back_trans_dur; /**< duration of back transition */ float airspeed_trans; /**< airspeed at which we switch to fw mode after transition */ float airspeed_blend_start; /**< airspeed at which we start blending mc/fw controls */ - int elevons_mc_lock; /**< lock elevons in multicopter mode */ - } _params_tailsitter; struct { @@ -79,7 +77,6 @@ private: param_t back_trans_dur; param_t airspeed_trans; param_t airspeed_blend_start; - param_t elevons_mc_lock; } _params_handles_tailsitter; diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 4ef89ced8e..e06676d95f 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -66,7 +66,6 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : _params_handles_tiltrotor.tilt_fw = param_find("VT_TILT_FW"); _params_handles_tiltrotor.airspeed_trans = param_find("VT_ARSP_TRANS"); _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_OFFID"); _params_handles_tiltrotor.airspeed_disabled = param_find("FW_ARSP_MODE"); @@ -117,10 +116,6 @@ Tiltrotor::parameters_update() param_get(_params_handles_tiltrotor.airspeed_blend_start, &v); _params_tiltrotor.airspeed_blend_start = v; - /* vtol lock elevons in multicopter */ - param_get(_params_handles_tiltrotor.elevons_mc_lock, &l); - _params_tiltrotor.elevons_mc_lock = l; - /* vtol front transition phase 2 duration */ param_get(_params_handles_tiltrotor.front_trans_dur_p2, &v); _params_tiltrotor.front_trans_dur_p2 = v; diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index 8f39b20137..672602b928 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -69,7 +69,6 @@ private: float tilt_fw; /**< actuator value corresponding to fw tilt */ float airspeed_trans; /**< airspeed at which we switch to fw mode after transition */ float airspeed_blend_start; /**< airspeed at which we start blending mc/fw controls */ - int32_t elevons_mc_lock; /**< lock elevons in multicopter mode */ float front_trans_dur_p2; int32_t fw_motors_off; /**< bitmask of all motors that should be off in fixed wing mode */ int32_t airspeed_disabled; @@ -85,7 +84,6 @@ private: param_t tilt_fw; param_t airspeed_trans; param_t airspeed_blend_start; - param_t elevons_mc_lock; param_t front_trans_dur_p2; param_t fw_motors_off; param_t airspeed_disabled; 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 6a87bd9166..3588905900 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -448,7 +448,7 @@ VtolAttitudeControl::parameters_update() /* vtol lock elevons in multicopter */ param_get(_params_handles.elevons_mc_lock, &l); - _params.elevons_mc_lock = l; + _params.elevons_mc_lock = (l == 1); /* minimum relative altitude for FW mode (QuadChute) */ param_get(_params_handles.fw_min_alt, &v); 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 c1b44dd305..65ec471026 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -107,7 +107,7 @@ PARAM_DEFINE_INT32(VT_TYPE, 0); * @boolean * @group VTOL Attitude Control */ -PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK, 0); +PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK, 1); /** * Duration of a front transition diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 20596f64e4..1aa7d18e8f 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -51,7 +51,7 @@ struct Params { int32_t vtol_motor_count; // number of motors float fw_pitch_trim; // trim for neutral elevon position in fw mode int32_t vtol_type; - int32_t elevons_mc_lock; // lock elevons in multicopter mode + bool elevons_mc_lock; // lock elevons in multicopter mode float fw_min_alt; // minimum relative altitude for FW mode (QuadChute) float fw_alt_err; // maximum negative altitude error for FW mode (Adaptive QuadChute) float fw_qc_max_pitch; // maximum pitch angle FW mode (QuadChute)