FW mode manager/Commander: hanlde invlaid pos in FW manager

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

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
 

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);

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;

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.
 	 *