From e987082292b82e2dce5065dc343867521741f47c Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 24 Sep 2015 21:53:10 +0200 Subject: [PATCH] split takeoff into 2 phases, reseting integrators when still on runway --- msg/vehicle_attitude_setpoint.msg | 2 +- src/lib/runway_takeoff/RunwayTakeoff.cpp | 29 +++++++++++++++---- src/lib/runway_takeoff/RunwayTakeoff.h | 12 ++++---- .../runway_takeoff/runway_takeoff_params.c | 6 ++-- .../fw_att_control/fw_att_control_main.cpp | 2 +- .../fw_pos_control_l1_main.cpp | 10 +++++-- 6 files changed, 43 insertions(+), 18 deletions(-) diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index bcfe0f41da..3bbb5efa7b 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -22,4 +22,4 @@ bool roll_reset_integral # Reset roll integral part (navigation logic change) bool pitch_reset_integral # Reset pitch integral part (navigation logic change) bool yaw_reset_integral # Reset yaw integral part (navigation logic change) -bool fw_control_wheel # control heading with rudder (used for auto takeoff on runway) +bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway) diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 5067260d32..77d2047882 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -59,6 +59,7 @@ RunwayTakeoff::RunwayTakeoff() : _init_yaw(0), _climbout(false), _min_airspeed_scaling(1.3f), + _max_takeoff_roll(15.0f), _runway_takeoff_enabled(this, "TKOFF"), _runway_takeoff_heading(this, "HDG"), _runway_takeoff_nav_alt(this, "NAV_ALT"), @@ -111,7 +112,15 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) break; case RunwayTakeoffState::TAKEOFF: - if (alt_agl > math::max(_runway_takeoff_nav_alt.get(), _climbout_diff.get())) { + if (alt_agl > _runway_takeoff_nav_alt.get()) { + _state = RunwayTakeoffState::CLIMBOUT; + mavlink_log_info(mavlink_fd, "#Climbout"); + } + + break; + + case RunwayTakeoffState::CLIMBOUT: + if (alt_agl > _climbout_diff.get()) { _state = RunwayTakeoffState::FLY; mavlink_log_info(mavlink_fd, "#Navigating to waypoint"); } @@ -123,10 +132,10 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) } } -bool RunwayTakeoff::controlWheel() +bool RunwayTakeoff::controlYaw() { - // keep controlling wheel until takeoff - return _state < RunwayTakeoffState::TAKEOFF; + // keep controlling yaw directly until we start navigation + return _state < RunwayTakeoffState::CLIMBOUT; } float RunwayTakeoff::getPitch(float tecsPitch) @@ -141,10 +150,17 @@ float RunwayTakeoff::getPitch(float tecsPitch) float RunwayTakeoff::getRoll(float navigatorRoll) { // until we have enough ground clearance, set roll to 0 - if (_state < RunwayTakeoffState::FLY) { + if (_state < RunwayTakeoffState::CLIMBOUT) { return 0.0f; } + // allow some roll during climbout + else if (_state < RunwayTakeoffState::FLY) { + return math::constrain(navigatorRoll, + math::radians(-_max_takeoff_roll), + math::radians(_max_takeoff_roll)); + } + return navigatorRoll; } @@ -186,7 +202,8 @@ float RunwayTakeoff::getThrottle(float tecsThrottle) bool RunwayTakeoff::resetIntegrators() { - return _state <= RunwayTakeoffState::TAKEOFF; + // reset integrators if we're still on runway + return _state < RunwayTakeoffState::TAKEOFF; } float RunwayTakeoff::getMinPitch(float sp_min, float climbout_min, float min) diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index 2f84afee2b..265ff24cb3 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -55,10 +55,11 @@ namespace runwaytakeoff { enum RunwayTakeoffState { - THROTTLE_RAMP = 0, /**< */ - CLAMPED_TO_RUNWAY = 1, /**< */ - TAKEOFF = 2, /**< */ - FLY = 3 /**< */ + THROTTLE_RAMP = 0, /**< ramping up throttle */ + CLAMPED_TO_RUNWAY = 1, /**< clamped to runway, controlling yaw directly (wheel or rudder) */ + TAKEOFF = 2, /**< taking off, get ground clearance, roll 0 */ + CLIMBOUT = 3, /**< climbout to safe height before navigation, roll limited */ + FLY = 4 /**< fly towards takeoff waypoint */ }; class __EXPORT RunwayTakeoff : public control::SuperBlock @@ -75,7 +76,7 @@ public: bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); }; - bool controlWheel(); + bool controlYaw(); bool climbout() { return _climbout; }; float getPitch(float tecsPitch); float getRoll(float navigatorRoll); @@ -98,6 +99,7 @@ private: /** magic values **/ float _min_airspeed_scaling; + float _max_takeoff_roll; /** parameters **/ control::BlockParamInt _runway_takeoff_enabled; diff --git a/src/lib/runway_takeoff/runway_takeoff_params.c b/src/lib/runway_takeoff/runway_takeoff_params.c index 6ccc21e76a..d3ce5d238d 100644 --- a/src/lib/runway_takeoff/runway_takeoff_params.c +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -66,10 +66,10 @@ PARAM_DEFINE_INT32(RWTO_TKOFF, 0); PARAM_DEFINE_INT32(RWTO_HDG, 0); /** - * Altitude AGL at which navigation towards takeoff waypoint starts. + * Altitude AGL at which we have enough ground clearance to allow some roll. * 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. + * rudder is used to keep the heading (see RWTO_HDG). This should be below + * FW_CLMBOUT_DIFF if FW_CLMBOUT_DIFF > 0. * * @min 0.0 * @max 100.0 diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 144a8ed048..da91fda9f7 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1046,7 +1046,7 @@ FixedwingAttitudeControl::task_main() } float yaw_u = 0.0f; - if (_att_sp.fw_control_wheel == true) { + if (_att_sp.fw_control_yaw == true) { yaw_u = _wheel_ctrl.control_bodyrate(control_input); } 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 08deeb60a9..ed71cf2955 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 @@ -1074,7 +1074,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool setpoint = true; - _att_sp.fw_control_wheel = false; // by default we don't want yaw to be contoller directly with rudder + _att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder float eas2tas = 1.0f; // XXX calculate actual number based on current measurements @@ -1371,6 +1371,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } // update navigation + // FIXME: after reselecting the takeoff wp, prev_wp is set to the next wp which causes wrong navigation behaviour _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); @@ -1406,8 +1407,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // 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_wheel = _runway_takeoff.controlWheel(); + _att_sp.fw_control_yaw = _runway_takeoff.controlYaw(); _att_sp.pitch_body = _runway_takeoff.getPitch(_tecs.get_pitch_demand()); + + _att_sp.roll_reset_integral = _runway_takeoff.resetIntegrators(); + _att_sp.pitch_reset_integral = _runway_takeoff.resetIntegrators(); + _att_sp.yaw_reset_integral = _runway_takeoff.resetIntegrators(); + /*warnx("yaw: %.4f, roll: %.4f, pitch: %.4f", (double)_att_sp.yaw_body, (double)_att_sp.roll_body, (double)_att_sp.pitch_body);*/