Merge remote-tracking branch 'upstream/master' into mtecs

2 files changed, 58 insertions, 56 deletions

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 87ec4293e..6d06f897a 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -65,12 +65,12 @@ then
 	# Start CDC/ACM serial driver
 	#
 	sercon
-	
+
 	#
 	# Start the ORB (first app to start)
 	#
 	uorb start
-	
+
 	#
 	# Load parameters
 	#
@@ -79,7 +79,7 @@ then
 	then
 		set PARAM_FILE /fs/mtd_params
 	fi
-	
+
 	param select $PARAM_FILE
 	if param load
 	then
@@ -87,7 +87,7 @@ then
 	else
 		echo "[init] ERROR: Params loading failed: $PARAM_FILE"
 	fi
-	
+
 	#
 	# Start system state indicator
 	#
@@ -105,7 +105,7 @@ then
 	if pca8574 start
 	then
 	fi
-	
+
 	#
 	# Set default values
 	#
@@ -126,7 +126,7 @@ then
 	set LOAD_DEFAULT_APPS yes
 	set GPS yes
 	set GPS_FAKE no
-	
+
 	#
 	# Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts
 	#
@@ -136,7 +136,7 @@ then
 	else
 		set DO_AUTOCONFIG no
 	fi
-	
+
 	#
 	# Set USE_IO flag
 	#
@@ -146,7 +146,7 @@ then
 	else
 		set USE_IO no
 	fi
-		
+
 	#
 	# Set parameters and env variables for selected AUTOSTART
 	#
@@ -176,9 +176,9 @@ then
 		param set SYS_AUTOCONFIG 0
 		param save
 	fi
-	
+
 	set IO_PRESENT no
-	
+
 	if [ $USE_IO == yes ]
 	then
 		#
@@ -190,19 +190,19 @@ then
 		else
 			set IO_FILE /etc/extras/px4io-v1_default.bin
 		fi
-		
+
 		if px4io checkcrc $IO_FILE
 		then
 			echo "[init] PX4IO CRC OK"
 			echo "PX4IO CRC OK" >> $LOG_FILE
-	
+
 			set IO_PRESENT yes
 		else
 			echo "[init] Trying to update"
 			echo "PX4IO Trying to update" >> $LOG_FILE
-			
+
 			tone_alarm MLL32CP8MB
-			
+
 			if px4io forceupdate 14662 $IO_FILE
 			then
 				usleep 500000
@@ -211,7 +211,7 @@ then
 					echo "[init] PX4IO CRC OK, update successful"
 					echo "PX4IO CRC OK after updating" >> $LOG_FILE
 					tone_alarm MLL8CDE
-	
+
 					set IO_PRESENT yes
 				else
 					echo "[init] ERROR: PX4IO update failed"
@@ -224,14 +224,14 @@ then
 				tone_alarm $TUNE_OUT_ERROR
 			fi
 		fi
-		
+
 		if [ $IO_PRESENT == no ]
 		then
 			echo "[init] ERROR: PX4IO not found"
 			tone_alarm $TUNE_OUT_ERROR
 		fi
 	fi
-	
+
 	#
 	# Set default output if not set
 	#
@@ -250,7 +250,7 @@ then
 		# Need IO for output but it not present, disable output
 		set OUTPUT_MODE none
 		echo "[init] ERROR: PX4IO not found, disabling output"
-		
+
 		# Avoid using ttyS0 for MAVLink on FMUv1
 		if ver hwcmp PX4FMU_V1
 		then
@@ -274,17 +274,17 @@ then
 
 	# Try to get an USB console
 	nshterm /dev/ttyACM0 &
-	
+
 	#
 	# Start the Commander (needs to be this early for in-air-restarts)
 	#
 	commander start
-	
+
 	#
 	# Start primary output
 	#
 	set TTYS1_BUSY no
-	
+
 	# If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output
 	if [ $OUTPUT_MODE != none ]
 	then
