navigator: mission class added (WIP)

1 files changed, 315 insertions, 123 deletions

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index adbb0cfa2..f3a86666f 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -38,30 +38,42 @@
  * @author Julian Oes <julian@oes.ch>
  */
 
+#include <sys/types.h>
 #include <string.h>
 #include <stdlib.h>
+#include <unistd.h>
+
+#include <drivers/drv_hrt.h>
 
 #include <dataman/dataman.h>
 #include <systemlib/err.h>
+#include <geo/geo.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/mission_result.h>
 
+#include "navigator.h"
 #include "mission.h"
 
 
-Mission::Mission() :
-
-	_offboard_dataman_id(-1),
-	_current_offboard_mission_index(0),
-	_current_onboard_mission_index(0),
-	_offboard_mission_item_count(0),
-	_onboard_mission_item_count(0),
-	_onboard_mission_allowed(false),
-	_current_mission_type(MISSION_TYPE_NONE),
+Mission::Mission(Navigator *navigator) :
+	SuperBlock(NULL, "MIS"),
+	_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}),
 	_mission_result_pub(-1),
-	_mission_result({})
+	_mission_result({0}),
+	_mission_type(MISSION_TYPE_NONE)
 {
+	/* load initial params */
+	updateParams();
+	/* set initial mission items */
+	reset();
 }
 
 Mission::~Mission()
@@ -69,134 +81,335 @@ Mission::~Mission()
 }
 
 void
-Mission::set_offboard_dataman_id(const int new_id)
+Mission::reset()
 {
-	_offboard_dataman_id = new_id;
+	_first_run = true;
 }
 
-void
-Mission::set_offboard_mission_count(int new_count)
+bool
+Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
-	_offboard_mission_item_count = new_count;
+	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) {
+		set_mission_items(pos_sp_triplet);
+		updated = true;
+		_first_run = false;
+	}
+
+	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
+		advance_mission();
+		set_mission_items(pos_sp_triplet);
+		updated = true;
+	}
+	return updated;
 }
 
-void
-Mission::set_onboard_mission_count(int new_count)
+bool
+Mission::is_mission_item_reached()
 {
-	_onboard_mission_item_count = new_count;
+	/* TODO: count turns */
+#if 0
+	if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+	     _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
+	    _mission_item.loiter_radius > 0.01f) {
+
+		return false;
+	}
+#endif
+
+	hrt_abstime now = hrt_absolute_time();
+
+	if (!_waypoint_position_reached) {
+
+		float dist = -1.0f;
+		float dist_xy = -1.0f;
+		float dist_z = -1.0f;
+
+		float altitude_amsl = _mission_item.altitude_is_relative
+				      ? _mission_item.altitude + _navigator->get_home_position()->alt
+			              : _mission_item.altitude;
+
+		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
+				                          _navigator->get_global_position()->lat,
+							  _navigator->get_global_position()->lon,
+							  _navigator->get_global_position()->alt,
+				&dist_xy, &dist_z);
+
+		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
+
+			/* require only altitude for takeoff */
+			if (_navigator->get_global_position()->alt > altitude_amsl - _mission_item.acceptance_radius) {
+				_waypoint_position_reached = true;
+			}
+		} else {
+			if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {
+				_waypoint_position_reached = true;
+			}
+		}
+	}
+
+	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
+
+		/* TODO: removed takeoff, why? */
+		if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
+
+			/* check yaw if defined only for rotary wing except takeoff */
+			float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
+
+			if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
+				_waypoint_yaw_reached = true;
+			}
+
+		} else {
+			_waypoint_yaw_reached = true;
+		}
+	}
+
+	/* check if the current waypoint was reached */
+	if (_waypoint_position_reached && _waypoint_yaw_reached) {
+
+		if (_time_first_inside_orbit == 0) {
+			_time_first_inside_orbit = now;
+
+			// if (_mission_item.time_inside > 0.01f) {
+			// 	mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs",
+			// 		(double)_mission_item.time_inside);
+			// }
+		}
+
+		/* check if the MAV was long enough inside the waypoint orbit */
+		if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) {
+			return true;
+		}
+	}
+	return false;
 }
 
 void
