From 51ef8541739ffb864fd4797874df15fb5314fac0 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 16 Sep 2015 01:00:44 +0200 Subject: [PATCH] extracted runway takeoff logic into external class --- src/lib/runway_takeoff/RunwayTakeoff.cpp | 208 ++++++++++++++++++ src/lib/runway_takeoff/RunwayTakeoff.h | 115 ++++++++++ src/lib/runway_takeoff/module.mk | 41 ++++ .../runway_takeoff/runway_takeoff_params.c | 100 +++++++++ .../fw_pos_control_l1_main.cpp | 155 +++++-------- .../fw_pos_control_l1_params.c | 55 ----- 6 files changed, 521 insertions(+), 153 deletions(-) create mode 100644 src/lib/runway_takeoff/RunwayTakeoff.cpp create mode 100644 src/lib/runway_takeoff/RunwayTakeoff.h create mode 100644 src/lib/runway_takeoff/module.mk create mode 100644 src/lib/runway_takeoff/runway_takeoff_params.c diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp new file mode 100644 index 0000000000..e1814cecc1 --- /dev/null +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -0,0 +1,208 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file RunwayTakeoff.cpp + * Runway takeoff handling for fixed-wing UAVs with steerable wheels. + * + * @author Roman Bapst + * @author Andreas Antener + */ + +#include +#include +#include + +#include "RunwayTakeoff.h" +#include +#include +#include +#include + +namespace runwaytakeoff +{ + +RunwayTakeoff::RunwayTakeoff() : + SuperBlock(NULL, "RWTO"), + _state(), + _initialized(false), + _initialized_time(0), + _init_yaw(0), + _climbout(false), + _min_airspeed_scaling(1.3f), + _runway_takeoff_enabled(this, "TKOFF"), + _runway_takeoff_heading(this, "HDG"), + _runway_takeoff_nav_alt(this, "NAV_ALT"), + _runway_takeoff_throttle(this, "MAX_THR"), + _runway_takeoff_pitch_sp(this, "PSP"), + _airspeed_min(this, "FW_AIRSPD_MIN", false), + _climbout_diff(this, "FW_CLMBOUT_DIFF", false) +{ + + updateParams(); +} + +RunwayTakeoff::~RunwayTakeoff() +{ + +} + +void RunwayTakeoff::init(float yaw) +{ + _init_yaw = yaw; + _initialized = true; + _state = RunwayTakeoffState::THROTTLE_RAMP; + _initialized_time = hrt_absolute_time(); +} + +void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) +{ + if (_climbout_diff.get() > 0.0001f && alt_agl < _climbout_diff.get()) { + _climbout = true; + } + + else { + _climbout = false; + } + + switch (_state) { + case RunwayTakeoffState::THROTTLE_RAMP: + if (hrt_elapsed_time(&_initialized_time) > 1e6) { + _state = RunwayTakeoffState::CLAMPED_TO_RUNWAY; + } + + break; + + case RunwayTakeoffState::CLAMPED_TO_RUNWAY: + if (airspeed > _airspeed_min.get() * _min_airspeed_scaling) { + _state = RunwayTakeoffState::TAKEOFF; + mavlink_log_info(mavlink_fd, "#audio: Takeoff airspeed reached"); + } + + break; + + case RunwayTakeoffState::TAKEOFF: + if (alt_agl > math::max(_runway_takeoff_nav_alt.get(), _climbout_diff.get())) { + _state = RunwayTakeoffState::FLY; + mavlink_log_info(mavlink_fd, "#audio: Navigating to waypoint"); + } + + break; + + default: + return; + } +} + +bool RunwayTakeoff::controlYaw() +{ + // keep controlling yaw with rudder until we have enough ground clearance + return _state < RunwayTakeoffState::FLY; +} + +float RunwayTakeoff::getPitch(float tecsPitch) +{ + if (_state <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) { + return math::radians(_runway_takeoff_pitch_sp.get()); + } + + return tecsPitch; +} + +float RunwayTakeoff::getRoll(float navigatorRoll) +{ + if (_state < RunwayTakeoffState::FLY) { + return 0.0f; + } + + return navigatorRoll; +} + +float RunwayTakeoff::getYaw(float navigatorYaw) +{ + if (_state < RunwayTakeoffState::FLY) { + if (_runway_takeoff_heading.get() == 0) { + // fix heading in the direction the airframe points + return _init_yaw; + + } else if (_runway_takeoff_heading.get() == 1) { + // or head into the direction of the takeoff waypoint + // XXX this needs a check if the deviation from actual heading is too big (else we do a full throttle wheel turn on the ground) + return navigatorYaw; + } + } + + return navigatorYaw; +} + +float RunwayTakeoff::getThrottle(float tecsThrottle) +{ + switch (_state) { + case RunwayTakeoffState::THROTTLE_RAMP: { + float throttle = hrt_elapsed_time(&_initialized_time) / (float)2000000 * + _runway_takeoff_throttle.get(); + return throttle < _runway_takeoff_throttle.get() ? + throttle : + _runway_takeoff_throttle.get(); + } + + case RunwayTakeoffState::CLAMPED_TO_RUNWAY: + return _runway_takeoff_throttle.get(); + + default: + return tecsThrottle; + } +} + +bool RunwayTakeoff::resetIntegrators() +{ + return _state <= RunwayTakeoffState::TAKEOFF; +} + +float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) +{ + if (_climbout) { + return math::max(sp_min, climbout_min); + } + + else { + return min; + } +} + +void RunwayTakeoff::reset() +{ + _initialized = false; + _state = RunwayTakeoffState::THROTTLE_RAMP; +} + +} diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h new file mode 100644 index 0000000000..7a4dd17f50 --- /dev/null +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file RunwayTakeoff.h + * Runway takeoff handling for fixed-wing UAVs with steerable wheels. + * + * @author Roman Bapst + * @author Andreas Antener + */ + +#ifndef RUNWAYTAKEOFF_H +#define RUNWAYTAKEOFF_H + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace runwaytakeoff +{ + +enum RunwayTakeoffState { + THROTTLE_RAMP = 0, /**< */ + CLAMPED_TO_RUNWAY = 1, /**< */ + TAKEOFF = 2, /**< */ + FLY = 3 /**< */ +}; + +class __EXPORT RunwayTakeoff : public control::SuperBlock +{ +public: + RunwayTakeoff(); + ~RunwayTakeoff(); + + void init(float yaw); + void update(float airspeed, float alt_agl, int mavlink_fd); + + RunwayTakeoffState getState() { return _state; }; + bool isInitialized() { return _initialized; }; + + bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); }; + + bool controlYaw(); + bool climbout() { return _climbout; }; + float getPitch(float tecsPitch); + float getRoll(float navigatorRoll); + float getYaw(float navigatorYaw); + float getThrottle(float tecsThrottle); + bool resetIntegrators(); + float getMinAirspeedScaling() { return _min_airspeed_scaling; }; + float getMinPitch(float sp_min, float climbout_min, float min); + + void reset(); + +protected: +private: + /** state variables **/ + RunwayTakeoffState _state; + bool _initialized; + hrt_abstime _initialized_time; + float _init_yaw; + bool _climbout; + + /** magic values **/ + float _min_airspeed_scaling; + + /** parameters **/ + control::BlockParamInt _runway_takeoff_enabled; + control::BlockParamInt _runway_takeoff_heading; + control::BlockParamFloat _runway_takeoff_nav_alt; + control::BlockParamFloat _runway_takeoff_throttle; + control::BlockParamFloat _runway_takeoff_pitch_sp; + control::BlockParamFloat _airspeed_min; + control::BlockParamFloat _climbout_diff; + +}; + +} + +#endif // RUNWAYTAKEOFF_H diff --git a/src/lib/runway_takeoff/module.mk b/src/lib/runway_takeoff/module.mk new file mode 100644 index 0000000000..95b9aea53e --- /dev/null +++ b/src/lib/runway_takeoff/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013, 2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# RunwayTakeoff Library +# + +SRCS = RunwayTakeoff.cpp \ + runway_takeoff_params.c + +MAXOPTIMIZATION = -Os diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c new file mode 100644 index 0000000000..6ccc21e76a --- /dev/null +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file runway_takeoff_params.c + * + * Parameters for runway takeoff + * + * @author Andreas Antener + */ + +#include + +#include + +/** + * Enable or disable runway takeoff with landing gear + * + * 0: disabled, 1: enabled + * + * @min 0 + * @max 1 + * @group Runway Takeoff + */ +PARAM_DEFINE_INT32(RWTO_TKOFF, 0); + +/** + * Specifies which heading should be held during runnway takeoff. + * + * 0: airframe heading, 1: heading towards takeoff waypoint + * + * @min 0 + * @max 1 + * @group Runway Takeoff + */ +PARAM_DEFINE_INT32(RWTO_HDG, 0); + +/** + * Altitude AGL at which navigation towards takeoff waypoint starts. + * Until RWTO_NAV_ALT is reached the plane is held level and only + * rudder is used to keep the heading (see RWTO_HDG). If this is lower + * than FW_CLMBOUT_DIFF, FW_CLMBOUT_DIFF is used instead. + * + * @min 0.0 + * @max 100.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_NAV_ALT, 5.0); + +/** + * Max throttle during runway takeoff. + * (Can be used to test taxi on runway) + * + * @min 0.0 + * @max 1.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0); + +/** + * Pitch setpoint during runway takeoff. + * A taildragger with stearable wheel might need to pitch up + * a little to keep it's wheel on the ground before airspeed + * to takeoff is reached. + * + * @min 0.0 + * @max 20.0 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); 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 16ed16ad10..34f414f0d6 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 @@ -95,6 +95,7 @@ #include "landingslope.h" #include "mtecs/mTecs.h" #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 @@ -195,12 +196,10 @@ private: bool land_useterrain; bool _was_in_air; /**< indicated wether the plane was in the air in the previous interation*/ - bool _takeoff_initialized; - bool _takeoff_on_runway; /**< true as long as runway takeoff is in progress */ - float _runway_takeoff_hdg_hold; - hrt_abstime _takeoff_runway_start; /**< time at which we start takeoff on runway */ hrt_abstime _time_went_in_air; /**< time at which the plane went in the air */ + runwaytakeoff::RunwayTakeoff _runway_takeoff; + /* takeoff/launch states */ LaunchDetectionResult launch_detection_state; @@ -282,12 +281,6 @@ private: float land_flare_pitch_max_deg; int land_use_terrain_estimate; - int do_runway_takeoff; - int runway_takeoff_heading; - float runway_takeoff_nav_alt; - float runway_takeoff_throttle; - float runway_takeoff_pitch_sp; - } _parameters; /**< local copies of interesting parameters */ struct { @@ -336,12 +329,6 @@ private: param_t land_flare_pitch_max_deg; param_t land_use_terrain_estimate; - param_t do_runway_takeoff; - param_t runway_takeoff_heading; - param_t runway_takeoff_nav_alt; - param_t runway_takeoff_throttle; - param_t runway_takeoff_pitch_sp; - } _parameter_handles; /**< handles for interesting parameters */ @@ -541,11 +528,8 @@ FixedwingPositionControl::FixedwingPositionControl() : land_onslope(false), land_useterrain(false), _was_in_air(false), - _takeoff_initialized(false), - _takeoff_on_runway(false), - _runway_takeoff_hdg_hold(0), - _takeoff_runway_start(0), _time_went_in_air(0), + _runway_takeoff(), launch_detection_state(LAUNCHDETECTION_RES_NONE), last_manual(false), landingslope(), @@ -588,12 +572,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX"); _parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER"); - _parameter_handles.do_runway_takeoff = param_find("FW_RNW_TKOFF"); - _parameter_handles.runway_takeoff_heading = param_find("FW_RNW_HDG"); - _parameter_handles.runway_takeoff_nav_alt = param_find("FW_RNW_NAV_ALT"); - _parameter_handles.runway_takeoff_throttle = param_find("FW_RNW_MAX_THR"); - _parameter_handles.runway_takeoff_pitch_sp = param_find("FW_RNW_PSP"); - _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); @@ -697,12 +675,6 @@ 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.do_runway_takeoff, &(_parameters.do_runway_takeoff)); - param_get(_parameter_handles.runway_takeoff_heading, &(_parameters.runway_takeoff_heading)); - param_get(_parameter_handles.runway_takeoff_nav_alt, &(_parameters.runway_takeoff_nav_alt)); - param_get(_parameter_handles.runway_takeoff_throttle, &(_parameters.runway_takeoff_throttle)); - param_get(_parameter_handles.runway_takeoff_pitch_sp, &(_parameters.runway_takeoff_pitch_sp)); - _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); @@ -752,6 +724,8 @@ FixedwingPositionControl::parameters_update() /* Update the mTecs */ _mTecs.updateParams(); + _runway_takeoff.updateParams(); + return OK; } @@ -1386,72 +1360,54 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { - if (_parameters.do_runway_takeoff == true) { - if (!_takeoff_initialized) { - _runway_takeoff_hdg_hold = _att.yaw; - _takeoff_runway_start = hrt_absolute_time(); - _takeoff_initialized = true; - _takeoff_on_runway = true; + if (_runway_takeoff.runwayTakeoffEnabled()) { + if (!_runway_takeoff.isInitialized()) { + _runway_takeoff.init(_att.yaw); + /* need this already before takeoff is detected * doesn't matter if it gets reset when takeoff is detected eventually */ _takeoff_ground_alt = _global_pos.alt; - } - - if (_airspeed.true_airspeed_m_s < _parameters.airspeed_min * 1.2f) { // XXX takeoff: magic value - _takeoff_on_runway = true; - _att_sp.pitch_body = math::radians(_parameters.runway_takeoff_pitch_sp); - } else { - // min airspeed reached, let tecs control it - _takeoff_on_runway = false; - float takeoff_pitch_max_deg = _parameters.pitch_limit_max; - float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); - - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, - calculate_target_airspeed(1.3f * _parameters.airspeed_min), // XXX takeoff: magic value - eas2tas, - math::radians(_parameters.pitch_limit_min), - takeoff_pitch_max_rad, - _parameters.throttle_min, - _parameters.throttle_max, // XXX should we also set runway_takeoff_throttle here? - _parameters.throttle_cruise, - true, - math::max(math::radians(_pos_sp_triplet.current.pitch_min), - math::radians(10.0f)), - _global_pos.alt, - ground_speed, - tecs_status_s::TECS_MODE_TAKEOFF, - takeoff_pitch_max_deg != _parameters.pitch_limit_max); - - _att_sp.pitch_body = _tecs.get_pitch_demand(); + mavlink_log_info(_mavlink_fd, "#audio: Takeoff on runway"); } // update navigation _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos); - //PX4_WARN("talt: %f", (double)terrain_alt); - if (_global_pos.alt - terrain_alt > _parameters.runway_takeoff_nav_alt) { - // XXX once in here we should maybe not fall out again (although it shouldn't matter when we're climbing) - // start to navigate to takeoff waypoint as soon as we are high enough in the air - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); - } else { - // for takeoff we want heading control with rudder, roll and pitch stabilization to zero - // throttle should be ramped up to max - _att_sp.roll_body = 0.0f; - _att_sp.fw_control_yaw = true; // tell attitude controller he should control yaw directly - - if (_parameters.runway_takeoff_heading == 0) { - // fix heading in the direction the airframe points - _att_sp.yaw_body = _runway_takeoff_hdg_hold; - //PX4_WARN("using initial heading: %.4f", (double)_att_sp.yaw_body); - } else if (_parameters.runway_takeoff_heading == 1) { - // or head into the direction of the takeoff waypoint - // XXX this needs a check if the deviation from actual heading is too big (else we do a full throttle wheel turn on the ground) - _att_sp.yaw_body = _l1_control.nav_bearing(); // XXX check if this is really the actual bearing - //PX4_WARN("using nav heading: %.4f", (double)_att_sp.yaw_body); - } - } + // update runway takeoff helper + _runway_takeoff.update( + _airspeed.true_airspeed_m_s, + _global_pos.alt - terrain_alt, + _mavlink_fd); + + // update tecs + float takeoff_pitch_max_deg = _parameters.pitch_limit_max; + float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg); + + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed( + _runway_takeoff.getMinAirspeedScaling() * _parameters.airspeed_min), + eas2tas, + math::radians(_parameters.pitch_limit_min), + takeoff_pitch_max_rad, + _parameters.throttle_min, + _parameters.throttle_max, // XXX should we also set runway_takeoff_throttle here? + _parameters.throttle_cruise, + _runway_takeoff.climbout(), + math::radians(_runway_takeoff.getMinPitch( + _pos_sp_triplet.current.pitch_min, + 10.0f, + _parameters.pitch_limit_min)), + _global_pos.alt, + ground_speed, + tecs_status_s::TECS_MODE_TAKEOFF, + takeoff_pitch_max_deg != _parameters.pitch_limit_max); + + // assign values + _att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.nav_roll()); + _att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing()); + _att_sp.fw_control_yaw = _runway_takeoff.controlYaw(); // tell attitude controller if he should control yaw directly + _att_sp.pitch_body = _runway_takeoff.getPitch(_tecs.get_pitch_demand()); } else { @@ -1553,6 +1509,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } /* reset takeoff/launch state */ + // FIXME: reset on arm/disarm cycle and mode switch if (pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { reset_takeoff_state(); } @@ -1754,23 +1711,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { /* Set thrust to 0 to minimize damage */ _att_sp.thrust = 0.0f; + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && - !_parameters.do_runway_takeoff) { + !_runway_takeoff.runwayTakeoffEnabled()) { /* making sure again that the correct thrust is used, * without depending on library calls for safety reasons */ _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); - } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto + + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && - _parameters.do_runway_takeoff && _takeoff_on_runway == true) { - /* Ramp-up thrust and stay at max */ - _att_sp.thrust = _att_sp.thrust < _parameters.runway_takeoff_throttle ? - hrt_elapsed_time(&_takeoff_runway_start) / (float)2000000 : - _parameters.runway_takeoff_throttle; + _runway_takeoff.runwayTakeoffEnabled()) { + _att_sp.thrust = _runway_takeoff.getThrottle( + math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : + _tecs.get_throttle_demand(), throttle_max)); + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { _att_sp.thrust = 0.0f; + } else { /* Copy thrust and pitch values from tecs */ if (_vehicle_status.condition_landed && @@ -1950,8 +1910,7 @@ void FixedwingPositionControl::reset_takeoff_state() { launch_detection_state = LAUNCHDETECTION_RES_NONE; launchDetector.reset(); - _takeoff_initialized = false; - _takeoff_on_runway = false; + _runway_takeoff.reset(); } void FixedwingPositionControl::reset_landing_state() diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 76b22f8350..5870039c24 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -426,61 +426,6 @@ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); */ PARAM_DEFINE_INT32(FW_LND_USETER, 0); -/** - * Enable or disable runway takeoff with landing gear - * - * 0: disabled, 1: enabled - * - * @min 0 - * @max 1 - * @group L1 Control - */ -PARAM_DEFINE_INT32(FW_RNW_TKOFF, 0); - -/** - * Specifies which heading should be held during runnway takeoff. - * - * 0: airframe heading, 1: heading towards takeoff waypoint - * - * @min 0 - * @max 1 - * @group L1 Control - */ -PARAM_DEFINE_INT32(FW_RNW_HDG, 0); - -/** - * Altitude AGL at which navigation towards takeoff waypoint starts. - * Until FW_RNW_NAV_ALT is reached the plane is held level and only - * rudder is used to keep the heading (see FW_RNW_HDG). - * - * @min 0.0 - * @max 100.0 - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_RNW_NAV_ALT, 5.0); - -/** - * Max throttle during runway takeoff. - * (Can be used to test taxi on runway) - * - * @min 0.0 - * @max 1.0 - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_RNW_MAX_THR, 1.0); - -/** - * Pitch setpoint during runway takeoff. - * A taildragger with stearable wheel might need to pitch up - * a little to keep it's wheel on the ground before airspeed - * to takeoff is reached. - * - * @min 0.0 - * @max 20.0 - * @group L1 Control - */ -PARAM_DEFINE_FLOAT(FW_RNW_PSP, 0.0); - /** * Flare, minimum pitch *