Added TeraRanger one sensor

1 files changed, 33 insertions, 17 deletions

diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 4da6a1155..ea7cc0927 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -36,6 +36,8 @@
  * Navigator mode to access missions
  *
  * @author Julian Oes <julian@oes.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #ifndef NAVIGATOR_MISSION_H
@@ -75,6 +77,11 @@ public:
 
 	virtual void on_active();
 
+	enum mission_altitude_mode {
+		MISSION_ALTMODE_ZOH = 0,
+		MISSION_ALTMODE_FOH = 1
+	};
+
 private:
 	/**
 	 * Update onboard mission topic
@@ -103,6 +110,16 @@ private:
 	void set_mission_items();
 
 	/**
+	 * Updates the altitude sp to follow a foh
+	 */
+	void altitude_sp_foh_update();
+
+	/**
+	 * Resets the altitude sp foh logic
+	 */
+	void altitude_sp_foh_reset();
+
+	/**
 	 * Read current or next mission item from the dataman and watch out for DO_JUMPS
 	 * @return true if successful
 	 */
@@ -114,39 +131,32 @@ private:
 	void save_offboard_mission_state();
 
 	/**
-	 * Report that a mission item has been reached
-	 */
-	void report_mission_item_reached();
-
-	/**
-	 * Rport the current mission item
+	 * Set a mission item as reached
 	 */
-	void report_current_offboard_mission_item();
+	void set_mission_item_reached();
 
 	/**
-	 * Report that the mission is finished if one exists or that none exists
+	 * Set the current offboard mission item
 	 */
-	void report_mission_finished();
+	void set_current_offboard_mission_item();
 
 	/**
-	 * Publish the mission result so commander and mavlink know what is going on
+	 * Set that the mission is finished if one exists or that none exists
 	 */
-	void publish_mission_result();
+	void set_mission_finished();
 
 	control::BlockParamInt _param_onboard_enabled;
 	control::BlockParamFloat _param_takeoff_alt;
 	control::BlockParamFloat _param_dist_1wp;
+	control::BlockParamInt _param_altmode;
 
 	struct mission_s _onboard_mission;
 	struct mission_s _offboard_mission;
 
 	int _current_onboard_mission_index;
 	int _current_offboard_mission_index;
-	bool _need_takeoff;
-	bool _takeoff;
-
-	orb_advert_t _mission_result_pub;
-	struct mission_result_s _mission_result;
+	bool _need_takeoff;					/**< if true, then takeoff must be performed before going to the first waypoint (if needed) */
+	bool _takeoff;						/**< takeoff state flag */
 
 	enum {
 		MISSION_TYPE_NONE,
@@ -157,7 +167,13 @@ private:
 	bool _inited;
 	bool _dist_1wp_ok;
 
-	MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */
+	MissionFeasibilityChecker _missionFeasiblityChecker; /**< class that checks if a mission is feasible */
+
+	float _min_current_sp_distance_xy; /**< minimum distance which was achieved to the current waypoint  */
+	float _mission_item_previous_alt; /**< holds the altitude of the previous mission item,
+					    can be replaced by a full copy of the previous mission item if needed*/
+	float _distance_current_previous; /**< distance from previous to current sp in pos_sp_triplet,
+					    only use if current and previous are valid */
 };
 
 #endif