WIP, datalinkloss: implementing basic behavior

3 files changed, 130 insertions, 222 deletions

diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp
index 17e85b284..2bd80165d 100644
--- a/src/modules/navigator/datalinkloss.cpp
+++ b/src/modules/navigator/datalinkloss.cpp
@@ -57,10 +57,14 @@
 
 DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
-	_rtl_state(RTL_STATE_NONE),
-	_param_return_alt(this, "RETURN_ALT"),
-	_param_descend_alt(this, "DESCEND_ALT"),
-	_param_land_delay(this, "LAND_DELAY")
+	_dll_state(DLL_STATE_NONE),
+	_param_commsholdwaittime(this, "CH_T"),
+	_param_commsholdlat(this, "CH_LAT"),
+	_param_commsholdlon(this, "CH_LON"),
+	_param_commsholdalt(this, "CH_ALT"),
+	_param_airfieldhomelat(this, "AH_LAT"),
+	_param_airfieldhomelon(this, "AH_LON"),
+	_param_airfieldhomealt(this, "AH_ALT")
 {
 	/* load initial params */
 	updateParams();
@@ -77,7 +81,7 @@ DataLinkLoss::on_inactive()
 {
 	/* reset RTL state only if setpoint moved */
 	if (!_navigator->get_can_loiter_at_sp()) {
-		_rtl_state = RTL_STATE_NONE;
+		_dll_state = DLL_STATE_NONE;
 	}
 }
 
@@ -85,40 +89,40 @@ void
 DataLinkLoss::on_activation()
 {
 	/* decide where to enter the RTL procedure when we switch into it */
-	if (_rtl_state == RTL_STATE_NONE) {
-		/* for safety reasons don't go into RTL if landed */
-		if (_navigator->get_vstatus()->condition_landed) {
-			_rtl_state = RTL_STATE_LANDED;
-			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
-
-		/* if lower than return altitude, climb up first */
-		} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
-			   + _param_return_alt.get()) {
-			_rtl_state = RTL_STATE_CLIMB;
-
-		/* otherwise go straight to return */
-		} else {
-			/* set altitude setpoint to current altitude */
-			_rtl_state = RTL_STATE_RETURN;
-			_mission_item.altitude_is_relative = false;
-			_mission_item.altitude = _navigator->get_global_position()->alt;
-		}
-	}
-
-	set_rtl_item();
+	//if (_rtl_state == RTL_STATE_NONE) {
+		//[> for safety reasons don't go into RTL if landed <]
+		//if (_navigator->get_vstatus()->condition_landed) {
+			//_rtl_state = RTL_STATE_LANDED;
+			//mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
+
+		//[> if lower than return altitude, climb up first <]
+		//} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+			   //+ _param_return_alt.get()) {
+			//_rtl_state = RTL_STATE_CLIMB;
+
+		//[> otherwise go straight to return <]
+		//} else {
+			//[> set altitude setpoint to current altitude <]
+			//_rtl_state = RTL_STATE_RETURN;
+			//_mission_item.altitude_is_relative = false;
+			//_mission_item.altitude = _navigator->get_global_position()->alt;
+		//}
+	//}
+	_dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
+	set_dll_item();
 }
 
 void
 DataLinkLoss::on_active()
 {
-	if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
-		advance_rtl();
-		set_rtl_item();
+	if (is_mission_item_reached()) {
+		advance_dll();
+		set_dll_item();
 	}
 }
 
 void
