navigator: when landed only exit idle if mission/takoff

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -1605,6 +1605,7 @@ void MulticopterPositionControl::control_auto(float dt)
 
 	} else {
 		/* no waypoint, do nothing, setpoint was already reset */
+		/* we are in idle */
 	}
 }
 

src/modules/navigator/navigator_main.cpp

@@ -200,6 +200,7 @@ void
 Navigator::vehicle_land_detected_update()
 {
 	orb_copy(ORB_ID(vehicle_land_detected), _land_detected_sub, &_land_detected);
+
 }
 
 void
@@ -633,6 +634,18 @@ Navigator::task_main()
 			_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
 		}
 
+		/* if we landed and have not received takoff setpoint then stay in idle */
+		if (_land_detected.landed &&
+		    !((_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF)
+		      || (_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION))) {
+
+			_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
+			_pos_sp_triplet.current.valid = true;
+			_pos_sp_triplet.previous.valid = false;
+			_pos_sp_triplet.next.valid = false;
+
+		}
+
 		/* if nothing is running, set position setpoint triplet invalid once */
 		if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) {
 			_pos_sp_triplet_published_invalid_once = true;