-Mission::set_onboard_mission_allowed(bool allowed)
-{
-	_onboard_mission_allowed = allowed;
+Mission::reset_mission_item_reached() {
+	_waypoint_position_reached = false;
+	_waypoint_yaw_reached = false;
+	_time_first_inside_orbit = 0;
 }
 
-bool
-Mission::command_current_offboard_mission_index(const int new_index)
+void
+Mission::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp)
 {
-	if (new_index >= 0) {
-		_current_offboard_mission_index = (unsigned)new_index;
-	} else {
+	sp->valid = true;
 
-		/* if less WPs available, reset to first WP */
-		if (_current_offboard_mission_index >= _offboard_mission_item_count) {
-			_current_offboard_mission_index = 0;
-		}
+	sp->lat = item->lat;
+	sp->lon = item->lon;
+	sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;
+	sp->yaw = item->yaw;
+	sp->loiter_radius = item->loiter_radius;
+	sp->loiter_direction = item->loiter_direction;
+	sp->pitch_min = item->pitch_min;
+
+	if (item->nav_cmd == NAV_CMD_TAKEOFF) {
+		sp->type = SETPOINT_TYPE_TAKEOFF;
+
+	} else if (item->nav_cmd == NAV_CMD_LAND) {
+		sp->type = SETPOINT_TYPE_LAND;
+
+	} else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+		   item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+		   item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
+		sp->type = SETPOINT_TYPE_LOITER;
+
+	} else {
+		sp->type = SETPOINT_TYPE_POSITION;
 	}
-	report_current_offboard_mission_item();
 }
 
 bool
-Mission::command_current_onboard_mission_index(int new_index)
+Mission::is_onboard_mission_updated()
 {
-	if (new_index != -1) {
-		_current_onboard_mission_index = (unsigned)new_index;
-	} else {
+	bool updated;
+	orb_check(_onboard_mission_sub, &updated);
 
-		/* if less WPs available, reset to first WP */
-		if (_current_onboard_mission_index >= _onboard_mission_item_count) {
-			_current_onboard_mission_index = 0;
-		}
+	if (!updated) {
+		return false;
 	}
-	// TODO: implement this for onboard missions as well
-	// report_current_mission_item();
+
+	if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &_onboard_mission) != OK) {
+		_onboard_mission.count = 0;
+		_onboard_mission.current_index = 0;
+	}
+	return true;
 }
 
 bool
-Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, int *index)
+Mission::is_offboard_mission_updated()
 {
-	*onboard = false;
-	*index = -1;
-
-	/* try onboard mission first */
-	if (_current_onboard_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) {
-		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, new_mission_item)) {
-			_current_mission_type = MISSION_TYPE_ONBOARD;
-			*onboard = true;
-			*index = _current_onboard_mission_index;
-
-			return true;
-		}
+	bool updated;
+	orb_check(_offboard_mission_sub, &updated);
+	if (!updated) {
+		return false;
 	}
 
