diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index e39ec6d6ce..fee34d393b 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1379,44 +1379,27 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons void FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const float control_interval, - const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, - const position_setpoint_s &pos_sp_curr) + const Vector2d &global_position, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr) { - /* current waypoint (the one currently heading for) */ - Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon); - Vector2d prev_wp{0, 0}; /* previous waypoint */ - Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; - Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1)); - - if (pos_sp_prev.valid) { - prev_wp(0) = pos_sp_prev.lat; - prev_wp(1) = pos_sp_prev.lon; - - } else { - /* - * No valid previous waypoint, go for the current wp. - * This is automatically handled by the L1 library. - */ - prev_wp(0) = pos_sp_curr.lat; - prev_wp(1) = pos_sp_curr.lon; - } - - if (_skipping_takeoff_detection) { - _launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; - } - if (!_control_mode.flag_armed) { - _runway_takeoff.reset(); - _launchDetector.reset(); - _launch_detection_state = LAUNCHDETECTION_RES_NONE; - _launch_detection_notify = 0; + reset_takeoff_state(); } + // for now taking current position setpoint altitude as clearance altitude. this is the altitude we need to + // clear all occlusions in the takeoff path + const float clearance_altitude_amsl = pos_sp_curr.alt; + + // set the altitude to something above the clearance altitude to ensure the vehicle climbs past the value + // (navigator will accept the takeoff as complete once crossing the clearance altitude) + const float altitude_setpoint_amsl = clearance_altitude_amsl + _param_nav_fw_alt_rad.get(); + + Vector2f local_2D_position{_local_pos.x, _local_pos.y}; + float terrain_alt = _takeoff_ground_alt; if (_runway_takeoff.runwayTakeoffEnabled()) { if (!_runway_takeoff.isInitialized()) { - _runway_takeoff.init(now, _yaw, _current_latitude, _current_longitude); + _runway_takeoff.init(now, _yaw, global_position); /* need this already before takeoff is detected * doesn't matter if it gets reset when takeoff is detected eventually */ @@ -1432,57 +1415,94 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt); - // update runway takeoff helper - _runway_takeoff.update(now, _airspeed, _current_altitude - terrain_alt, - _current_latitude, _current_longitude, &_mavlink_log_pub); + _runway_takeoff.update(now, _airspeed, _current_altitude - terrain_alt, clearance_altitude_amsl - _takeoff_ground_alt, + &_mavlink_log_pub); - float target_airspeed = get_auto_airspeed_setpoint(control_interval, - _runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed); + const float takeoff_airspeed = _runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(); + float target_airspeed = get_auto_airspeed_setpoint(control_interval, takeoff_airspeed, ground_speed); - /* - * Update navigation: _runway_takeoff returns the start WP according to mode and phase. - * If we use the navigator heading or not is decided later. - */ - Vector2f prev_wp_local = _global_local_proj_ref.project(_runway_takeoff.getStartWP()(0), - _runway_takeoff.getStartWP()(1)); + // yaw control is disabled once in "taking off" state + _att_sp.fw_control_yaw = _runway_takeoff.controlYaw(); + + // tune up the lateral position control guidance when on the ground + if (_att_sp.fw_control_yaw) { + _npfg.setPeriod(_param_rwto_l1_period.get()); + _l1_control.set_l1_period(_param_rwto_l1_period.get()); + + } + + const Vector2f start_pos_local = _global_local_proj_ref.project(_runway_takeoff.getStartPosition()(0), + _runway_takeoff.getStartPosition()(1)); + const Vector2f takeoff_waypoint_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); + + // the bearing from runway start to the takeoff waypoint is followed until the clearance altitude is exceeded + const Vector2f takeoff_bearing_vector = calculateTakeoffBearingVector(start_pos_local, takeoff_waypoint_local); if (_param_fw_use_npfg.get()) { _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); - // NOTE: current waypoint is passed twice to trigger the "point following" logic -- TODO: create - // point following navigation interface instead of this hack. - _npfg.navigateWaypoints(curr_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); + _npfg.navigatePathTangent(local_2D_position, start_pos_local, takeoff_bearing_vector, ground_speed, + _wind_vel, 0.0f); + _att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint()); + target_airspeed = _npfg.getAirspeedRef() / _eas2tas; + // use npfg's bearing to commanded course, controlled via yaw angle while on runway + const float bearing = _npfg.getBearing(); + + // heading hold mode will override this bearing setpoint + _att_sp.yaw_body = _runway_takeoff.getYaw(bearing); + } else { - _l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed); - _att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.get_roll_setpoint()); + // make a fake waypoint in the direction of the takeoff bearing + const Vector2f virtual_waypoint = start_pos_local + takeoff_bearing_vector * HDG_HOLD_DIST_NEXT; + + _l1_control.navigate_waypoints(start_pos_local, virtual_waypoint, local_2D_position, ground_speed); + + const float l1_roll_setpoint = _l1_control.get_roll_setpoint(); + _att_sp.roll_body = _runway_takeoff.getRoll(l1_roll_setpoint); + + // use l1's nav bearing to command course, controlled via yaw angle while on runway + const float bearing = _l1_control.nav_bearing(); + + // heading hold mode will override this bearing setpoint + _att_sp.yaw_body = _runway_takeoff.getYaw(bearing); } - _att_sp.yaw_body = _runway_takeoff.getYaw(_yaw); - - // update tecs const float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_param_fw_p_lim_max.get()); + const float takeoff_pitch_min_climbout_deg = _runway_takeoff.getMinPitch(_takeoff_pitch_min.get(), + _param_fw_p_lim_min.get()); + + if (_runway_takeoff.resetIntegrators()) { + // reset integrals except yaw (which also counts for the wheel controller) + _att_sp.roll_reset_integral = true; + _att_sp.pitch_reset_integral = true; + + // throttle is open loop anyway during ground roll, no need to wind up the integrator + _tecs.resetIntegrals(); + } + + if (takeoff_airspeed < _param_fw_airspd_min.get()) { + // adjust underspeed detection bounds for takeoff airspeed + _tecs.set_equivalent_airspeed_min(takeoff_airspeed); + } tecs_update_pitch_throttle(control_interval, - pos_sp_curr.alt, + altitude_setpoint_amsl, target_airspeed, radians(_param_fw_p_lim_min.get()), radians(takeoff_pitch_max_deg), _param_fw_thr_min.get(), - _param_fw_thr_max.get(), // XXX should we also set runway_takeoff_throttle here? + _param_fw_thr_max.get(), _runway_takeoff.climbout(), - radians(_runway_takeoff.getMinPitch(_takeoff_pitch_min.get(), _param_fw_p_lim_min.get()))); + radians(takeoff_pitch_min_climbout_deg)); + + _tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation - // assign values - _att_sp.fw_control_yaw = _runway_takeoff.controlYaw(); _att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch()); - - // reset integrals except yaw (which also counts for the wheel controller) - _att_sp.roll_reset_integral = _runway_takeoff.resetIntegrators(); - _att_sp.pitch_reset_integral = _runway_takeoff.resetIntegrators(); + _att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, get_tecs_thrust()); // apply flaps for takeoff according to the corresponding scale factor set via FW_FLAPS_TO_SCL _att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_TAKEOFF; @@ -1490,7 +1510,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo } else { /* Perform launch detection */ - if (_launchDetector.launchDetectionEnabled() && + if (!_skipping_takeoff_detection && _launchDetector.launchDetectionEnabled() && _launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { if (_control_mode.flag_armed) { @@ -1515,23 +1535,36 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS; } + if (!_launch_detected && _launch_detection_state != LAUNCHDETECTION_RES_NONE) { + _launch_detected = true; + _launch_global_position = global_position; + } + + const Vector2f launch_local_position = _global_local_proj_ref.project(_launch_global_position(0), + _launch_global_position(1)); + const Vector2f takeoff_waypoint_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); + + // the bearing from launch to the takeoff waypoint is followed until the clearance altitude is exceeded + const Vector2f takeoff_bearing_vector = calculateTakeoffBearingVector(launch_local_position, takeoff_waypoint_local); + /* Set control values depending on the detection state */ if (_launch_detection_state != LAUNCHDETECTION_RES_NONE) { /* Launch has been detected, hence we have to control the plane. */ float target_airspeed = get_auto_airspeed_setpoint(control_interval, _param_fw_airspd_trim.get(), ground_speed); - Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1)); - if (_param_fw_use_npfg.get()) { _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); - _npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); + _npfg.navigatePathTangent(local_2D_position, launch_local_position, takeoff_bearing_vector, ground_speed, _wind_vel, + 0.0f); _att_sp.roll_body = _npfg.getRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; } else { - _l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed); + // make a fake waypoint in the direction of the takeoff bearing + const Vector2f virtual_waypoint = launch_local_position + takeoff_bearing_vector * HDG_HOLD_DIST_NEXT; + _l1_control.navigate_waypoints(launch_local_position, virtual_waypoint, local_2D_position, ground_speed); _att_sp.roll_body = _l1_control.get_roll_setpoint(); } @@ -1545,16 +1578,13 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo takeoff_throttle = _param_fw_thr_idle.get(); } - /* select maximum pitch: the launchdetector may impose another limit for the pitch - * depending on the state of the launch */ - const float takeoff_pitch_max_deg = _launchDetector.getPitchMax(_param_fw_p_lim_max.get()); - const float altitude_error = pos_sp_curr.alt - _current_altitude; + if (_current_altitude < clearance_altitude_amsl) { + // select maximum pitch: the launchdetector may impose another limit for the pitch + // depending on the state of the launch + const float takeoff_pitch_max_deg = _launchDetector.getPitchMax(_param_fw_p_lim_max.get()); - /* apply minimum pitch and limit roll if target altitude is not within climbout_diff meters */ - if (_param_fw_clmbout_diff.get() > 0.0f && altitude_error > _param_fw_clmbout_diff.get()) { - /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ tecs_update_pitch_throttle(control_interval, - pos_sp_curr.alt, + altitude_setpoint_amsl, target_airspeed, radians(_param_fw_p_lim_min.get()), radians(takeoff_pitch_max_deg), @@ -1565,7 +1595,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo } else { tecs_update_pitch_throttle(control_interval, - pos_sp_curr.alt, + altitude_setpoint_amsl, target_airspeed, radians(_param_fw_p_lim_min.get()), radians(_param_fw_p_lim_max.get()), @@ -1575,6 +1605,16 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo radians(_param_fw_p_lim_min.get())); } + if (_launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { + // explicitly set idle throttle until motors are enabled + _att_sp.thrust_body[0] = _param_fw_thr_idle.get(); + + } else { + _att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust(); + } + + _att_sp.pitch_body = get_tecs_pitch(); + } else { /* Tell the attitude controller to stop integrating while we are waiting * for the launch */ @@ -1584,6 +1624,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo /* Set default roll and pitch setpoints during detection phase */ _att_sp.roll_body = 0.0f; + _att_sp.thrust_body[0] = _param_fw_thr_idle.get(); _att_sp.pitch_body = radians(_takeoff_pitch_min.get()); } @@ -1591,28 +1632,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF; } - if (_launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && !_runway_takeoff.runwayTakeoffEnabled()) { - - /* making sure again that the correct thrust is used, - * without depending on library calls for safety reasons. - the pre-takeoff throttle and the idle throttle normally map to the same parameter. */ - _att_sp.thrust_body[0] = _param_fw_thr_idle.get(); - - } else if (_runway_takeoff.runwayTakeoffEnabled()) { - - _att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, get_tecs_thrust()); - - } else { - /* Copy thrust and pitch values from tecs */ - // when we are landed state we want the motor to spin at idle speed - _att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust(); - } - - // only use TECS pitch setpoint if launch has not been detected and runway takeoff is not enabled - if (!(_launch_detection_state == LAUNCHDETECTION_RES_NONE || _runway_takeoff.runwayTakeoffEnabled())) { - _att_sp.pitch_body = get_tecs_pitch(); - } - _att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt); if (!_vehicle_status.in_transition_to_fw) { @@ -2329,6 +2348,10 @@ FixedwingPositionControl::Run() _tecs.set_speed_weight(_param_fw_t_spdweight.get()); _tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get()); + // restore lateral-directional guidance parameters (changed in takeoff mode) + _npfg.setPeriod(_param_npfg_period.get()); + _l1_control.set_l1_period(_param_fw_l1_period.get()); + _att_sp.roll_reset_integral = false; _att_sp.pitch_reset_integral = false; _att_sp.yaw_reset_integral = false; @@ -2368,8 +2391,7 @@ FixedwingPositionControl::Run() } case FW_POSCTRL_MODE_AUTO_TAKEOFF: { - control_auto_takeoff(_local_pos.timestamp, control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous, - _pos_sp_triplet.current); + control_auto_takeoff(_local_pos.timestamp, control_interval, curr_pos, ground_speed, _pos_sp_triplet.current); break; } @@ -2440,6 +2462,8 @@ FixedwingPositionControl::reset_takeoff_state() _launchDetector.reset(); _launch_detection_state = LAUNCHDETECTION_RES_NONE; _launch_detection_notify = 0; + + _launch_detected = false; } void @@ -2624,6 +2648,27 @@ FixedwingPositionControl::constrainRollNearGround(const float roll_setpoint, con return math::constrain(roll_setpoint, -roll_wingtip_strike, roll_wingtip_strike); } +Vector2f +FixedwingPositionControl::calculateTakeoffBearingVector(const Vector2f &launch_position, + const Vector2f &takeoff_waypoint) const +{ + Vector2f takeoff_bearing_vector = takeoff_waypoint - launch_position; + + if (takeoff_bearing_vector.norm_squared() > FLT_EPSILON) { + takeoff_bearing_vector.normalize(); + + } else { + // TODO: a new bearing only based fixed-wing takeoff command / mission item will get rid of the need + // for this check + + // takeoff in the direction of the airframe + takeoff_bearing_vector(0) = cosf(_yaw); + takeoff_bearing_vector(0) = sinf(_yaw); + } + + return takeoff_bearing_vector; +} + void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpoint_s ¤t_waypoint) { if (_global_local_proj_ref.isInitialized()) { diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index eb13efac2f..550ba23551 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -216,6 +216,12 @@ private: // [m] ground altitude where the plane was launched float _takeoff_ground_alt{0.0f}; + // true if a launch, specifically using the launch detector, has been detected + bool _launch_detected{false}; + + // [deg] global position of the vehicle at the time launch is detected (using launch detector) + Vector2d _launch_global_position{0, 0}; + // [rad] yaw setpoint for manual position mode heading hold float _hdg_hold_yaw{0.0f}; @@ -525,13 +531,12 @@ private: * * @param now Current system time [us] * @param control_interval Time since last position control call [s] - * @param curr_pos Current 2D local position vector of vehicle [m] + * @param global_position Vechile global position [deg] * @param ground_speed Local 2D ground speed of vehicle [m/s] - * @param pos_sp_prev previous position setpoint * @param pos_sp_curr current position setpoint */ - void control_auto_takeoff(const hrt_abstime &now, const float control_interval, const Vector2d &curr_pos, - const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr); + void control_auto_takeoff(const hrt_abstime &now, const float control_interval, const Vector2d &global_position, + const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr); /** * @brief Controls automatic landing. @@ -656,6 +661,15 @@ private: */ float constrainRollNearGround(const float roll_setpoint, const float altitude, const float terrain_altitude) const; + /** + * @brief Calculates the unit takeoff bearing vector from the launch position to takeoff waypont. + * + * @param launch_position Vehicle launch position in local coordinates (NE) [m] + * @param takeoff_waypoint Takeoff waypoint position in local coordinates (NE) [m] + * @return Unit takeoff bearing vector + */ + Vector2f calculateTakeoffBearingVector(const Vector2f &launch_position, const Vector2f &takeoff_waypoint) const; + DEFINE_PARAMETERS( (ParamFloat) _param_fw_airspd_max, @@ -749,7 +763,9 @@ private: (ParamFloat) _param_weight_gross, (ParamFloat) _param_fw_wing_span, - (ParamFloat) _param_fw_wing_height + (ParamFloat) _param_fw_wing_height, + + (ParamFloat) _param_rwto_l1_period ) }; diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp index d6aa863ce0..1c23dcaaac 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp +++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp @@ -52,75 +52,45 @@ using namespace time_literals; namespace runwaytakeoff { -RunwayTakeoff::RunwayTakeoff(ModuleParams *parent) : - ModuleParams(parent), - _state(), - _initialized(false), - _initialized_time(0), - _init_yaw(0), - _climbout(false) +void RunwayTakeoff::init(const hrt_abstime &time_now, const float initial_yaw, const matrix::Vector2d &start_pos_global) { + initial_yaw_ = initial_yaw; + start_pos_global_ = start_pos_global; + takeoff_state_ = RunwayTakeoffState::THROTTLE_RAMP; + climbout_ = true; // this is true until climbout is finished + initialized_ = true; + time_initialized_ = time_now; + takeoff_time_ = 0; } -void RunwayTakeoff::init(const hrt_abstime &now, float yaw, double current_lat, double current_lon) +void RunwayTakeoff::update(const hrt_abstime &time_now, const float calibrated_airspeed, const float vehicle_altitude, + const float clearance_altitude, orb_advert_t *mavlink_log_pub) { - _init_yaw = yaw; - _initialized = true; - _state = RunwayTakeoffState::THROTTLE_RAMP; - _initialized_time = now; - _climbout = true; // this is true until climbout is finished - _start_wp(0) = current_lat; - _start_wp(1) = current_lon; -} - -void RunwayTakeoff::update(const hrt_abstime &now, float airspeed, float alt_agl, - double current_lat, double current_lon, orb_advert_t *mavlink_log_pub) -{ - switch (_state) { + switch (takeoff_state_) { case RunwayTakeoffState::THROTTLE_RAMP: - if (((now - _initialized_time) > (_param_rwto_ramp_time.get() * 1_s)) - || (airspeed > (_param_fw_airspd_min.get() * _param_rwto_airspd_scl.get() * 0.9f))) { - - _state = RunwayTakeoffState::CLAMPED_TO_RUNWAY; + if ((time_now - time_initialized_) > (param_rwto_ramp_time_.get() * 1_s)) { + takeoff_state_ = RunwayTakeoffState::CLAMPED_TO_RUNWAY; } break; case RunwayTakeoffState::CLAMPED_TO_RUNWAY: - if (airspeed > _param_fw_airspd_min.get() * _param_rwto_airspd_scl.get()) { - _state = RunwayTakeoffState::TAKEOFF; - mavlink_log_info(mavlink_log_pub, "#Takeoff airspeed reached\t"); + if (calibrated_airspeed > param_fw_airspd_min_.get() * param_rwto_airspd_scl_.get()) { + takeoff_time_ = time_now; + takeoff_state_ = RunwayTakeoffState::CLIMBOUT; + mavlink_log_info(mavlink_log_pub, "Takeoff airspeed reached, climbout\t"); events::send(events::ID("runway_takeoff_reached_airspeed"), events::Log::Info, - "Takeoff airspeed reached"); - } - - break; - - case RunwayTakeoffState::TAKEOFF: - if (alt_agl > _param_rwto_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 (_param_rwto_hdg.get() == 0) { - _start_wp(0) = current_lat; - _start_wp(1) = current_lon; - } - - mavlink_log_info(mavlink_log_pub, "#Climbout\t"); - events::send(events::ID("runway_takeoff_climbout"), events::Log::Info, "Climbout"); + "Takeoff airspeed reached, climbout"); } break; case RunwayTakeoffState::CLIMBOUT: - if (alt_agl > _param_fw_clmbout_diff.get()) { - _climbout = false; - _state = RunwayTakeoffState::FLY; - mavlink_log_info(mavlink_log_pub, "#Navigating to waypoint\t"); - events::send(events::ID("runway_takeoff_nav_to_wp"), events::Log::Info, "Navigating to waypoint"); + if (vehicle_altitude > clearance_altitude) { + climbout_ = false; + takeoff_state_ = RunwayTakeoffState::FLY; + mavlink_log_info(mavlink_log_pub, "Reached clearance altitude\t"); + events::send(events::ID("runway_takeoff_reached_clearance_altitude"), events::Log::Info, "Reached clearance altitude"); } break; @@ -130,132 +100,96 @@ void RunwayTakeoff::update(const hrt_abstime &now, float airspeed, float alt_agl } } -/* - * Returns true as long as we're below navigation altitude - */ bool RunwayTakeoff::controlYaw() { // keep controlling yaw directly until we start navigation - return _state < RunwayTakeoffState::CLIMBOUT; + return takeoff_state_ < RunwayTakeoffState::CLIMBOUT; } -/* - * Returns pitch setpoint to use. - * - * Limited (parameter) as long as the plane is on runway. Otherwise - * use the one from TECS - */ -float RunwayTakeoff::getPitch(float tecsPitch) +float RunwayTakeoff::getPitch(float external_pitch_setpoint) { - if (_state <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) { - return math::radians(_param_rwto_psp.get()); + if (takeoff_state_ <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) { + return math::radians(param_rwto_psp_.get()); } - return tecsPitch; + return external_pitch_setpoint; } -/* - * Returns the roll setpoint to use. - */ -float RunwayTakeoff::getRoll(float navigatorRoll) +float RunwayTakeoff::getRoll(float external_roll_setpoint) { // until we have enough ground clearance, set roll to 0 - if (_state < RunwayTakeoffState::CLIMBOUT) { + if (takeoff_state_ < RunwayTakeoffState::CLIMBOUT) { return 0.0f; } - // allow some roll during climbout - else if (_state < RunwayTakeoffState::FLY) { - return math::constrain(navigatorRoll, - math::radians(-_param_rwto_max_roll.get()), - math::radians(_param_rwto_max_roll.get())); - } - - return navigatorRoll; + return external_roll_setpoint; } -/* - * 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) +float RunwayTakeoff::getYaw(float external_yaw_setpoint) { - if (_param_rwto_hdg.get() == 0 && _state < RunwayTakeoffState::CLIMBOUT) { - return _init_yaw; + if (param_rwto_hdg_.get() == 0 && takeoff_state_ < RunwayTakeoffState::CLIMBOUT) { + return initial_yaw_; } else { - return navigatorYaw; + return external_yaw_setpoint; } } -/* - * Returns the throttle setpoint to use. - * - * Ramps up in the beginning, until it lifts off the runway it is set to - * parameter value, then it returns the TECS throttle. - */ -float RunwayTakeoff::getThrottle(const hrt_abstime &now, float tecsThrottle) +float RunwayTakeoff::getThrottle(const hrt_abstime &time_now, float external_throttle_setpoint) { - switch (_state) { - case RunwayTakeoffState::THROTTLE_RAMP: { - float throttle = ((now - _initialized_time) / (_param_rwto_ramp_time.get() * 1_s)) * _param_rwto_max_thr.get(); - return math::min(throttle, _param_rwto_max_thr.get()); - } + float throttle = 0.0f; + + switch (takeoff_state_) { + case RunwayTakeoffState::THROTTLE_RAMP: + throttle = ((time_now - time_initialized_) / (param_rwto_ramp_time_.get() * 1_s)) * param_rwto_max_thr_.get(); + throttle = math::constrain(throttle, 0.0f, param_rwto_max_thr_.get()); + break; case RunwayTakeoffState::CLAMPED_TO_RUNWAY: - return _param_rwto_max_thr.get(); + throttle = param_rwto_max_thr_.get(); + break; default: - return tecsThrottle; + const float interpolator = math::constrain((time_now - takeoff_time_ * 1_s) / (kThrottleHysteresisTime * 1_s), 0.0f, + 1.0f); + throttle = external_throttle_setpoint * interpolator + (1.0f - interpolator) * param_rwto_max_thr_.get(); } + + return throttle; } bool RunwayTakeoff::resetIntegrators() { // reset integrators if we're still on runway - return _state < RunwayTakeoffState::TAKEOFF; + return takeoff_state_ < RunwayTakeoffState::CLIMBOUT; } -/* - * Returns the minimum pitch for TECS to use. - * - * In climbout we either want what was set on the waypoint (sp_min) but at least - * the climbtout minimum pitch (parameter). - * Otherwise use the minimum that is enforced generally (parameter). - */ -float RunwayTakeoff::getMinPitch(float climbout_min, float min) +float RunwayTakeoff::getMinPitch(float min_pitch_in_climbout, float min_pitch) { - if (_state < RunwayTakeoffState::FLY) { - return climbout_min; + if (takeoff_state_ < RunwayTakeoffState::FLY) { + return min_pitch_in_climbout; } else { - return min; + return min_pitch; } } -/* - * Returns the maximum pitch for TECS to use. - * - * Limited by parameter (if set) until climbout is done. - */ -float RunwayTakeoff::getMaxPitch(float max) +float RunwayTakeoff::getMaxPitch(const float max_pitch) { - // use max pitch from parameter if set (> 0.1) - if (_state < RunwayTakeoffState::FLY && _param_rwto_max_pitch.get() > 0.1f) { - return _param_rwto_max_pitch.get(); - } + // use max pitch from parameter if set + if (takeoff_state_ < RunwayTakeoffState::FLY && param_rwto_max_pitch_.get() > kMinMaxPitch) { + return param_rwto_max_pitch_.get(); - else { - return max; + } else { + return max_pitch; } } void RunwayTakeoff::reset() { - _initialized = false; - _state = RunwayTakeoffState::THROTTLE_RAMP; + initialized_ = false; + takeoff_state_ = RunwayTakeoffState::THROTTLE_RAMP; + takeoff_time_ = 0; } -} +} // namespace runwaytakeoff diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h index 91f1857ad9..fbcabf0092 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h +++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h @@ -55,71 +55,204 @@ namespace runwaytakeoff { enum RunwayTakeoffState { - 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 */ + THROTTLE_RAMP = 0, // ramping up throttle + CLAMPED_TO_RUNWAY, // clamped to runway, controlling yaw directly (wheel or rudder) + CLIMBOUT, // climbout to safe height before navigation + FLY // navigate freely }; class __EXPORT RunwayTakeoff : public ModuleParams { public: - RunwayTakeoff(ModuleParams *parent); + RunwayTakeoff(ModuleParams *parent) : ModuleParams(parent) {} ~RunwayTakeoff() = default; - void init(const hrt_abstime &now, float yaw, double current_lat, double current_lon); - void update(const hrt_abstime &now, float airspeed, float alt_agl, double current_lat, double current_lon, - orb_advert_t *mavlink_log_pub); + /** + * @brief Initializes the state machine. + * + * @param time_now Absolute time since system boot [us] + * @param initial_yaw Vehicle yaw angle at time of initialization [us] + * @param start_pos_global Vehicle global (lat, lon) position at time of initialization [deg] + */ + void init(const hrt_abstime &time_now, const float initial_yaw, const matrix::Vector2d &start_pos_global); - RunwayTakeoffState getState() { return _state; } - bool isInitialized() { return _initialized; } + /** + * @brief Updates the state machine based on the current vehicle condition. + * + * @param time_now Absolute time since system boot [us] + * @param calibrated_airspeed Vehicle calibrated airspeed [m/s] + * @param vehicle_altitude Vehicle altitude (AGL) [m] + * @param clearance_altitude Altitude (AGL) above which we have cleared all occlusions in the runway path [m] + * @param mavlink_log_pub + */ + void update(const hrt_abstime &time_now, const float calibrated_airspeed, const float vehicle_altitude, + const float clearance_altitude, orb_advert_t *mavlink_log_pub); - bool runwayTakeoffEnabled() { return _param_rwto_tkoff.get(); } - float getMinAirspeedScaling() { return _param_rwto_airspd_scl.get(); } - float getInitYaw() { return _init_yaw; } + /** + * @return Current takeoff state + */ + RunwayTakeoffState getState() { return takeoff_state_; } + /** + * @return The state machine is initialized + */ + bool isInitialized() { return initialized_; } + + /** + * @return Runway takeoff is enabled + */ + bool runwayTakeoffEnabled() { return param_rwto_tkoff_.get(); } + + /** + * @return Scale factor for minimum indicated airspeed + */ + float getMinAirspeedScaling() { return param_rwto_airspd_scl_.get(); } + + /** + * @return Initial vehicle yaw angle [rad] + */ + float getInitYaw() { return initial_yaw_; } + + /** + * @return The vehicle should control yaw via rudder or nose gear + */ bool controlYaw(); - bool climbout() { return _climbout; } - float getPitch(float tecsPitch); - float getRoll(float navigatorRoll); - float getYaw(float navigatorYaw); - float getThrottle(const hrt_abstime &now, float tecsThrottle); - bool resetIntegrators(); - float getMinPitch(float climbout_min, float min); - float getMaxPitch(float max); - const matrix::Vector2d &getStartWP() const { return _start_wp; }; + + /** + * @return TECS should be commanded to climbout mode + */ + bool climbout() { return climbout_; } + + /** + * @param external_pitch_setpoint Externally commanded pitch angle setpoint (usually from TECS) [rad] + * @return Pitch angle setpoint (limited while plane is on runway) [rad] + */ + float getPitch(float external_pitch_setpoint); + + /** + * @param external_roll_setpoint Externally commanded roll angle setpoint (usually from L1) [rad] + * @return Roll angle setpoint [rad] + */ + float getRoll(float external_roll_setpoint); + + /** + * @brief Returns the appropriate yaw angle setpoint. + * + * 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. + * + * @param external_yaw_setpoint Externally commanded yaw angle setpoint [rad] + * @return Yaw angle setpoint [rad] + */ + float getYaw(float external_yaw_setpoint); + + /** + * @brief Returns the throttle setpoint. + * + * Ramps up over RWTO_RAMP_TIME to RWTO_MAX_THR until the aircraft lifts off the runway, then passes + * through the externally defined throttle setting. + * + * @param time_now Absolute time since system boot [us] + * @param external_throttle_setpoint Externally commanded throttle setpoint (usually from TECS), normalized [0,1] + * @return Throttle setpoint, normalized [0,1] + */ + float getThrottle(const hrt_abstime &time_now, const float external_throttle_setpoint); + + /** + * @param min_pitch_in_climbout Minimum pitch angle during climbout [rad] + * @param min_pitch Externally commanded minimum pitch angle [rad] + * @return Minimum pitch angle [rad] + */ + float getMinPitch(const float min_pitch_in_climbout, const float min_pitch); + + /** + * @param max_pitch Externally commanded maximum pitch angle [rad] + * @return Maximum pitch angle [rad] + */ + float getMaxPitch(const float max_pitch); + + /** + * @return Runway takeoff starting position in global frame (lat, lon) [deg] + */ + const matrix::Vector2d &getStartPosition() const { return start_pos_global_; }; // NOTE: this is only to be used for mistaken mode transitions to takeoff while already in air - void forceSetFlyState() { _state = RunwayTakeoffState::FLY; } + void forceSetFlyState() { takeoff_state_ = RunwayTakeoffState::FLY; } + /** + * @return If the attitude / rate control integrators should be continually reset. + * This is the case during ground roll. + */ + bool resetIntegrators(); + + /** + * @brief Reset the state machine. + */ void reset(); private: - /** state variables **/ - RunwayTakeoffState _state{THROTTLE_RAMP}; - bool _initialized{false}; - hrt_abstime _initialized_time{0}; - float _init_yaw{0.f}; - bool _climbout{false}; - matrix::Vector2d _start_wp; + /** + * Minimum allowed maximum pitch constraint (from parameter) for runway takeoff. [rad] + */ + static constexpr float kMinMaxPitch = 0.1f; + + /** + * Time from takeoff during which the takeoff throttle is transitioned to the externally commanded throttle. + * Used to avoid throttle jumps immediately on lift off when switching from the constant, parameterized, takeoff + * throttle to the airspeed/altitude controller's commanded throttle [s] + */ + static constexpr float kThrottleHysteresisTime = 2.0f; + + /** + * Current state of runway takeoff procedure + */ + RunwayTakeoffState takeoff_state_{THROTTLE_RAMP}; + + /** + * True if the runway state machine is initialized + */ + bool initialized_{false}; + + /** + * The absolute time since system boot at which the state machine was intialized [us] + */ + hrt_abstime time_initialized_{0}; + + /** + * The absolute time the takeoff state transitions from CLAMPED_TO_RUNWAY -> CLIMBOUT [us] + */ + hrt_abstime takeoff_time_{0}; + + /** + * Initial yaw of the vehicle on first pass through the runway takeoff state machine. + * used for heading hold mode. [rad] + */ + float initial_yaw_{0.f}; + + /** + * True if TECS should be commanded to "climbout" mode. + */ + bool climbout_{false}; + + /** + * The global (lat, lon) position of the vehicle on first pass through the runway takeoff state machine. The + * takeoff path emanates from this point to correct for any GNSS uncertainty from the planned takeoff point. The + * vehicle should accordingly be set on the center of the runway before engaging the mission. [deg] + */ + matrix::Vector2d start_pos_global_{}; DEFINE_PARAMETERS( - (ParamBool) _param_rwto_tkoff, - (ParamInt) _param_rwto_hdg, - (ParamFloat) _param_rwto_nav_alt, - (ParamFloat) _param_rwto_max_thr, - (ParamFloat) _param_rwto_psp, - (ParamFloat) _param_rwto_max_pitch, - (ParamFloat) _param_rwto_max_roll, - (ParamFloat) _param_rwto_airspd_scl, - (ParamFloat) _param_rwto_ramp_time, - (ParamFloat) _param_fw_airspd_min, - (ParamFloat) _param_fw_clmbout_diff + (ParamBool) param_rwto_tkoff_, + (ParamInt) param_rwto_hdg_, + (ParamFloat) param_rwto_max_thr_, + (ParamFloat) param_rwto_psp_, + (ParamFloat) param_rwto_max_pitch_, + (ParamFloat) param_rwto_airspd_scl_, + (ParamFloat) param_rwto_ramp_time_, + (ParamFloat) param_fw_airspd_min_ ) - }; -} +} // namespace runwaytakeoff #endif // RUNWAYTAKEOFF_H diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c b/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c index 5f165d06ca..6423c56bfb 100644 --- a/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c +++ b/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c @@ -50,32 +50,17 @@ PARAM_DEFINE_INT32(RWTO_TKOFF, 0); /** * Specifies which heading should be held during runnway takeoff. * - * 0: airframe heading, 1: heading towards takeoff waypoint + * 0: airframe heading + * 1: position control along runway direction (defined by operator in MAV_CMD) * * @value 0 Airframe - * @value 1 Waypoint + * @value 1 Runway * @min 0 * @max 1 * @group Runway Takeoff */ PARAM_DEFINE_INT32(RWTO_HDG, 0); -/** - * 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). This should be below - * FW_CLMBOUT_DIFF if FW_CLMBOUT_DIFF > 0. - * - * @unit m - * @min 0.0 - * @max 100.0 - * @decimal 1 - * @increment 1 - * @group Runway Takeoff - */ -PARAM_DEFINE_FLOAT(RWTO_NAV_ALT, 5.0); - /** * Max throttle during runway takeoff. * @@ -121,21 +106,6 @@ PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0); */ PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0); -/** - * Max roll during climbout. - * - * Roll is limited during climbout to ensure enough lift and prevents aggressive - * navigation before we're on a safe height. - * - * @unit deg - * @min 0.0 - * @max 60.0 - * @decimal 1 - * @increment 0.5 - * @group Runway Takeoff - */ -PARAM_DEFINE_FLOAT(RWTO_MAX_ROLL, 25.0); - /** * Min airspeed scaling factor for takeoff. * @@ -162,3 +132,15 @@ PARAM_DEFINE_FLOAT(RWTO_AIRSPD_SCL, 1.3); * @group Runway Takeoff */ PARAM_DEFINE_FLOAT(RWTO_RAMP_TIME, 2.0f); + +/** + * L1 period while steering on runway + * + * @unit s + * @min 1.0 + * @max 100.0 + * @decimal 1 + * @increment 0.1 + * @group Runway Takeoff + */ +PARAM_DEFINE_FLOAT(RWTO_L1_PERIOD, 5.0f); diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 1c1c8d2630..903948e263 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -182,6 +182,13 @@ MissionBlock::is_mission_item_reached() _waypoint_position_reached = true; } + } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF + && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + /* fixed-wing takeoff is reached once the vehicle has exceeded the takeoff altitude */ + if (_navigator->get_global_position()->alt > mission_item_altitude_amsl) { + _waypoint_position_reached = true; + } + } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) { /* for takeoff mission items use the parameter for the takeoff acceptance radius */