diff --git a/docs/en/flight_modes_fw/takeoff.md b/docs/en/flight_modes_fw/takeoff.md index 7faca65a74..0330d9c950 100644 --- a/docs/en/flight_modes_fw/takeoff.md +++ b/docs/en/flight_modes_fw/takeoff.md @@ -9,10 +9,10 @@ Vehicles are [hand or catapult launched](#catapult-hand-launch) by default, but ::: info - Mode is automatic - no user intervention is _required_ to control the vehicle. -- Mode requires at least a valid local position estimate (does not require a global position). - - Flying vehicles can't switch to this mode without valid local position. - - Flying vehicles will failsafe if they lose the position estimate. - - Disarmed vehicles can switch to mode without valid position estimate but can't arm. +- Mode requires at least a valid altitude estimation. + - Flying vehicles can't switch to this mode without valid altitude. + - Flying vehicles will failsafe if they lose the altitude estimate. + - Disarmed vehicles can switch to mode without valid altitude estimate but can't arm. - RC control switches can be used to change flight modes. - RC stick movement is ignored in catapult takeoff but can can be used to nudge the vehicle in runway takeoff. - The [Failure Detector](../config/safety.md#failure-detector) will automatically stop the engines if there is a problem on takeoff. @@ -38,6 +38,7 @@ Irrespective of the modality, a flight path (starting point and takeoff course) On takeoff, the aircraft will follow line defined by the starting point and course, climbing at the maximum climb rate ([FW_T_CLMB_MAX](../advanced_config/parameter_reference.md#FW_T_CLMB_MAX)) until reaching the clearance altitude. Reaching the clearance altitude causes the vehicle to enter [Hold mode](../flight_modes_fw/takeoff.md). +Special case for invalid local position: In case the local position is invalid or becomes invalid while executing the takeoff, the controller is not able to track a course setpoint and will instead proceed climbing while keeping the wings level until the clearance altitude is reached. ### Parameters diff --git a/src/modules/commander/ModeUtil/mode_requirements.cpp b/src/modules/commander/ModeUtil/mode_requirements.cpp index de13d4155b..9e64c529f2 100644 --- a/src/modules/commander/ModeUtil/mode_requirements.cpp +++ b/src/modules/commander/ModeUtil/mode_requirements.cpp @@ -158,9 +158,13 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags) // NAVIGATION_STATE_AUTO_TAKEOFF setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF, flags.mode_req_angular_velocity); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF, flags.mode_req_attitude); - setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF, flags.mode_req_local_position); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF, flags.mode_req_local_alt); + if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + // only require local position for rotary wing vehicles, fixed wing vehicles can take off without it + setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF, flags.mode_req_local_position); + } + // NAVIGATION_STATE_AUTO_LAND setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, flags.mode_req_angular_velocity); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, flags.mode_req_attitude); diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.cpp b/src/modules/fw_mode_manager/FixedWingModeManager.cpp index f1f8810907..4f003094e9 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.cpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.cpp @@ -411,9 +411,10 @@ FixedWingModeManager::set_control_mode_current(const hrt_abstime &now) _pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; } else { - _control_mode_current = FW_POSCTRL_MODE_AUTO_TAKEOFF; + _control_mode_current = _local_pos.xy_valid ? FW_POSCTRL_MODE_AUTO_TAKEOFF : FW_POSCTRL_MODE_AUTO_TAKEOFF_NO_NAV; - if (previous_position_control_mode != FW_POSCTRL_MODE_AUTO_TAKEOFF && !_landed) { + if (previous_position_control_mode != FW_POSCTRL_MODE_AUTO_TAKEOFF_NO_NAV + && previous_position_control_mode != FW_POSCTRL_MODE_AUTO_TAKEOFF && !_landed) { // skip takeoff detection when switching from any other mode, auto or manual, // while already in air. // TODO: find a better place for / way of doing this @@ -1262,6 +1263,127 @@ FixedWingModeManager::control_auto_takeoff(const hrt_abstime &now, const float c } } +void +FixedWingModeManager::control_auto_takeoff_no_nav(const hrt_abstime &now, const float control_interval, + const float current_setpoint_altitude_amsl) +{ + if (!_control_mode.flag_armed) { + reset_takeoff_state(); + } + + const float takeoff_airspeed = (_param_fw_tko_airspd.get() > FLT_EPSILON) ? _param_fw_tko_airspd.get() : + _param_fw_airspd_min.get(); + + if (_runway_takeoff.runwayTakeoffEnabled()) { + if (!_runway_takeoff.isInitialized()) { + _runway_takeoff.init(now); + _takeoff_ground_alt = _current_altitude; + _launch_current_yaw = _yaw; + } + + if (_skipping_takeoff_detection) { + _runway_takeoff.forceSetFlyState(); + } + + const float clearance_altitude_amsl = _current_altitude + 20.f; // hard code it to 20m above takeoff point + + _runway_takeoff.update(now, takeoff_airspeed, _airspeed_eas, _current_altitude - _takeoff_ground_alt, + clearance_altitude_amsl - _takeoff_ground_alt); + + fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = now; + fw_lateral_ctrl_sp.lateral_acceleration = 0.f; // level wings + + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); + + const float pitch_max = _runway_takeoff.getMaxPitch(math::radians(_param_fw_p_lim_max.get())); + const float pitch_min = _runway_takeoff.getMinPitch(math::radians(_takeoff_pitch_min.get()), + math::radians(_param_fw_p_lim_min.get())); + + const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = { + .timestamp = now, + .altitude = NAN, + .height_rate = _param_fw_t_clmb_max.get(), + .equivalent_airspeed = takeoff_airspeed, + .pitch_direct = _runway_takeoff.getPitch(), + .throttle_direct = _runway_takeoff.getThrottle(_param_fw_thr_idle.get()) + }; + + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); + + _ctrl_configuration_handler.setPitchMin(pitch_min); + _ctrl_configuration_handler.setPitchMax(pitch_max); + _ctrl_configuration_handler.setDisableUnderspeedProtection(true); + + _flaps_setpoint = _param_fw_flaps_to_scl.get(); + + // retract ladning gear once passed the climbout state + if (_runway_takeoff.getState() > RunwayTakeoffState::CLIMBOUT) { + _new_landing_gear_position = landing_gear_s::GEAR_UP; + } + + fixed_wing_runway_control_s fw_runway_control{}; + fw_runway_control.timestamp = now; + fw_runway_control.wheel_steering_enabled = true; + fw_runway_control.wheel_steering_nudging_rate = _param_rwto_nudge.get() ? _manual_control_setpoint.yaw : 0.f; + + _fixed_wing_runway_control_pub.publish(fw_runway_control); + + } else { + /* Perform launch detection */ + if (!_skipping_takeoff_detection && _param_fw_laun_detcn_on.get() && + _launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) { + + if (_control_mode.flag_armed) { + /* Detect launch using body X (forward) acceleration */ + _launchDetector.update(control_interval, _body_acceleration_x); + } + + } else { + /* no takeoff detection --> fly */ + _launchDetector.forceSetFlyState(); + } + + if (!_launch_detected && _launchDetector.getLaunchDetected() > launch_detection_status_s::STATE_WAITING_FOR_LAUNCH) { + _launch_detected = true; + _takeoff_ground_alt = _current_altitude; + } + + /* Launch has been detected, hence we have to control the plane. */ + + fixed_wing_lateral_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint}; + fw_lateral_ctrl_sp.timestamp = now; + fw_lateral_ctrl_sp.lateral_acceleration = 0.f; // level wings + + _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); + + const float max_takeoff_throttle = (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) ? + _param_fw_thr_idle.get() : NAN; + const fixed_wing_longitudinal_setpoint_s fw_longitudinal_control_sp = { + .timestamp = now, + .altitude = NAN, + .height_rate = _param_fw_t_clmb_max.get(), + .equivalent_airspeed = takeoff_airspeed, + .pitch_direct = NAN, + .throttle_direct = NAN + }; + + _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); + + _ctrl_configuration_handler.setPitchMin(radians(_takeoff_pitch_min.get())); + _ctrl_configuration_handler.setThrottleMax(max_takeoff_throttle); + _ctrl_configuration_handler.setClimbRateTarget(_param_fw_t_clmb_max.get()); + _ctrl_configuration_handler.setDisableUnderspeedProtection(true); + + launch_detection_status_s launch_detection_status; + launch_detection_status.timestamp = now; + launch_detection_status.launch_detection_state = _launchDetector.getLaunchDetected(); + _launch_detection_status_pub.publish(launch_detection_status); + } + + _flaps_setpoint = _param_fw_flaps_to_scl.get(); +} + void FixedWingModeManager::control_auto_landing_straight(const hrt_abstime &now, const float control_interval, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) @@ -2031,7 +2153,8 @@ FixedWingModeManager::Run() reset_landing_state(); } - if (_control_mode_current != FW_POSCTRL_MODE_AUTO_TAKEOFF) { + if (_control_mode_current != FW_POSCTRL_MODE_AUTO_TAKEOFF + && _control_mode_current != FW_POSCTRL_MODE_AUTO_TAKEOFF_NO_NAV) { reset_takeoff_state(); } @@ -2076,6 +2199,11 @@ FixedWingModeManager::Run() break; } + case FW_POSCTRL_MODE_AUTO_TAKEOFF_NO_NAV: { + control_auto_takeoff_no_nav(_local_pos.timestamp, control_interval, _pos_sp_triplet.current.alt); + break; + } + case FW_POSCTRL_MODE_MANUAL_POSITION: { control_manual_position(now, control_interval, curr_pos, ground_speed); break; diff --git a/src/modules/fw_mode_manager/FixedWingModeManager.hpp b/src/modules/fw_mode_manager/FixedWingModeManager.hpp index 81eed7d81f..b02e29678c 100644 --- a/src/modules/fw_mode_manager/FixedWingModeManager.hpp +++ b/src/modules/fw_mode_manager/FixedWingModeManager.hpp @@ -227,6 +227,7 @@ private: FW_POSCTRL_MODE_AUTO_ALTITUDE, FW_POSCTRL_MODE_AUTO_CLIMBRATE, FW_POSCTRL_MODE_AUTO_TAKEOFF, + FW_POSCTRL_MODE_AUTO_TAKEOFF_NO_NAV, FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT, FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR, FW_POSCTRL_MODE_AUTO_PATH, @@ -570,6 +571,16 @@ private: 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 takeoff without navigation. + * + * @param now Current system time [us] + * @param control_interval Time since last position control call [s] + * @param altitude_setpoint_amsl Altitude setpoint, AMSL [m] + */ + void control_auto_takeoff_no_nav(const hrt_abstime &now, const float control_interval, + const float altitude_setpoint_amsl); + /** * @brief Controls automatic landing with straight approach. *