Merge branch 'fw_autoland' of github.com:PX4/Firmware into fw_autoland

2 files changed, 62 insertions, 22 deletions

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 348a17ba6..219c6ffa6 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -635,6 +635,9 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 	/* no throttle limit as default */
 	float throttle_max = 1.0f;
 
+	/* not in non-abort mode for landing yet */
+	bool land_noreturn = false;
+
 	/* AUTONOMOUS FLIGHT */
 
 	// XXX this should only execute if auto AND safety off (actuators active),
@@ -717,23 +720,39 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 
 			} else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) {
 
-				_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
+				/* switch to heading hold for the last meters, continue heading hold after */
+
+				float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), next_wp.getX(), next_wp.getY());
+
+				if (wp_distance < 5.0f || land_noreturn) {
+
+					/* heading hold, along the line connecting this and the last waypoint */
+					float target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY());
+					_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed);
+
+					if (altitude_error > -20.0f)
+						land_noreturn = true;
+
+				} else {
+
+					/* normal navigation */
+					_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
+				}
+
 				_att_sp.roll_body = _l1_control.nav_roll();
 				_att_sp.yaw_body = _l1_control.nav_bearing();
 
 				/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
 				// XXX this could make a great param
 
-				if (altitude_error > -10.0f) {
-
-					float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1)
-					float land_pitch_min = math::radians(5.0f);
-					float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
+				float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1)
+				float land_pitch_min = math::radians(5.0f);
+				float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
+				float airspeed_land = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f;
 
-					// /* set the speed weight to 0.0 to push the system to control altitude with pitch */
-					// _tecs.set_speed_weight(0.0f);
+				if (altitude_error > -5.0f) {
 
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, _parameters.airspeed_min,
+					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min),
 								    _airspeed.indicated_airspeed_m_s, eas2tas,
 								    false, flare_angle_rad,
 								    0.0f, _parameters.throttle_max, throttle_land,
@@ -743,7 +762,15 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 					throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
 
 					/* limit roll motion to prevent wings from touching the ground first */
-					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-20.0f), math::radians(20.0f));
+					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f));
+
+				} else if (altitude_error > -20.0f) {
+
+					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land),
+								    _airspeed.indicated_airspeed_m_s, eas2tas,
+								    false, flare_angle_rad,
+								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+								    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
 
 				} else {
 
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index ddad5f0df..adaf81404 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -399,6 +399,13 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 			if (time_elapsed) {
 
 				if (cur_wp->autocontinue) {
+
+					/* safeguard against invalid missions with last wp autocontinue on */
+					if (wpm->current_active_wp_id == wpm->size - 1) {
+						/* stop handling missions here */
+						cur_wp->autocontinue = false;
+					}
+
 					cur_wp->current = 0;
 
 					float navigation_lat = -1.0f;
@@ -419,13 +426,16 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 						navigation_frame = MAV_FRAME_LOCAL_NED;
 					}
 
+					/* guard against missions without final land waypoint */
 					/* only accept supported navigation waypoints, skip unknown ones */
 					do {
+
 						/* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */
 						if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
+				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM ||
+				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_TAKEOFF) {
 
 							/* this is a navigation waypoint */
 							navigation_frame = cur_wp->frame;
@@ -434,17 +444,20 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 							navigation_alt = cur_wp->z;
 						}
 
-						if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) {
-							/* the last waypoint was reached, if auto continue is
-							 * activated keep the system loitering there.
-							 */
-							cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM;
-							cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius
-							cur_wp->frame = navigation_frame;
-							cur_wp->x = navigation_lat;
-							cur_wp->y = navigation_lon;
-							cur_wp->z = navigation_alt;
-							cur_wp->autocontinue = false;
+						if (wpm->current_active_wp_id == wpm->size - 1) {
+
+							/* if we're not landing at the last nav waypoint, we're falling back to loiter */
+							if (wpm->waypoints[wpm->current_active_wp_id].command != (int)MAV_CMD_NAV_LAND) {
+								/* the last waypoint was reached, if auto continue is
+								 * activated AND it is NOT a land waypoint, keep the system loitering there.
+								 */
+								cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM;
+								cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius
+								cur_wp->frame = navigation_frame;
+								cur_wp->x = navigation_lat;
+								cur_wp->y = navigation_lon;
+								cur_wp->z = navigation_alt;
+							}
 
 							/* we risk an endless loop for missions without navigation waypoints, abort. */
 							break;