navigator land abort use reposition (#7574)

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -1466,15 +1466,12 @@ FixedwingPositionControl::handle_command()
 {
 	if (_vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) {
 		// only abort landing before point of no return (horizontal and vertical)
-		if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
+		if (_control_mode.flag_control_auto_enabled &&
+		    _pos_sp_triplet.current.valid &&
+		    _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
 
-			if (_land_noreturn_vertical) {
-				mavlink_log_info(&_mavlink_log_pub, "Landing, can't abort after flare");
-
-			} else {
 			_fw_pos_ctrl_status.abort_landing = true;
-				mavlink_log_info(&_mavlink_log_pub, "Landing, aborted");
-			}
+			mavlink_log_critical(&_mavlink_log_pub, "Landing, aborted");
 		}
 	}
 }

src/modules/navigator/mission.cpp

@@ -262,7 +262,7 @@ Mission::find_offboard_land_start()
 	dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
 
 	for (size_t i = 0; i < _offboard_mission.count; i++) {
-		struct mission_item_s missionitem;
+		struct mission_item_s missionitem = {};
 		const ssize_t len = sizeof(missionitem);
 
 		if (dm_read(dm_current, i, &missionitem, len) != len) {
@@ -1120,7 +1120,7 @@ void
 Mission::do_abort_landing()
 {
 	// Abort FW landing
-	//  turn the land waypoint into a loiter and stay there
+	//  first climb out then loiter over intended landing location
 
 	if (_mission_item.nav_cmd != NAV_CMD_LAND) {
 		return;
@@ -1129,41 +1129,50 @@ Mission::do_abort_landing()
 	// loiter at the larger of MIS_LTRMIN_ALT above the landing point
 	//  or 2 * FW_CLMBOUT_DIFF above the current altitude
 	float alt_landing = get_absolute_altitude_for_item(_mission_item);
+	float min_climbout = _navigator->get_global_position()->alt + (2 * _param_fw_climbout_diff.get());
+	float alt_sp = math::max(alt_landing + _param_loiter_min_alt.get(), min_climbout);
 
-	// ignore _param_loiter_min_alt if smaller then 0 (-1)
-	float alt_sp;
-
-	if (_param_loiter_min_alt.get() > 0.0f) {
-		alt_sp = math::max(alt_landing + _param_loiter_min_alt.get(),
-				   _navigator->get_global_position()->alt + (2 * _param_fw_climbout_diff.get()));
-
-	} else {
-		alt_sp = math::max(alt_landing, _navigator->get_global_position()->alt + (2 * _param_fw_climbout_diff.get()));
-	}
-
+	// turn current landing waypoint into an indefinite loiter
 	_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
 	_mission_item.altitude_is_relative = false;
 	_mission_item.altitude = alt_sp;
-	_mission_item.yaw = NAN;
 	_mission_item.loiter_radius = _navigator->get_loiter_radius();
-	_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
 	_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 	_mission_item.autocontinue = false;
 	_mission_item.origin = ORIGIN_ONBOARD;
 
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
-
 	_navigator->set_position_setpoint_triplet_updated();
 
-	mavlink_log_info(_navigator->get_mavlink_log_pub(), "Holding at %dm above landing.", (int)(alt_sp - alt_landing));
+	mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "Holding at %dm above landing.",
+				     (int)(alt_sp - alt_landing));
+
+	// reset mission index to start of landing
+	int land_start_index = find_offboard_land_start();
 
-	// move mission index back 1 (landing approach point) so that re-entering
-	//  the mission doesn't try to land from the loiter above land
-	// TODO: reset index to MAV_CMD_DO_LAND_START
+	if (land_start_index != -1) {
+		_current_offboard_mission_index = land_start_index;
+
+	} else {
+		// move mission index back (landing approach point)
 		_current_offboard_mission_index -= 1;
 	}
 
+	// send reposition cmd to get out of mission
+	vehicle_command_s vcmd = {};
+	mission_item_to_vehicle_command(&_mission_item, &vcmd);
+	vcmd.timestamp = hrt_absolute_time();
+	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION;
+	vcmd.param1 = -1;
+	vcmd.param2 = 1;
+	vcmd.param5 = _mission_item.lat;
+	vcmd.param6 = _mission_item.lon;
+	vcmd.param7 = alt_sp;
+
+	_navigator->publish_vehicle_cmd(vcmd);
+}
+
 bool
 Mission::prepare_mission_items(bool onboard, struct mission_item_s *mission_item,
 			       struct mission_item_s *next_position_mission_item, bool *has_next_position_item)

src/modules/navigator/mission_block.cpp

@@ -509,12 +509,12 @@ MissionBlock::item_contains_position(const struct mission_item_s *item)
 	       item->nav_cmd == NAV_CMD_VTOL_LAND;
 }
 
-void
+bool
 MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp)
 {
 	/* don't change the setpoint for non-position items */
 	if (!item_contains_position(item)) {
-		return;
+		return false;
 	}
 
 	sp->lat = item->lat;
@@ -600,6 +600,8 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 	}
 
 	sp->valid = true;
+
+	return sp->valid;
 }
 
 void

src/modules/navigator/mission_block.h

@@ -86,7 +86,7 @@ protected:
 	 * @param the mission item to convert
 	 * @param the position setpoint that needs to be set
 	 */
-	void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
+	bool mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
 
 	/**
 	 * Set previous position setpoint to current setpoint

src/modules/navigator/navigator_main.cpp

@@ -481,15 +481,15 @@ Navigator::task_main()
 				int land_start = _mission.find_offboard_land_start();
 
 				if (land_start != -1) {
-					vehicle_command_s vcmd = {};
-					vcmd.target_system = get_vstatus()->system_id;
-					vcmd.target_component = get_vstatus()->component_id;
-					vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START;
-					vcmd.param1 = land_start;
-					vcmd.param2 = 0;
-
-					vcmd.timestamp = hrt_absolute_time();
-					publish_vehicle_cmd(vcmd);
+					vehicle_command_s cmd_mission_start = {};
+					cmd_mission_start.timestamp = hrt_absolute_time();
+					cmd_mission_start.target_system = get_vstatus()->system_id;
+					cmd_mission_start.target_component = get_vstatus()->component_id;
+					cmd_mission_start.command = vehicle_command_s::VEHICLE_CMD_MISSION_START;
+					cmd_mission_start.param1 = land_start;
+					cmd_mission_start.param2 = 0;
+
+					publish_vehicle_cmd(cmd_mission_start);
 
 				} else {
 					PX4_WARN("planned landing not available");

src/modules/navigator/rtl.h

@@ -75,7 +75,7 @@ private:
 	void		advance_rtl();
 
 	/**
-	 * Get rtl altitude
+	 * Get RTL altitude
 	 */
 	float 		get_rtl_altitude();
 
@@ -89,7 +89,7 @@ private:
 		RTL_STATE_LOITER,
 		RTL_STATE_LAND,
 		RTL_STATE_LANDED,
-	} _rtl_state;
+	} _rtl_state{RTL_STATE_NONE};
 
 	control::BlockParamFloat _param_return_alt;
 	control::BlockParamFloat _param_min_loiter_alt;