From e0cdf65fb446bc40ff032ea88b70ebd3f6a6d1ab Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 8 Oct 2015 22:23:18 +0200 Subject: [PATCH] use navigator to hold heading --- src/lib/runway_takeoff/RunwayTakeoff.cpp | 53 +++++++++++-------- src/lib/runway_takeoff/RunwayTakeoff.h | 10 +++- .../runway_takeoff/runway_takeoff_params.c | 11 ---- .../fw_pos_control_l1_main.cpp | 26 +++++++-- 4 files changed, 63 insertions(+), 37 deletions(-) diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index c04ad780f2..16cbb04e45 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -47,6 +47,7 @@ #include #include #include +#include namespace runwaytakeoff { @@ -58,6 +59,8 @@ RunwayTakeoff::RunwayTakeoff() : _initialized_time(0), _init_yaw(0), _climbout(false), + _start_sp{}, + _target_sp{}, _runway_takeoff_enabled(this, "TKOFF"), _heading_mode(this, "HDG"), _nav_alt(this, "NAV_ALT"), @@ -150,17 +153,11 @@ float RunwayTakeoff::getRoll(float navigatorRoll) return 0.0f; } - // allow some roll during climbout if waypoint heading is targeted + // allow some roll during climbout else if (_state < RunwayTakeoffState::FLY) { - if (_heading_mode.get() == 0) { - // otherwise stay at 0 roll - return 0.0f; - - } else if (_heading_mode.get() == 1) { - return math::constrain(navigatorRoll, - math::radians(-_max_takeoff_roll.get()), - math::radians(_max_takeoff_roll).get()); - } + return math::constrain(navigatorRoll, + math::radians(-_max_takeoff_roll.get()), + math::radians(_max_takeoff_roll.get())); } return navigatorRoll; @@ -168,18 +165,6 @@ float RunwayTakeoff::getRoll(float navigatorRoll) float RunwayTakeoff::getYaw(float navigatorYaw) { - if (_state < RunwayTakeoffState::FLY) { - if (_heading_mode.get() == 0) { - // fix heading in the direction the airframe points - return _init_yaw; - - } else if (_heading_mode.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; } @@ -230,6 +215,30 @@ float RunwayTakeoff::getMaxPitch(float max) } } +math::Vector<2> RunwayTakeoff::getPrevWP() +{ + math::Vector<2> prev_wp; + prev_wp(0) = (float)_start_sp.lat; + prev_wp(1) = (float)_start_sp.lon; + return prev_wp; +} + +math::Vector<2> RunwayTakeoff::getCurrWP(math::Vector<2> sp_curr_wp) +{ + if (_heading_mode.get() == 0 && _state < RunwayTakeoffState::FLY) { + // navigating towards calculated direction if heading mode 0 and as long as we're in climbout + math::Vector<2> curr_wp; + curr_wp(0) = (float)_target_sp.lat; + curr_wp(1) = (float)_target_sp.lon; + return curr_wp; + + } else { + // navigating towards next mission waypoint + return sp_curr_wp; + } + +} + void RunwayTakeoff::reset() { _initialized = false; diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index 8e3cd35528..2913b2ad64 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -50,6 +50,7 @@ #include #include #include +#include namespace runwaytakeoff { @@ -75,6 +76,10 @@ public: bool isInitialized() { return _initialized; }; bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); }; + float getMinAirspeedScaling() { return _min_airspeed_scaling.get(); }; + position_setpoint_s *getStartSP() { return &_start_sp; }; + position_setpoint_s *getTargetSP() { return &_target_sp; }; + float getInitYaw() { return _init_yaw; }; bool controlYaw(); bool climbout() { return _climbout; }; @@ -83,9 +88,10 @@ public: float getYaw(float navigatorYaw); float getThrottle(float tecsThrottle); bool resetIntegrators(); - float getMinAirspeedScaling() { return _min_airspeed_scaling.get(); }; float getMinPitch(float sp_min, float climbout_min, float min); float getMaxPitch(float max); + math::Vector<2> getPrevWP(); + math::Vector<2> getCurrWP(math::Vector<2> sp_curr_wp); void reset(); @@ -97,6 +103,8 @@ private: hrt_abstime _initialized_time; float _init_yaw; bool _climbout; + struct position_setpoint_s _start_sp; + struct position_setpoint_s _target_sp; /** 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 05110dddc2..7c64889e76 100644 --- a/src/lib/runway_takeoff/runway_takeoff_params.c +++ b/src/lib/runway_takeoff/runway_takeoff_params.c @@ -110,17 +110,6 @@ PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); */ PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); -/** - * Max pitch during takeoff. - * Fixed-wing settings are used if set to 0. Note that there is also a minimum - * pitch of 10 degrees during takeoff, so this must be larger if set. - * - * @min 0.0 - * @max 60.0 - * @group Runway Takeoff - */ -PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); - /** * Max roll during climbout. * Roll is limited during climbout to ensure enough lift and prevents aggressive 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 b9f24a6eb2..176d4ed11b 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 @@ -1368,12 +1368,32 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* 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; + + // draw a line from takeoff location into the direction the UAV is heading + get_waypoint_heading_distance( + _runway_takeoff.getInitYaw(), + HDG_HOLD_DIST_NEXT, + *_runway_takeoff.getStartSP(), + *_runway_takeoff.getTargetSP(), + true); + mavlink_log_info(_mavlink_fd, "#Takeoff on runway"); } - // 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); + // update takeoff path if we're reaching the end of it + if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, + _runway_takeoff.getTargetSP()->lat, _runway_takeoff.getTargetSP()->lon) < HDG_HOLD_REACHED_DIST) { + get_waypoint_heading_distance( + _runway_takeoff.getInitYaw(), + HDG_HOLD_DIST_NEXT, + *_runway_takeoff.getStartSP(), + *_runway_takeoff.getTargetSP(), + false); + } + + /* update navigation: _runway_takeoff decides if we target the current WP from setpoint triplet + * or the calculated one through initial heading */ + _l1_control.navigate_waypoints(_runway_takeoff.getPrevWP(), _runway_takeoff.getCurrWP(curr_wp), current_position, ground_speed_2d); float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt, _global_pos); // update runway takeoff helper