navigator: bugfixing (WIP: mission topic not copying)

5 files changed, 34 insertions, 20 deletions

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index f3a86666f..7e02f8c15 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -50,6 +50,7 @@
 #include <geo/geo.h>
 
 #include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
 
 #include "navigator.h"
@@ -61,8 +62,6 @@ Mission::Mission(Navigator *navigator) :
 	_navigator(navigator),
 	_first_run(true),
 	_param_onboard_enabled(this, "ONBOARD_EN"),
-	_onboard_mission_sub(-1),
-	_offboard_mission_sub(-1),
 	_onboard_mission({0}),
 	_offboard_mission({0}),
 	_mission_item({0}),
@@ -74,6 +73,9 @@ Mission::Mission(Navigator *navigator) :
 	updateParams();
 	/* set initial mission items */
 	reset();
+
+	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
+	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
 }
 
 Mission::~Mission()
@@ -89,9 +91,14 @@ Mission::reset()
 bool
 Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
+	/* check if anything has changed */
+	bool onboard_updated = false; //is_onboard_mission_updated();
+	bool offboard_updated = is_offboard_mission_updated();
+
 	bool updated = false;
-	/* check if anything has changed, and reset mission items if needed */
-	if (is_onboard_mission_updated() || is_offboard_mission_updated() || _first_run) {
+
+	/* reset mission items if needed */
+	if (onboard_updated || offboard_updated) {
 		set_mission_items(pos_sp_triplet);
 		updated = true;
 		_first_run = false;
@@ -229,7 +236,7 @@ Mission::is_onboard_mission_updated()
 	bool updated;
 	orb_check(_onboard_mission_sub, &updated);
 
-	if (!updated) {
+	if (!updated && !_first_run) {
 		return false;
 	}
 
@@ -244,12 +251,17 @@ bool
 Mission::is_offboard_mission_updated()
 {
 	bool updated;
+	warnx("sub: %d", _offboard_mission_sub);
 	orb_check(_offboard_mission_sub, &updated);
-	if (!updated) {
+	if (!updated && !_first_run) {
+		warnx("not updated");
 		return false;
 	}
-
-	if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &_offboard_mission) == OK) {
+	struct mission_s offboard_mission;
+	int ret = orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission);
+	warnx("ret: %d", ret);
+	if (ret == OK) {
+		warnx("copy new offboard mission");
 
 		/* Check mission feasibility, for now do not handle the return value,
 		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
@@ -263,8 +275,10 @@ Mission::is_offboard_mission_updated()
 
 		missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current,
 			                                      (size_t)_offboard_mission.count,
-							      _navigator->get_geofence());
+							      _navigator->get_geofence(),
+							      _navigator->get_home_position()->alt);
 	} else {
+		warnx("no success with orb_copy");
 		_offboard_mission.count = 0;
 		_offboard_mission.current_index = 0;
 	}
@@ -341,7 +355,9 @@ Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current
 bool
 Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
 {
+	warnx("try offboard mission: %d, %d", _offboard_mission.current_index, _offboard_mission.count );
 	if (_offboard_mission.current_index < (int)_offboard_mission.count) {
+		warnx("theoretically possible");
 		dm_item_t dm_current;
 		if (_offboard_mission.dataman_id == 0) {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
@@ -356,8 +372,11 @@ Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *curren
 
 			report_current_offboard_mission_item();
 			return true;
+		} else {
+			warnx("read fail");
 		}
 	}
+	warnx("failed with offboard mission");
 	return false;
 }
 
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index e86dd25bb..65a0991b5 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -47,9 +47,11 @@
 
 #include <dataman/dataman.h>
 
+#include <uORB/uORB.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
 
 #include "mission_feasibility_checker.h"
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 1838fe32b..927e77391 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -103,8 +103,6 @@ private:
 	int		_home_pos_sub;			/**< home position subscription */
 	int		_vstatus_sub;			/**< vehicle status subscription */
 	int		_params_sub;			/**< notification of parameter updates */
-	int		_offboard_mission_sub;		/**< notification of offboard mission updates */
-	int 		_onboard_mission_sub;		/**< notification of onboard mission updates */
 	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
 	int		_control_mode_sub;		/**< vehicle control mode subscription */
 
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index f91032196..9dd253127 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -103,8 +103,6 @@ Navigator::Navigator() :
 	_home_pos_sub(-1),
 	_vstatus_sub(-1),
 	_params_sub(-1),
-	_offboard_mission_sub(-1),
-	_onboard_mission_sub(-1),
 	_capabilities_sub(-1),
 	_control_mode_sub(-1),
 	_pos_sp_triplet_pub(-1),
@@ -248,8 +246,6 @@ Navigator::task_main()
 
 	/* do subscriptions */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
-	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
 	_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
@@ -360,6 +356,8 @@ Navigator::task_main()
 			case NAVIGATION_STATE_ACRO:
 			case NAVIGATION_STATE_ALTCTL:
 			case NAVIGATION_STATE_POSCTL:
+				_mission.reset();
+				_rtl.reset();
 				break;
 			case NAVIGATION_STATE_AUTO_MISSION:
 				_update_triplet = _mission.update(&_pos_sp_triplet);
@@ -660,8 +658,6 @@ Navigator::start_land()
 	_update_triplet = true;
 	return true;
 }
-#endif
-#if 0
 bool
 Navigator::check_mission_item_reached()
 {
@@ -766,6 +762,7 @@ Navigator::reset_reached()
 	_waypoint_yaw_reached = false;
 
 }
+#endif
 void
 Navigator::publish_position_setpoint_triplet()
 {
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index 9d7886aa6..dc87b0521 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -57,9 +57,7 @@ RTL::RTL(Navigator *navigator) :
 	_rtl_state(RTL_STATE_NONE),
 	_home_position({}),
 	_loiter_radius(50),
-	_acceptance_radius(50),
-	_param_return_alt(this, "RETURN_ALT"),
-	_param_descend_alt(this, "DESCEND_ALT"),
+	_acceptance_radius(50)
 	_param_land_delay(this, "LAND_DELAY")
 {
 	/* load initial params */