From 228a54fd51fc32c2e8599b1a60e7ac5c753cd0c1 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 18 Feb 2016 11:14:03 +0100 Subject: [PATCH] - fixed bad pitch setpoint in fw pos controller for tailsitter - created enum for vtol types - minor cleanup and fixes --- .../fw_pos_control_l1_main.cpp | 19 ++++++++++++++++++- .../vtol_att_control_main.cpp | 6 +++--- src/modules/vtol_att_control/vtol_type.cpp | 1 - src/modules/vtol_att_control/vtol_type.h | 6 ++++++ 4 files changed, 27 insertions(+), 5 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index ea127aca9f..fb18df5dc6 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -98,6 +98,7 @@ #include "mtecs/mTecs.h" #include #include +#include static int _control_task = -1; /**< task handle for sensor task */ #define HDG_HOLD_DIST_NEXT 3000.0f // initial distance of waypoint in front of plane in heading hold mode @@ -299,6 +300,7 @@ private: float land_flare_pitch_max_deg; int land_use_terrain_estimate; float land_airspeed_scale; + int vtol_type; } _parameters; /**< local copies of interesting parameters */ @@ -349,6 +351,7 @@ private: param_t land_flare_pitch_max_deg; param_t land_use_terrain_estimate; param_t land_airspeed_scale; + param_t vtol_type; } _parameter_handles; /**< handles for interesting parameters */ @@ -628,6 +631,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.heightrate_p = param_find("FW_T_HRATE_P"); _parameter_handles.heightrate_ff = param_find("FW_T_HRATE_FF"); _parameter_handles.speedrate_p = param_find("FW_T_SRATE_P"); + _parameter_handles.vtol_type = param_find("VT_TYPE"); /* fetch initial parameter values */ parameters_update(); @@ -716,6 +720,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg)); param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate)); param_get(_parameter_handles.land_airspeed_scale, &(_parameters.land_airspeed_scale)); + param_get(_parameter_handles.vtol_type, &(_parameters.vtol_type)); _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); @@ -2288,7 +2293,19 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM)); /* Using tecs library */ - _tecs.update_pitch_throttle(_R_nb, _pitch, altitude, alt_sp, v_sp, + float pitch_for_tecs = _pitch; + + // if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset + // between multirotor and fixed wing flight + if (_parameters.vtol_type == vtol_type::TAILSITTER && _vehicle_status.is_vtol) { + math::Matrix<3,3> R_offset; + R_offset.from_euler(0, M_PI_2_F, 0); + math::Matrix<3,3> R_fixed_wing = _R_nb * R_offset; + math::Vector<3> euler = R_fixed_wing.to_euler(); + pitch_for_tecs = euler(1); + } + + _tecs.update_pitch_throttle(_R_nb, pitch_for_tecs, altitude, alt_sp, v_sp, _ctrl_state.airspeed, eas2tas, climbout_mode, climbout_pitch_min_rad, throttle_min, throttle_max, throttle_cruise, 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 615c41911f..c4ffac913a 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -134,15 +134,15 @@ VtolAttitudeControl::VtolAttitudeControl() : /* fetch initial parameter values */ parameters_update(); - if (_params.vtol_type == 0) { + if (_params.vtol_type == vtol_type::TAILSITTER) { _tailsitter = new Tailsitter(this); _vtol_type = _tailsitter; - } else if (_params.vtol_type == 1) { + } else if (_params.vtol_type == vtol_type::TILTROTOR) { _tiltrotor = new Tiltrotor(this); _vtol_type = _tiltrotor; - } else if (_params.vtol_type == 2) { + } else if (_params.vtol_type == vtol_type::STANDARD) { _standard = new Standard(this); _vtol_type = _standard; diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 5230345b4f..681c782815 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -150,7 +150,6 @@ void VtolType::update_fw_state() { // copy virtual attitude setpoint to real attitude setpoint memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); - _mc_roll_weight = 0.0f; _mc_pitch_weight = 0.0f; _mc_yaw_weight = 0.0f; diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 8df24880a8..89038ad633 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -68,6 +68,12 @@ enum mode { EXTERNAL }; +enum vtol_type { + TAILSITTER = 0, + TILTROTOR, + STANDARD +}; + class VtolAttitudeControl; class VtolType