@@ -300,7 +300,7 @@ then
 				tone_alarm $TUNE_OUT_ERROR
 			fi
 		fi
-		
+
 		if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ]
 		then
 			echo "[init] Use FMU as primary output"
@@ -311,7 +311,7 @@ then
 				echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
 				tone_alarm $TUNE_OUT_ERROR
 			fi
-			
+
 			if ver hwcmp PX4FMU_V1
 			then
 				if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
@@ -324,7 +324,7 @@ then
 				fi
 			fi
 		fi
-		
+
 		if [ $OUTPUT_MODE == mkblctrl ]
 		then
 			echo "[init] Use MKBLCTRL as primary output"
@@ -337,7 +337,7 @@ then
 			then
 				set MKBLCTRL_ARG "-mkmode +"
 			fi
-			
+
 			if mkblctrl $MKBLCTRL_ARG
 			then
 				echo "[init] MKBLCTRL started"
@@ -345,9 +345,9 @@ then
 				echo "[init] ERROR: MKBLCTRL start failed"
 				tone_alarm $TUNE_OUT_ERROR
 			fi
-			
+
 		fi
-				
+
 		if [ $OUTPUT_MODE == hil ]
 		then
 			echo "[init] Use HIL as primary output"
@@ -359,7 +359,7 @@ then
 				tone_alarm $TUNE_OUT_ERROR
 			fi
 		fi
-		
+
 		#
 		# Start IO or FMU for RC PPM input if needed
 		#
@@ -386,7 +386,7 @@ then
 					echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
 					tone_alarm $TUNE_OUT_ERROR
 				fi
-	
+
 				if ver hwcmp PX4FMU_V1
 				then
 					if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
@@ -401,7 +401,7 @@ then
 			fi
 		fi
 	fi
-	
+
 	#
 	# MAVLink
 	#
@@ -422,7 +422,7 @@ then
 	fi
 
 	mavlink start $MAVLINK_FLAGS
-	
+
 	#
 	# Sensors, Logging, GPS
 	#
@@ -433,7 +433,7 @@ then
 		echo "[init] Start logging"
 		sh /etc/init.d/rc.logging
 	fi
-	
+
 	if [ $GPS == yes ]
 	then
 		echo "[init] Start GPS"
@@ -443,7 +443,7 @@ then
 			gps start -f
 		else
 			gps start
-		fi		
+		fi
 	fi
 
 	#
@@ -460,24 +460,24 @@ then
 	if [ $VEHICLE_TYPE == fw ]
 	then
 		echo "[init] Vehicle type: FIXED WING"
-		
+
 		if [ $MIXER == none ]
 		then
 			# Set default mixer for fixed wing if not defined
 			set MIXER FMU_AERT
 		fi
-		
+
 		if [ $MAV_TYPE == none ]
 		then
 			# Use MAV_TYPE = 1 (fixed wing) if not defined
 			set MAV_TYPE 1
 		fi
-		
+
 		param set MAV_TYPE $MAV_TYPE
-		
+
 		# Load mixer and configure outputs
 		sh /etc/init.d/rc.interface
-		
+
 		# Start standard fixedwing apps
 		if [ $LOAD_DEFAULT_APPS == yes ]
 		then
@@ -525,7 +525,7 @@ then
 				set MAV_TYPE 14
 			fi
 		fi
-		
+
 		# Still no MAV_TYPE found
 		if [ $MAV_TYPE == none ]
 		then
@@ -533,10 +533,10 @@ then
 		else
 			param set MAV_TYPE $MAV_TYPE
 		fi
-		
+
 		# Load mixer and configure outputs
 		sh /etc/init.d/rc.interface
-		
+
 		# Start standard multicopter apps
 		if [ $LOAD_DEFAULT_APPS == yes ]
 		then
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 28cdf65a3..540b88657 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/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) {
-
-				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();
+		/* 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);
 		}
+
 	}
 
 	/* 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];
 		}
 	}