Merge branch 'master' of github.com:PX4/Firmware

ROMFS/px4fmu_common/init.d/rc.mc_defaults

@@ -41,6 +41,7 @@ then
 	param set PE_POSNE_NOISE 0.5
 	param set PE_POSD_NOISE 1.0
 
+	param set NAV_ACCEPT_RAD 2.0
 fi
 
 set PWM_RATE 400

src/modules/mavlink/mavlink_main.cpp

@@ -105,7 +105,8 @@ static struct file_operations fops;
  */
 extern "C" __EXPORT int mavlink_main(int argc, char *argv[]);
 
-static uint64_t last_write_times[6] = {0};
+static uint64_t last_write_success_times[6] = {0};
+static uint64_t last_write_try_times[6] = {0};
 
 /*
  * Internal function to send the bytes through the right serial port
@@ -166,26 +167,25 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 	if (instance->get_flow_control_enabled()
 		&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
 
-		if (buf_free == 0) {
-
-			if (last_write_times[(unsigned)channel] != 0 &&
-				hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) {
-
+		/* Disable hardware flow control:
+		 * if no successful write since a defined time
+		 * and if the last try was not the last successful write
+		 */
+		if (last_write_try_times[(unsigned)channel] != 0 &&
+			hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL &&
+			last_write_success_times[(unsigned)channel] !=
+			last_write_try_times[(unsigned)channel])
+		{
 			warnx("DISABLING HARDWARE FLOW CONTROL");
 			instance->enable_flow_control(false);
 		}
 
-		} else {
-
-			/* apparently there is space left, although we might be
-			 * partially overflooding the buffer already */
-			last_write_times[(unsigned)channel] = hrt_absolute_time();
-		}
 	}
 
 	/* If the wait until transmit flag is on, only transmit after we've received messages.
 	   Otherwise, transmit all the time. */
 	if (instance->should_transmit()) {
+		last_write_try_times[(unsigned)channel] = hrt_absolute_time();
 
 		/* check if there is space in the buffer, let it overflow else */
 		if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) {
@@ -199,6 +199,8 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 		ssize_t ret = write(uart, ch, desired);
 		if (ret != desired) {
 			warnx("TX FAIL");
+		} else {
+			last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
 		}
 	}
 

src/modules/navigator/navigator_main.cpp

@@ -1458,7 +1458,6 @@ Navigator::check_mission_item_reached()
 
 	/* XXX TODO count turns */
 	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) {
 
@@ -1477,6 +1476,13 @@ Navigator::check_mission_item_reached()
 			acceptance_radius = _parameters.acceptance_radius;
 		}
 
+		if (_do_takeoff) {
+			/* require only altitude for takeoff */
+			if (_global_pos.alt > _pos_sp_triplet.current.alt - acceptance_radius) {
+				_waypoint_position_reached = true;
+			}
+
+		} else {
 			float dist = -1.0f;
 			float dist_xy = -1.0f;
 			float dist_z = -1.0f;
@@ -1491,13 +1497,6 @@ Navigator::check_mission_item_reached()
 					(double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt,
 					&dist_xy, &dist_z);
 
-		if (_do_takeoff) {
-			if (_global_pos.alt > wp_alt_amsl - acceptance_radius) {
-				/* require only altitude for takeoff */
-				_waypoint_position_reached = true;
-			}
-
-		} else {
 			if (dist >= 0.0f && dist <= acceptance_radius) {
 				_waypoint_position_reached = true;
 			}
@@ -1567,8 +1566,15 @@ Navigator::on_mission_item_reached()
 		}
 
 		if (_mission.current_mission_available()) {
+			if (_mission_item.autocontinue) {
+				/* continue mission */
 				set_mission_item();
 
+			} else {
+				/* autocontinue disabled for this item */
+				request_loiter_or_ready();
+			}
+
 		} else {
 			/* if no more mission items available then finish mission */
 			/* loiter at last waypoint */