check_current_pos_sp_reached tweaked to accept expected stepping type for correct land detection

5 files changed, 16 insertions, 12 deletions

diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp
index 5c27e611d..ceb814c3e 100644
--- a/src/modules/navigator/loiter.cpp
+++ b/src/modules/navigator/loiter.cpp
@@ -88,7 +88,7 @@ Loiter::on_activation()
 		set_sub_mode(LOITER_SUB_MODE_TAKING_OFF, 1);
 		takeoff();
 		//resetModeArguments(MAIN_STATE_LOITER); //now done in commander itself
-		
+
 	} else if (vstatus->airdog_state == AIRD_STATE_LANDED || vstatus->airdog_state == AIRD_STATE_STANDBY) {
 		set_sub_mode(LOITER_SUB_MODE_LANDED, 1);
 	} else {
@@ -112,7 +112,7 @@ Loiter::on_active()
         }
 	}
 
-	if (loiter_sub_mode == LOITER_SUB_MODE_LANDING && check_current_pos_sp_reached()) {
+	if (loiter_sub_mode == LOITER_SUB_MODE_LANDING && check_current_pos_sp_reached(SETPOINT_TYPE_LAND)) {
 		set_sub_mode(LOITER_SUB_MODE_LANDED, 0);
 
 		disarm();
@@ -230,9 +230,8 @@ Loiter::execute_command_in_aim_and_shoot(vehicle_command_s cmd){
 			case  REMOTE_CMD_LAND_DISARM: {
 
                 mavlink_log_info(_navigator->get_mavlink_fd(), "Land disarm command");
-
+                set_sub_mode(LOITER_SUB_MODE_LANDING, 0);
 				land();
-				set_sub_mode(LOITER_SUB_MODE_LANDING, 0);
 				break;
 			}
             case REMOTE_CMD_GOTO_DEFUALT_DST: {
diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp
index ee7cd1b18..e455e5cdd 100644
--- a/src/modules/navigator/navigator_mode.cpp
+++ b/src/modules/navigator/navigator_mode.cpp
@@ -240,12 +240,16 @@ NavigatorMode::point_camera_to_target(position_setpoint_s *sp)
 }
 
 bool
-NavigatorMode::check_current_pos_sp_reached()
-{
-	struct vehicle_status_s *vstatus = _navigator->get_vstatus();
+NavigatorMode::check_current_pos_sp_reached(SETPOINT_TYPE expected_sp_type)
+{	
 	pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-	global_pos = _navigator->get_global_position();;
+	if (expected_sp_type != SETPOINT_TYPE_UNDEFINED && pos_sp_triplet->current.type != expected_sp_type) {
+		return false;
+	}
 
+	struct vehicle_status_s *vstatus = _navigator->get_vstatus();	
+	global_pos = _navigator->get_global_position();;
+	
 	switch (pos_sp_triplet->current.type)
 	{
 	case SETPOINT_TYPE_IDLE:
@@ -414,7 +418,7 @@ NavigatorMode::go_to_intial_position(){
 
             double lat_new;
             double lon_new;
-            double alt_new = target_pos->alt + _parameters.takeoff_alt;
+            //double alt_new = target_pos->alt + _parameters.takeoff_alt;
             
             add_vector_to_global_position(
                     target_pos->lat,
@@ -432,7 +436,7 @@ NavigatorMode::go_to_intial_position(){
 
                 pos_sp_triplet->current.lat = lat_new;
                 pos_sp_triplet->current.lon = lon_new;
-                pos_sp_triplet->current.alt = alt_new;
+                //pos_sp_triplet->current.alt = alt_new;
                 pos_sp_triplet->current.type = SETPOINT_TYPE_POSITION;
                 
                 _navigator->set_position_setpoint_triplet_updated();
diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h
index b90bd35f8..4fa12d89b 100644
--- a/src/modules/navigator/navigator_mode.h
+++ b/src/modules/navigator/navigator_mode.h
@@ -169,7 +169,7 @@ protected:
 
 	int		_mavlink_fd;			/**< the file descriptor to send messages over mavlink */
 
-	bool check_current_pos_sp_reached();
+	bool check_current_pos_sp_reached(SETPOINT_TYPE expected_sp_type = SETPOINT_TYPE_UNDEFINED);
     void go_to_intial_position();
 
 
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index ccf58c352..d6e63c350 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -129,7 +129,7 @@ RTL::on_active()
 		if (!first_rtl_setpoint_set) {
 			set_rtl_setpoint();
 			first_rtl_setpoint_set = true;
-		} else if (check_current_pos_sp_reached()) {
+		} else if (check_current_pos_sp_reached(rtl_state == RTL_STATE_LAND ? SETPOINT_TYPE_LAND : SETPOINT_TYPE_UNDEFINED)) {
 			set_next_rtl_state();
 			set_rtl_setpoint();
 		}
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index d9a1a4c78..60bf98361 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -62,6 +62,7 @@ enum SETPOINT_TYPE
 	SETPOINT_TYPE_IDLE,		/**< do nothing, switch off motors or keep at idle speed (MC) */
 	SETPOINT_TYPE_OFFBOARD, 	/**< setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard */
 	SETPOINT_TYPE_MOVING,		/**< moving setpoint, current velocity of the setpoint set */
+	SETPOINT_TYPE_UNDEFINED
 };
 
 struct position_setpoint_s