mc_pos_control: takeoff fixes, ignore position and yaw, takeoff vertically

1 files changed, 19 insertions, 9 deletions

diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index c864db7ed..3c05cec07 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -609,29 +609,39 @@ MulticopterPositionControl::task_main()
 				/* AUTO */
 				if (_mission_items.current_valid) {
 					struct mission_item_s item = _mission_items.current;
-					map_projection_project(item.lat, item.lon, &_pos_sp(0), &_pos_sp(1));
 
-					// TODO home altitude can be != ref_alt, check home_position topic
-					_pos_sp(2) = -(item.altitude_is_relative ? item.altitude : item.altitude - ref_alt);
+					if (item.nav_cmd == NAV_CMD_TAKEOFF) {
+						/* use current position setpoint or current position */
+						if (reset_sp_xy) {
+							reset_sp_xy = false;
+							_pos_sp(0) = _pos(0);
+							_pos_sp(1) = _pos(1);
+						}
 
-					if (_mission_items.current.nav_cmd == NAV_CMD_TAKEOFF) {
 						/* add gap for takeoff setpoint */
 						_pos_sp(2) -= 0.5f * _params.vel_max(2) / _params.pos_p(2);
-					}
+						reset_sp_z = true;
+					} else {
+						map_projection_project(item.lat, item.lon, &_pos_sp(0), &_pos_sp(1));
 
-					_att_sp.yaw_body = _mission_items.current.yaw;
+						// TODO home altitude can be != ref_alt, check home_position topic
+						_pos_sp(2) = -(item.altitude_is_relative ? item.altitude : item.altitude - ref_alt);
 
-					/* in case of interrupted mission don't go to waypoint but stay at current position */
-					reset_sp_xy = true;
-					reset_sp_z = true;
+						_att_sp.yaw_body = _mission_items.current.yaw;
 
+						/* in case of interrupted mission don't go to waypoint but stay at current position */
+						reset_sp_xy = true;
+						reset_sp_z = true;
+					}
 				} else {
 					/* no waypoint, loiter, reset position setpoint if needed */
 					if (reset_sp_xy) {
+						reset_sp_xy = false;
 						_pos_sp(0) = _pos(0);
 						_pos_sp(1) = _pos(1);
 					}
 					if (reset_sp_z) {
+						reset_sp_z = false;
 						_pos_sp(2) = _pos(2);
 					}
 				}