diff --git a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp index 76419ef6f7..7f8a9ca6c7 100644 --- a/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp @@ -111,8 +111,8 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da _last_output = _rate_setpoint * _k_ff * ctl_data.groundspeed_scaler + _rate_error * _k_p * ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler + integrator_constrained; - /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, speed %.4f, _k_i %.4f, _k_p: %.4f", - (double)_last_output, (double)_integrator, (double)_integrator_max, (double)speed, (double)_k_i, (double)_k_p);*/ + /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, scaler %.4f", + (double)_last_output, (double)_integrator, (double)ctl_data.groundspeed_scaler);*/ return math::constrain(_last_output, -1.0f, 1.0f); diff --git a/src/lib/runway_takeoff/RunwayTakeoff.cpp b/src/lib/runway_takeoff/RunwayTakeoff.cpp index 90e3601e69..46df5cd744 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.cpp +++ b/src/lib/runway_takeoff/RunwayTakeoff.cpp @@ -58,9 +58,8 @@ RunwayTakeoff::RunwayTakeoff() : _initialized_time(0), _init_yaw(0), _climbout(false), - _start_sp{}, - _target_sp{}, _throttle_ramp_time(2 * 1e6), + _start_wp(), _runway_takeoff_enabled(this, "TKOFF"), _heading_mode(this, "HDG"), _nav_alt(this, "NAV_ALT"), @@ -81,16 +80,19 @@ RunwayTakeoff::~RunwayTakeoff() } -void RunwayTakeoff::init(float yaw) +void RunwayTakeoff::init(float yaw, double current_lat, double current_lon) { _init_yaw = yaw; _initialized = true; _state = RunwayTakeoffState::THROTTLE_RAMP; _initialized_time = hrt_absolute_time(); _climbout = true; // this is true until climbout is finished + _start_wp(0) = (float)current_lat; + _start_wp(1) = (float)current_lon; } -void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) +void RunwayTakeoff::update(float airspeed, float alt_agl, + double current_lat, double current_lon, int mavlink_fd) { switch (_state) { @@ -112,6 +114,16 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) case RunwayTakeoffState::TAKEOFF: if (alt_agl > _nav_alt.get()) { _state = RunwayTakeoffState::CLIMBOUT; + + /* + * If we started in heading hold mode, move the navigation start WP to the current location now. + * The navigator will take this as starting point to navigate towards the takeoff WP. + */ + if (_heading_mode.get() == 0) { + _start_wp(0) = (float)current_lat; + _start_wp(1) = (float)current_lon; + } + mavlink_log_info(mavlink_fd, "#Climbout"); } @@ -127,7 +139,7 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd) break; default: - return; + break; } } @@ -177,10 +189,18 @@ float RunwayTakeoff::getRoll(float navigatorRoll) /* * Returns the yaw setpoint to use. + * + * In heading hold mode (_heading_mode == 0), it returns initial yaw as long as it's on the + * runway. When it has enough ground clearance we start navigation towards WP. */ float RunwayTakeoff::getYaw(float navigatorYaw) { - return navigatorYaw; + if (_heading_mode.get() == 0 && _state < RunwayTakeoffState::CLIMBOUT) { + return _init_yaw; + + } else { + return navigatorYaw; + } } /* @@ -252,31 +272,9 @@ float RunwayTakeoff::getMaxPitch(float max) /* * Returns the "previous" (start) WP for navigation. */ -math::Vector<2> RunwayTakeoff::getPrevWP() +math::Vector<2> RunwayTakeoff::getStartWP() { - math::Vector<2> prev_wp; - prev_wp(0) = (float)_start_sp.lat; - prev_wp(1) = (float)_start_sp.lon; - return prev_wp; -} - -/* - * Returns the "current" (target) WP for navigation. - */ -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; - } - + return _start_wp; } void RunwayTakeoff::reset() diff --git a/src/lib/runway_takeoff/RunwayTakeoff.h b/src/lib/runway_takeoff/RunwayTakeoff.h index cf0ab455d8..0e1c5ed14b 100644 --- a/src/lib/runway_takeoff/RunwayTakeoff.h +++ b/src/lib/runway_takeoff/RunwayTakeoff.h @@ -68,16 +68,14 @@ public: RunwayTakeoff(); ~RunwayTakeoff(); - void init(float yaw); - void update(float airspeed, float alt_agl, int mavlink_fd); + void init(float yaw, double current_lat, double current_lon); + void update(float airspeed, float alt_agl, double current_lat, double current_lon, int mavlink_fd); RunwayTakeoffState getState() { return _state; }; 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(); @@ -89,8 +87,7 @@ public: bool resetIntegrators(); 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); + math::Vector<2> getStartWP(); void reset(); @@ -102,9 +99,8 @@ private: hrt_abstime _initialized_time; float _init_yaw; bool _climbout; - struct position_setpoint_s _start_sp; - struct position_setpoint_s _target_sp; unsigned _throttle_ramp_time; + math::Vector<2> _start_wp; /** parameters **/ control::BlockParamInt _runway_takeoff_enabled; diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 41ca2edf44..ce2db9ad5c 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -230,7 +230,7 @@ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f); * @max 1.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_WR_P, 0.3f); +PARAM_DEFINE_FLOAT(FW_WR_P, 0.5f); /** * Wheel steering rate integrator gain @@ -254,7 +254,7 @@ PARAM_DEFINE_FLOAT(FW_WR_I, 0.1f); * @max 1.0 * @group FW Attitude Control */ -PARAM_DEFINE_FLOAT(FW_WR_IMAX, 0.2f); +PARAM_DEFINE_FLOAT(FW_WR_IMAX, 1.0f); /** * Maximum wheel steering rate 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 02a61a20e7..45032d4735 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 @@ -1375,45 +1375,31 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_runway_takeoff.runwayTakeoffEnabled()) { if (!_runway_takeoff.isInitialized()) { - _runway_takeoff.init(_att.yaw); + _runway_takeoff.init(_att.yaw, _global_pos.lat, _global_pos.lon); /* 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 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 _runway_takeoff.update( _airspeed.true_airspeed_m_s, _global_pos.alt - terrain_alt, + _global_pos.lat, + _global_pos.lon, _mavlink_fd); + /* + * Update navigation: _runway_takeoff returns the start WP according to mode and phase. + * If we use the navigator heading or not is decided later. + */ + _l1_control.navigate_waypoints(_runway_takeoff.getStartWP(), curr_wp, current_position, ground_speed_2d); + // update tecs float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_parameters.pitch_limit_max); float takeoff_pitch_max_rad = math::radians(takeoff_pitch_max_deg);