navigator: break smoothly when entering hold mode

This uses the existing breaking functionality when hold/loiter mode is
activated.

src/modules/navigator/loiter.cpp

@@ -110,7 +110,7 @@ Loiter::set_loiter_position()
 			setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
 
 		} else {
-			setLoiterItemFromCurrentPosition(&_mission_item);
+			setLoiterItemFromCurrentPositionWithBreaking(&_mission_item);
 		}
 
 	}

src/modules/navigator/mission_block.cpp

@@ -704,6 +704,17 @@ MissionBlock::setLoiterItemFromCurrentPosition(struct mission_item_s *item)
 	item->loiter_radius = _navigator->get_loiter_radius();
 }
 
+void
+MissionBlock::setLoiterItemFromCurrentPositionWithBreaking(struct mission_item_s *item)
+{
+	setLoiterItemCommonFields(item);
+
+	_navigator->calculate_breaking_stop(item->lat, item->lon, item->yaw);
+
+	item->altitude = _navigator->get_global_position()->alt;
+	item->loiter_radius = _navigator->get_loiter_radius();
+}
+
 void
 MissionBlock::setLoiterItemCommonFields(struct mission_item_s *item)
 {

src/modules/navigator/mission_block.h

@@ -111,6 +111,7 @@ protected:
 	void setLoiterItemFromCurrentPositionSetpoint(struct mission_item_s *item);
 
 	void setLoiterItemFromCurrentPosition(struct mission_item_s *item);
+	void setLoiterItemFromCurrentPositionWithBreaking(struct mission_item_s *item);
 
 	void setLoiterItemCommonFields(struct mission_item_s *item);
 

src/modules/navigator/navigator_main.cpp

@@ -1577,8 +1577,6 @@ void Navigator::calculate_breaking_stop(double &lat, double &lon, float &yaw)
 
 	waypoint_from_heading_and_distance(get_global_position()->lat, get_global_position()->lon, course_over_ground,
 					   multirotor_braking_distance, &lat, &lon);
-	lat = lat;
-	lon = lon;
 	yaw = get_local_position()->heading;
 }