-	/* otherwise fallback to offboard */
-	if (_current_offboard_mission_index < _offboard_mission_item_count) {
+	if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &_offboard_mission) == OK) {
 
+		/* Check mission feasibility, for now do not handle the return value,
+		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
 		dm_item_t dm_current;
-		if (_offboard_dataman_id == 0) {
+
+		if (_offboard_mission.dataman_id == 0) {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
 		} else {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
 		}
-		if (read_mission_item(dm_current, true, &_current_offboard_mission_index, new_mission_item)) {
 
-			_current_mission_type = MISSION_TYPE_OFFBOARD;
-			*onboard = false;
-			*index = _current_offboard_mission_index;
+		missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current,
+			                                      (size_t)_offboard_mission.count,
+							      _navigator->get_geofence());
+	} else {
+		_offboard_mission.count = 0;
+		_offboard_mission.current_index = 0;
+	}
+	return true;
+}
 
-			return true;
-		}
+
+void
+Mission::advance_mission()
+{
+	switch (_mission_type) {
+	case MISSION_TYPE_ONBOARD:
+		_onboard_mission.current_index++;
+		break;
+
+	case MISSION_TYPE_OFFBOARD:
+		_offboard_mission.current_index++;
+		break;
+
+	case MISSION_TYPE_NONE:
+	default:
+		break;
 	}
+}
+
+void
+Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+	warnx("set mission items");
+
+	set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous);
 
-	/* happens when no more mission items can be added as a next item */
-	_current_mission_type = MISSION_TYPE_NONE;
+	if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) {
+		/* try setting onboard mission item */
+		_mission_type = MISSION_TYPE_ONBOARD;
 
+	} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
+		/* try setting offboard mission item */
+		_mission_type = MISSION_TYPE_OFFBOARD;
+	} else {
+		_mission_type = MISSION_TYPE_NONE;
+	}
+}
+
+void
+Mission::set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp,
+		                   struct position_setpoint_s *previous_pos_sp)
+{
+	/* reuse current setpoint as previous setpoint */
+	if (current_pos_sp->valid) {
+		memcpy(previous_pos_sp, current_pos_sp, sizeof(struct position_setpoint_s));
+	}
+}
+
+bool
+Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
+{
+	if (_param_onboard_enabled.get() > 0
+	    && _onboard_mission.current_index < (int)_onboard_mission.count) {
+		struct mission_item_s new_mission_item;
+		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_onboard_mission.current_index,
+					&new_mission_item)) {
+			/* convert the current mission item and set it valid */
+			mission_item_to_position_setpoint(&_mission_item, current_pos_sp);
+			current_pos_sp->valid = true;
+
+			/* TODO: report this somehow */
+			return true;
+		}
+	}
 	return false;
 }
 
 bool
-Mission::get_next_mission_item(struct mission_item_s *new_mission_item)
+Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
 {
-	int next_temp_mission_index = _current_onboard_mission_index + 1;
+	if (_offboard_mission.current_index < (int)_offboard_mission.count) {
+		dm_item_t dm_current;
+		if (_offboard_mission.dataman_id == 0) {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+		} else {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+		}
+		struct mission_item_s new_mission_item;
+		if (read_mission_item(dm_current, true, &_offboard_mission.current_index, &new_mission_item)) {
+			/* convert the current mission item and set it valid */
+			mission_item_to_position_setpoint(&_mission_item, current_pos_sp);
+			current_pos_sp->valid = true;
 
-	/* try onboard mission first */
-	if (next_temp_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) {
-		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, new_mission_item)) {
+			report_current_offboard_mission_item();
 			return true;
 		}
 	}
+	return false;
+}
 
-	/* then try offboard mission */
-	dm_item_t dm_current;
-	if (_offboard_dataman_id == 0) {
-		dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
-	} else {
-		dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
-	}
-	next_temp_mission_index = _current_offboard_mission_index + 1;
-	if (next_temp_mission_index < _offboard_mission_item_count) {
-		if (read_mission_item(dm_current, false, &next_temp_mission_index, new_mission_item)) {
-			return true;
+void
+Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp)
+{
+	int next_temp_mission_index = _onboard_mission.current_index + 1;
+
+	/* try if there is a next onboard mission */
+	if (next_temp_mission_index < (int)_onboard_mission.count) {
+		struct mission_item_s new_mission_item;
+		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) {
+			/* convert next mission item to position setpoint */
+			mission_item_to_position_setpoint(&new_mission_item, next_pos_sp);
+			next_pos_sp->valid = true;
+			return;
 		}
 	}
 
-	/* both failed, bail out */
-	return false;
+	/* give up */
+	next_pos_sp->valid = false;
+	return;
+}
+
+void
+Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp)
+{
+	/* try if there is a next offboard mission */
+	int next_temp_mission_index = _offboard_mission.current_index + 1;
+	if (next_temp_mission_index < (int)_offboard_mission.count) {
+		dm_item_t dm_current;
+		if (_offboard_mission.dataman_id == 0) {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+		} else {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+		}
+		struct mission_item_s new_mission_item;
+		if (read_mission_item(dm_current, false, &next_temp_mission_index, &new_mission_item)) {
+			/* convert next mission item to position setpoint */
+			mission_item_to_position_setpoint(&new_mission_item, next_pos_sp);
+			next_pos_sp->valid = true;
+			return;
+		}
+	}
+	/* give up */
+	next_pos_sp->valid = false;
+	return;
 }
 
 bool
-Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, struct mission_item_s *new_mission_item)
+Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index,
+		           struct mission_item_s *new_mission_item)
 {
 	/* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
 	for (int i=0; i<10; i++) {
@@ -211,25 +424,25 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
 		/* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */
 		if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) {
 
-			if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) {
+			if (new_mission_item->do_jump_current_count >= new_mission_item->do_jump_repeat_count) {
+				return false;
+			}
 
-				/* only raise the repeat count if this is for the current mission item
-				 * but not for the next mission item */
-				if (is_current) {
-					(new_mission_item->do_jump_current_count)++;
+			/* only raise the repeat count if this is for the current mission item
+			 * but not for the next mission item */
+			if (is_current) {
+				(new_mission_item->do_jump_current_count)++;
 
-					/* save repeat count */
-					if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, new_mission_item, len) != len) {
-						/* not supposed to happen unless the datamanager can't access the SD card, etc. */
-						return false;
-					}
+				/* save repeat count */
+				if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET,
+					     new_mission_item, len) != len) {
+					/* not supposed to happen unless the datamanager can't access the dataman */
+					return false;
 				}
-				/* set new mission item index and repeat
-				 * we don't have to validate here, if it's invalid, we should realize this later .*/
-				*mission_index = new_mission_item->do_jump_mission_index;
-			} else {
-				return false;
 			}
+			/* set new mission item index and repeat
+			 * we don't have to validate here, if it's invalid, we should realize this later .*/
+			*mission_index = new_mission_item->do_jump_mission_index;
 
 		} else {
 			/* if it's not a DO_JUMP, then we were successful */
@@ -243,31 +456,11 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
 }
 
 void
-Mission::move_to_next()
-{
-	report_mission_item_reached();
-
-	switch (_current_mission_type) {
-	case MISSION_TYPE_ONBOARD:
-		_current_onboard_mission_index++;
-		break;
-
-	case MISSION_TYPE_OFFBOARD:
-		_current_offboard_mission_index++;
-		break;
-
-	case MISSION_TYPE_NONE:
-	default:
-		break;
-	}
-}
-
-void
 Mission::report_mission_item_reached()
 {
-	if (_current_mission_type == MISSION_TYPE_OFFBOARD) {
+	if (_mission_type == MISSION_TYPE_OFFBOARD) {
 		_mission_result.mission_reached = true;
-		_mission_result.mission_index_reached = _current_offboard_mission_index;
+		_mission_result.mission_index_reached = _offboard_mission.current_index;
 	}
 	publish_mission_result();
 }
@@ -275,7 +468,7 @@ Mission::report_mission_item_reached()
 void
 Mission::report_current_offboard_mission_item()
 {
-	_mission_result.index_current_mission = _current_offboard_mission_index;
+	_mission_result.index_current_mission = _offboard_mission.current_index;
 	publish_mission_result();
 }
 
@@ -294,4 +487,3 @@ Mission::publish_mission_result()
 	/* reset reached bool */
 	_mission_result.mission_reached = false;
 }
-