-DataLinkLoss::set_rtl_item()
+DataLinkLoss::set_dll_item()
 {
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
@@ -128,146 +132,43 @@ DataLinkLoss::set_rtl_item()
 	set_previous_pos_setpoint();
 	_navigator->set_can_loiter_at_sp(false);
 
-	switch (_rtl_state) {
-	case RTL_STATE_CLIMB: {
-		float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();
-
-		_mission_item.lat = _navigator->get_global_position()->lat;
-		_mission_item.lon = _navigator->get_global_position()->lon;
-		_mission_item.altitude_is_relative = false;
-		_mission_item.altitude = climb_alt;
-		_mission_item.yaw = NAN;
-		_mission_item.loiter_radius = _navigator->get_loiter_radius();
-		_mission_item.loiter_direction = 1;
-		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
-		_mission_item.time_inside = 0.0f;
-		_mission_item.pitch_min = 0.0f;
-		_mission_item.autocontinue = true;
-		_mission_item.origin = ORIGIN_ONBOARD;
-
-		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home",
-			(int)(climb_alt - _navigator->get_home_position()->alt));
-		break;
-	}
-
-	case RTL_STATE_RETURN: {
-		_mission_item.lat = _navigator->get_home_position()->lat;
-		_mission_item.lon = _navigator->get_home_position()->lon;
-		 // don't change altitude
-
-		 if (pos_sp_triplet->previous.valid) {
-		 	/* if previous setpoint is valid then use it to calculate heading to home */
-		 	_mission_item.yaw = get_bearing_to_next_waypoint(
-		 	        pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
-		 	        _mission_item.lat, _mission_item.lon);
-
-		 } else {
-		 	/* else use current position */
-		 	_mission_item.yaw = get_bearing_to_next_waypoint(
-		 	        _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
-		 	        _mission_item.lat, _mission_item.lon);
-		 }
-		_mission_item.loiter_radius = _navigator->get_loiter_radius();
-		_mission_item.loiter_direction = 1;
-		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
-		_mission_item.time_inside = 0.0f;
-		_mission_item.pitch_min = 0.0f;
-		_mission_item.autocontinue = true;
-		_mission_item.origin = ORIGIN_ONBOARD;
-
-		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home",
-			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
-		break;
-	}
-
-	case RTL_STATE_DESCEND: {
-		_mission_item.lat = _navigator->get_home_position()->lat;
-		_mission_item.lon = _navigator->get_home_position()->lon;
+	switch (_dll_state) {
+	case DLL_STATE_FLYTOCOMMSHOLDWP: {
+		_mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7;
+		_mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7;
 		_mission_item.altitude_is_relative = false;
-		_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
+		_mission_item.altitude = (double)(_param_commsholdalt.get());
 		_mission_item.yaw = NAN;
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
 		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
-		_mission_item.time_inside = 0.0f;
-		_mission_item.pitch_min = 0.0f;
-		_mission_item.autocontinue = false;
-		_mission_item.origin = ORIGIN_ONBOARD;
-
-		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home",
-			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
-		break;
-	}
-
-	case RTL_STATE_LOITER: {
-		bool autoland = _param_land_delay.get() > -DELAY_SIGMA;
-
-		_mission_item.lat = _navigator->get_home_position()->lat;
-		_mission_item.lon = _navigator->get_home_position()->lon;
-		_mission_item.altitude_is_relative = false;
-		_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
-		_mission_item.yaw = NAN;
-		_mission_item.loiter_radius = _navigator->get_loiter_radius();
-		_mission_item.loiter_direction = 1;
-		_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
-		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
-		_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
-		_mission_item.pitch_min = 0.0f;
-		_mission_item.autocontinue = autoland;
-		_mission_item.origin = ORIGIN_ONBOARD;
-
-		_navigator->set_can_loiter_at_sp(true);
-
-		if (autoland) {
-			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", (double)_mission_item.time_inside);
-
-		} else {
-			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter");
-		}
-		break;
-	}
-
-	case RTL_STATE_LAND: {
-		_mission_item.lat = _navigator->get_home_position()->lat;
-		_mission_item.lon = _navigator->get_home_position()->lon;
-		_mission_item.altitude_is_relative = false;
-		_mission_item.altitude = _navigator->get_home_position()->alt;
-		_mission_item.yaw = NAN;
-		_mission_item.loiter_radius = _navigator->get_loiter_radius();
-		_mission_item.loiter_direction = 1;
-		_mission_item.nav_cmd = NAV_CMD_LAND;
-		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
-		_mission_item.time_inside = 0.0f;
+		_mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get();
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = true;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home");
+		_navigator->set_can_loiter_at_sp(true);
 		break;
 	}
-
-	case RTL_STATE_LANDED: {
-		_mission_item.lat = _navigator->get_home_position()->lat;
-		_mission_item.lon = _navigator->get_home_position()->lon;
+	case DLL_STATE_FLYTOAIRFIELDHOMEWP: {
+		_mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7;
+		_mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7;
 		_mission_item.altitude_is_relative = false;
-		_mission_item.altitude = _navigator->get_home_position()->alt;
+		_mission_item.altitude = (double)(_param_airfieldhomealt.get());
 		_mission_item.yaw = NAN;
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
-		_mission_item.nav_cmd = NAV_CMD_IDLE;
+		_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
 		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
-		_mission_item.time_inside = 0.0f;
+		_mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get();
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = true;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, landed");
+		_navigator->set_can_loiter_at_sp(true);
 		break;
 	}
-
 	default:
 		break;
 	}
@@ -282,35 +183,16 @@ DataLinkLoss::set_rtl_item()
 }
 
 void
-DataLinkLoss::advance_rtl()
+DataLinkLoss::advance_dll()
 {
-	switch (_rtl_state) {
-	case RTL_STATE_CLIMB:
-		_rtl_state = RTL_STATE_RETURN;
+	switch (_dll_state) {
+	case DLL_STATE_NONE:
+		_dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
 		break;
-
-	case RTL_STATE_RETURN:
-		_rtl_state = RTL_STATE_DESCEND;
+	case DLL_STATE_FLYTOCOMMSHOLDWP:
+		//XXX check here if time is over are over
+		_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 		break;
-
-	case RTL_STATE_DESCEND:
-		/* only go to land if autoland is enabled */
-		if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) {
-			_rtl_state = RTL_STATE_LOITER;
-
-		} else {
-			_rtl_state = RTL_STATE_LAND;
-		}
-		break;
-
-	case RTL_STATE_LOITER:
-		_rtl_state = RTL_STATE_LAND;
-		break;
-
-	case RTL_STATE_LAND:
-		_rtl_state = RTL_STATE_LANDED;
-		break;
-
 	default:
 		break;
 	}
diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h
index 242cfac8d..101c88a25 100644
--- a/src/modules/navigator/datalinkloss.h
+++ b/src/modules/navigator/datalinkloss.h
@@ -68,28 +68,27 @@ public:
 
 private:
 	/**
-	 * Set the RTL item
+	 * Set the DLL item
 	 */
-	void		set_rtl_item();
+	void		set_dll_item();
 
 	/**
-	 * Move to next RTL item
+	 * Move to next DLL item
 	 */
-	void		advance_rtl();
+	void		advance_dll();
 
-	enum RTLState {
-		RTL_STATE_NONE = 0,
-		RTL_STATE_CLIMB,
-		RTL_STATE_RETURN,
-		RTL_STATE_DESCEND,
-		RTL_STATE_LOITER,
-		RTL_STATE_LAND,
-		RTL_STATE_LANDED,
-	} _rtl_state;
+	enum DLLState {
+		DLL_STATE_NONE = 0,
+		DLL_STATE_FLYTOCOMMSHOLDWP = 1,
+		DLL_STATE_FLYTOAIRFIELDHOMEWP = 2,
+	} _dll_state;
 
-	control::BlockParamFloat _param_return_alt;
-	control::BlockParamFloat _param_descend_alt;
-	control::BlockParamFloat _param_land_delay;
+	control::BlockParamFloat _param_commsholdwaittime;
+	control::BlockParamInt _param_commsholdlat; // * 1e7
+	control::BlockParamInt _param_commsholdlon; // * 1e7
+	control::BlockParamFloat _param_commsholdalt;
+	control::BlockParamInt _param_airfieldhomelat; // * 1e7
+	control::BlockParamInt _param_airfieldhomelon; // * 1e7
+	control::BlockParamFloat _param_airfieldhomealt;
 };
-
 #endif
diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c
index bfe6ce7e1..a836fc8ca 100644
--- a/src/modules/navigator/datalinkloss_params.c
+++ b/src/modules/navigator/datalinkloss_params.c
@@ -32,11 +32,11 @@
  ****************************************************************************/
 
 /**
- * @file rtl_params.c
+ * @file datalinkloss_params.c
  *
- * Parameters for RTL
+ * Parameters for DLL
  *
- * @author Julian Oes <julian@oes.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #include <nuttx/config.h>
@@ -44,55 +44,82 @@
 #include <systemlib/param/param.h>
 
 /*
- * RTL parameters, accessible via MAVLink
+ * Data Link Loss parameters, accessible via MAVLink
  */
 
 /**
- * Loiter radius after RTL (FW only)
+ * Comms hold wait time
  *
- * Default value of loiter radius after RTL (fixedwing only).
+ * The amount of time in seconds the system should wait at the comms hold waypoint
  *
- * @unit meters
+ * @unit seconds
  * @min 0.0
- * @group RTL
+ * @group DLL
  */
-PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f);
+PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f);
 
 /**
- * RTL altitude
+ * Comms hold Lat
  *
- * Altitude to fly back in RTL in meters
+ * Latitude of comms hold waypoint
  *
- * @unit meters
- * @min 0
- * @max 1
- * @group RTL
+ * @unit degrees * 1e7
+ * @min 0.0
+ * @group DLL
  */
-PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100);
+PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, 266072120);
 
+/**
+ * Comms hold Lon
+ *
+ * Longitude of comms hold waypoint
+ *
+ * @unit degrees * 1e7
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890);
 
 /**
- * RTL loiter altitude
+ * Comms hold alt
  *
- * Stay at this altitude above home position after RTL descending.
- * Land (i.e. slowly descend) from this altitude if autolanding allowed.
+ * Altitude of comms hold waypoint
  *
- * @unit meters
- * @min 0
- * @max 100
- * @group RTL
+ * @unit m
+ * @min 0.0
+ * @group DLL
  */
-PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20);
+PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f);
 
 /**
- * RTL delay
+ * Airfield home Lat
  *
- * Delay after descend before landing in RTL mode.
- * If set to -1 the system will not land but loiter at NAV_LAND_ALT.
+ * Latitude of airfield home waypoint
  *
- * @unit seconds
- * @min -1
- * @max
- * @group RTL
+ * @unit degrees * 1e7
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_INT32(NAV_DLL_AH_LAT, 265847810);
+
+/**
+ * Airfield home Lon
+ *
+ * Longitude of airfield home waypoint
+ *
+ * @unit degrees * 1e7
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_INT32(NAV_DLL_AH_LON, 1518423250);
+
+/**
+ * Airfield home alt
+ *
+ * Altitude of airfield home waypoint
+ *
+ * @unit m
+ * @min 0.0
+ * @group DLL
  */
-PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
+PARAM_DEFINE_FLOAT(NAV_DLL_AH_ALT, 600.0f);