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

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

Conflicts:
	makefiles/firmware.mk
	src/modules/commander/module.mk
	src/modules/mavlink/mavlink_ftp.h
	src/modules/mavlink/mavlink_tests/module.mk

ROMFS/px4fmu_common/init.d/10015_tbs_discovery

@@ -28,3 +28,10 @@ set MIXER quad_w
 
 set PWM_OUT 1234
 set PWM_MIN 1200
+
+set MIXER_AUX pass
+set PWM_AUX_RATE 50
+set PWM_AUX_OUT 1234
+set PWM_AUX_DISARMED 1000
+set PWM_AUX_MIN 1000
+set PWM_AUX_MAX 2000

ROMFS/px4fmu_common/mixers/pass.aux.mix

@@ -0,0 +1,21 @@
+# Manual pass through mixer for servo outputs 1-4
+
+# AUX1 channel (select RC channel with RC_MAP_AUX1 param)
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 3 5  10000  10000      0 -10000  10000
+
+# AUX2 channel (select RC channel with RC_MAP_AUX2 param)
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 3 6  10000  10000      0 -10000  10000
+
+# AUX3 channel (select RC channel with RC_MAP_AUX3 param)
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 3 7  10000  10000      0 -10000  10000
+
+# FLAPS channel (select RC channel with RC_MAP_FLAPS param)
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 3 4  10000  10000      0 -10000  10000

Tools/px4params/srcparser.py

@@ -57,10 +57,10 @@ class Parameter(object):
 
     def GetType(self):
         return self.type
-    
+
     def GetDefault(self):
         return self.default
-    
+
     def SetField(self, code, value):
         """
         Set named field value
@@ -80,6 +80,10 @@ class Parameter(object):
         """
         Return value of the given field code or None if not found.
         """
+        fv =  self.fields.get(code)
+        if not fv:
+                # required because python 3 sorted does not accept None
+                return ""
         return self.fields.get(code)
 
 class SourceParser(object):
@@ -89,7 +93,7 @@ class SourceParser(object):
 
     re_split_lines = re.compile(r'[\r\n]+')
     re_comment_start = re.compile(r'^\/\*\*')
-    re_comment_content = re.compile(r'^\*\s*(.*)') 
+    re_comment_content = re.compile(r'^\*\s*(.*)')
     re_comment_tag = re.compile(r'@([a-zA-Z][a-zA-Z0-9_]*)\s*(.*)')
     re_comment_end = re.compile(r'(.*?)\s*\*\/')
     re_parameter_definition = re.compile(r'PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*,\s*([^ ,\)]+)\s*\)\s*;')

Tools/px4params/xmlout.py

@@ -52,5 +52,4 @@ class XMLOutput():
         self.xml_document = ET.ElementTree(xml_parameters)
 
     def Save(self, filename):
-        with codecs.open(filename, 'w', 'utf-8') as f:
-            self.xml_document.write(f)
+        self.xml_document.write(filename, encoding="UTF-8")

msg/actuator_armed.msg

@@ -4,3 +4,4 @@ bool armed		# Set to true if system is armed
 bool ready_to_arm	# Set to true if system is ready to be armed
 bool lockdown		# Set to true if actuators are forced to being disabled (due to emergency or HIL)
 bool force_failsafe	# Set to true if the actuators are forced to the failsafe position
+bool in_esc_calibration_mode # IO/FMU should ignore messages from the actuator controls topics

src/drivers/gimbal/gimbal.cpp

@@ -306,17 +306,17 @@ Gimbal::cycle()
 	orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att);
 
 	if (_attitude_compensation_roll) {
-		roll = -att.roll;
+		roll = 1.0f / M_PI_F * -att.roll;
 		updated = true;
 	}
 
 	if (_attitude_compensation_pitch) {
-		pitch = -att.pitch;
+		pitch = 1.0f / M_PI_F * -att.pitch;
 		updated = true;
 	}
 
 	if (_attitude_compensation_yaw) {
-		yaw = att.yaw;
+		yaw = 1.0f / M_PI_F * att.yaw;
 		updated = true;
 	}
 

src/drivers/px4io/px4io.cpp

@@ -302,6 +302,7 @@ private:
 	float			_battery_mamphour_total;///< amp hours consumed so far
 	uint64_t		_battery_last_timestamp;///< last amp hour calculation timestamp
 	bool			_cb_flighttermination;	///< true if the flight termination circuit breaker is enabled
+	bool 			_in_esc_calibration_mode;	///< do not send control outputs to IO (used for esc calibration)
 
 	int32_t			_rssi_pwm_chan; ///< RSSI PWM input channel
 	int32_t			_rssi_pwm_max; ///< max RSSI input on PWM channel
@@ -529,6 +530,7 @@ PX4IO::PX4IO(device::Device *interface) :
 	_battery_mamphour_total(0),
 	_battery_last_timestamp(0),
 	_cb_flighttermination(true),
+	_in_esc_calibration_mode(false),
 	_rssi_pwm_chan(0),
 	_rssi_pwm_max(0),
 	_rssi_pwm_min(0)
@@ -1167,12 +1169,29 @@ PX4IO::io_set_control_state(unsigned group)
 		break;
 	}
 
-	if (!changed) {
+	if (!changed && (!_in_esc_calibration_mode || group != 0)) {
 		return -1;
+
+	} else if (_in_esc_calibration_mode && group == 0) {
+		/* modify controls to get max pwm (full thrust) on every esc */
+		memset(&controls, 0, sizeof(controls));
+
+		/* set maximum thrust */
+		controls.control[3] = 1.0f;
 	}
 
 	for (unsigned i = 0; i < _max_controls; i++) {
-		regs[i] = FLOAT_TO_REG(controls.control[i]);
+
+		/* ensure FLOAT_TO_REG does not produce an integer overflow */
+		float ctrl = controls.control[i];
+
+		if (ctrl < -1.0f) {
+			ctrl = -1.0f;
+		} else if (ctrl > 1.0f) {
+			ctrl = 1.0f;
+		}
+
+		regs[i] = FLOAT_TO_REG(ctrl);
 	}
 
 	/* copy values to registers in IO */
@@ -1188,12 +1207,14 @@ PX4IO::io_set_arming_state()
 
 	int have_armed = orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed);
 	int have_control_mode = orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode);
+	_in_esc_calibration_mode = armed.in_esc_calibration_mode;
 
 	uint16_t set = 0;
 	uint16_t clear = 0;
 
-        if (have_armed == OK) {
-		if (armed.armed) {
+    if (have_armed == OK) {
+		_in_esc_calibration_mode = armed.in_esc_calibration_mode;
+		if (armed.armed || _in_esc_calibration_mode) {
 			set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 		} else {
 			clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;

src/lib/ecl/attitude_fw/ecl_roll_controller.cpp

@@ -38,7 +38,7 @@
  * Authors and acknowledgements in header.
  */
 
-#include "../ecl.h"
+#include <ecl/ecl.h>
 #include "ecl_roll_controller.h"
 #include <stdint.h>
 #include <float.h>

src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp

@@ -45,7 +45,7 @@
 #include <ecl/ecl.h>
 #include <mathlib/mathlib.h>
 #include <systemlib/err.h>
-#include <drivers/drv_hrt.h>
+#include <ecl/ecl.h>
 
 ECL_YawController::ECL_YawController() :
 	ECL_Controller("yaw"),
@@ -70,9 +70,9 @@ float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data
 	default:
 		static hrt_abstime last_print = 0;
 
-		if (hrt_elapsed_time(&last_print) > 5e6) {
+		if (ecl_elapsed_time(&last_print) > 5e6) {
 			warnx("invalid param setting FW_YCO_METHOD");
-			last_print = hrt_absolute_time();
+			last_print = ecl_absolute_time();
 		}
 	}
 
@@ -89,9 +89,9 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data
 	default:
 		static hrt_abstime last_print = 0;
 
-		if (hrt_elapsed_time(&last_print) > 5e6) {
+		if (ecl_elapsed_time(&last_print) > 5e6) {
 			warnx("invalid param setting FW_YCO_METHOD");
-			last_print = hrt_absolute_time();
+			last_print = ecl_absolute_time();
 		}
 	}
 

src/modules/commander/calibration_routines.cpp

@@ -47,12 +47,12 @@
 #include <float.h>
 #include <poll.h>
 #include <drivers/drv_hrt.h>
-#include <drivers/drv_accel.h>
 #include <mavlink/mavlink_log.h>
 #include <geo/geo.h>
 #include <string.h>
 
 #include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/sensor_combined.h>
 
 #include "calibration_routines.h"
 #include "calibration_messages.h"
@@ -237,7 +237,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
 {
 	const unsigned ndim = 3;
 	
-	struct accel_report sensor;
+	struct sensor_combined_s sensor;
 	float		accel_ema[ndim] = { 0.0f };		// exponential moving average of accel
 	float		accel_disp[3] = { 0.0f, 0.0f, 0.0f };	// max-hold dispersion of accel
 	float		ema_len = 0.5f;				// EMA time constant in seconds
@@ -265,7 +265,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
 		int poll_ret = px4_poll(fds, 1, 1000);
 		
 		if (poll_ret) {
-			orb_copy(ORB_ID(sensor_accel), accel_sub, &sensor);
+			orb_copy(ORB_ID(sensor_combined), accel_sub, &sensor);
 			t = hrt_absolute_time();
 			float dt = (t - t_prev) / 1000000.0f;
 			t_prev = t;
@@ -276,13 +276,13 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
 				float di = 0.0f;
 				switch (i) {
 					case 0:
-						di = sensor.x;
+						di = sensor.accelerometer_m_s2[0];
 						break;
 					case 1:
-						di = sensor.y;
+						di = sensor.accelerometer_m_s2[1];
 						break;
 					case 2:
-						di = sensor.z;
+						di = sensor.accelerometer_m_s2[2];
 						break;
 				}
 				
@@ -411,7 +411,7 @@ calibrate_return calibrate_from_orientation(int		mavlink_fd,
 	
 	// Setup subscriptions to onboard accel sensor
 	
-	int sub_accel = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
+	int sub_accel = orb_subscribe(ORB_ID(sensor_combined));
 	if (sub_accel < 0) {
 		mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "No onboard accel");
 		return calibrate_return_error;

src/modules/commander/commander.cpp

@@ -119,6 +119,7 @@
 #include "baro_calibration.h"
 #include "rc_calibration.h"
 #include "airspeed_calibration.h"
+#include "esc_calibration.h"
 #include "PreflightCheck.h"
 
 /* oddly, ERROR is not defined for c++ */
@@ -1166,7 +1167,7 @@ int commander_thread_main(int argc, char *argv[])
 	/* initialize low priority thread */
 	pthread_attr_t commander_low_prio_attr;
 	pthread_attr_init(&commander_low_prio_attr);
-	pthread_attr_setstacksize(&commander_low_prio_attr, 2000);
+	pthread_attr_setstacksize(&commander_low_prio_attr, 2600);
 
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
@@ -2717,6 +2718,16 @@ void *commander_low_prio_loop(void *arg)
 						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 						calib_ret = do_airspeed_calibration(mavlink_fd);
 
+					} else if ((int)(cmd.param7) == 1) {
+						/* do esc calibration */
+						calib_ret = check_if_batt_disconnected(mavlink_fd);
+						if(calib_ret == OK) {
+							answer_command(cmd,VEHICLE_CMD_RESULT_ACCEPTED);
+							armed.in_esc_calibration_mode = true;
+							calib_ret = do_esc_calibration(mavlink_fd);
+							armed.in_esc_calibration_mode = false;
+						}
+
 					} else if ((int)(cmd.param4) == 0) {
 						/* RC calibration ended - have we been in one worth confirming? */
 						if (status.rc_input_blocked) {
@@ -2726,6 +2737,8 @@ void *commander_low_prio_loop(void *arg)
 							mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN");
                             calib_ret = OK;
 						}
+						/* this always succeeds */
+						calib_ret = OK;
 					}
 
 					if (calib_ret == OK) {

src/modules/commander/esc_calibration.cpp

@@ -0,0 +1,152 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file esc_calibration.cpp
+ *
+ * Definition of esc calibration
+ *
+ * @author Roman Bapst <bapstr@ethz.ch>
+ */
+
+#include "esc_calibration.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <systemlib/err.h>
+#include <fcntl.h>
+#include <poll.h>
+#include "drivers/drv_pwm_output.h"
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/uORB.h>
+#include <drivers/drv_hrt.h>
+#include <mavlink/mavlink_log.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+int check_if_batt_disconnected(int mavlink_fd) {
+	struct battery_status_s battery;
+	memset(&battery,0,sizeof(battery));
+	int batt_sub = orb_subscribe(ORB_ID(battery_status));
+	orb_copy(ORB_ID(battery_status), batt_sub, &battery);
+
+	if (battery.voltage_filtered_v > 3.0f && !(hrt_absolute_time() - battery.timestamp > 500000)) {
+		mavlink_log_info(mavlink_fd, "Please disconnect battery and try again!");
+		return ERROR;
+	}
+	return OK;
+}
+
+
+int do_esc_calibration(int mavlink_fd) {
+
+	int fd = open(PWM_OUTPUT0_DEVICE_PATH, 0);
+	int ret;
+
+	if(fd < 0) {
+		err(1,"Can't open %s", PWM_OUTPUT0_DEVICE_PATH);
+	}
+
+	/* tell IO/FMU that its ok to disable its safety with the switch */
+	ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
+	if (ret != OK)
+		err(1, "PWM_SERVO_SET_ARM_OK");
+	/* tell IO/FMU that the system is armed (it will output values if safety is off) */
+	ret = ioctl(fd, PWM_SERVO_ARM, 0);
+	if (ret != OK)
+		err(1, "PWM_SERVO_ARM");
+	/* tell IO to switch off safety without using the safety switch */
+	ret = ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0);
+	if(ret!=0) {
+		err(1,"PWM_SERVO_SET_FORCE_SAFETY_OFF");
+	}
+
+	mavlink_log_info(mavlink_fd,"Please connect battery now");
+
+	struct battery_status_s battery;
+	memset(&battery,0,sizeof(battery));
+	int batt_sub = orb_subscribe(ORB_ID(battery_status));
+	orb_copy(ORB_ID(vehicle_command),batt_sub, &battery);
+	bool updated = false;
+
+	struct vehicle_command_s cmd;
+	memset(&cmd, 0, sizeof(cmd));
+	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
+	orb_copy(ORB_ID(vehicle_command),cmd_sub, &cmd);
+
+	/* wait for one of the following events:
+		1) user has pressed the button in QGroundControl
+		2) timeout of 5 seconds is reached
+	*/
+	hrt_abstime start_time = hrt_absolute_time();
+
+	while(true) {
+		orb_check(batt_sub,&updated);
+		if(updated) {
+			orb_copy(ORB_ID(battery_status), batt_sub, &battery);
+		}
+		// user has connected battery
+		if(battery.voltage_filtered_v > 3.0f) {
+			orb_check(cmd_sub,&updated);
+			if(updated) {
+				orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
+			}
+			if((int)(cmd.param7) == 2 && cmd.command == VEHICLE_CMD_PREFLIGHT_CALIBRATION) {
+				break;
+			} else if (hrt_absolute_time() - start_time > 5000000) {
+				// waited for 5 seconds, switch to low pwm
+				break;
+			}
+		}
+		else {
+			start_time = hrt_absolute_time();
+		}
+		usleep(50000);
+	}
+
+	/* disarm */
+	ret = ioctl(fd, PWM_SERVO_DISARM, 0);
+	if (ret != OK)
+		err(1, "PWM_SERVO_DISARM");
+
+	mavlink_log_info(mavlink_fd,"ESC calibration finished");
+	return OK;
+ }

src/modules/commander/esc_calibration.h

@@ -0,0 +1,47 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file esc_calibration.h
+ *
+ * Definition of esc calibration
+ *
+ * @author Roman Bapst <bapstr@ethz.ch>
+ */
+
+#ifndef ESC_CALIBRATION_H_
+#define ESC_CALIBRATION_H_
+int check_if_batt_disconnected(int mavlink_fd);
+int do_esc_calibration(int mavlink_fd);
+
+#endif

src/modules/commander/module.mk

@@ -38,16 +38,17 @@
 MODULE_COMMAND	 	= commander
 SRCS		 	= commander.cpp \
 			commander_params.c \
+			state_machine_helper.cpp \
 			commander_helper.cpp \
 			calibration_routines.cpp \
-			mag_calibration.cpp \
-			gyro_calibration.cpp \
-			baro_calibration.cpp \
 			accelerometer_calibration.cpp \
+			gyro_calibration.cpp \
+			mag_calibration.cpp \
+			baro_calibration.cpp \
 			rc_calibration.cpp \
 			airspeed_calibration.cpp \
-			PreflightCheck.cpp \
-			state_machine_helper.cpp 
+			esc_calibration.cpp \
+			PreflightCheck.cpp
 
 MODULE_STACKSIZE = 5000
 

src/modules/mavlink/mavlink_ftp.cpp

@@ -42,116 +42,130 @@
 #include <errno.h>
 
 #include "mavlink_ftp.h"
+#include "mavlink_main.h"
 #include "mavlink_tests/mavlink_ftp_test.h"
 
 // Uncomment the line below to get better debug output. Never commit with this left on.
 //#define MAVLINK_FTP_DEBUG
 
-MavlinkFTP *
-MavlinkFTP::get_server(void)
+int buf_size_1 = 0;
+int buf_size_2 = 0;
+
+MavlinkFTP::MavlinkFTP(Mavlink* mavlink) :
+	MavlinkStream(mavlink),
+	_session_info{},
+	_utRcvMsgFunc{},
+	_worker_data{}
 {
-	static MavlinkFTP server;
-        return &server;
+	// initialize session
+	_session_info.fd = -1;
 }
 
-MavlinkFTP::MavlinkFTP() :
-	_request_bufs{},
-	_request_queue{},
-	_request_queue_sem{},
-	_utRcvMsgFunc{},
-	_ftp_test{}
+MavlinkFTP::~MavlinkFTP()
 {
-	// initialise the request freelist
-	dq_init(&_request_queue);
-	sem_init(&_request_queue_sem, 0, 1);
+	
+}
 
-	// initialize session list
-	for (size_t i=0; i<kMaxSession; i++) {
-		_session_fds[i] = -1;
-	}
-
-	// drop work entries onto the free list
-	for (unsigned i = 0; i < kRequestQueueSize; i++) {
-		_return_request(&_request_bufs[i]);
+const char*
+MavlinkFTP::get_name(void) const
+{
+	return "MAVLINK_FTP";
+}
+
+uint8_t
+MavlinkFTP::get_id(void)
+{
+	return MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL;
+}
+
+unsigned
+MavlinkFTP::get_size(void)
+{
+	if (_session_info.stream_download) {
+		return MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		
+	} else {
+		return 0;
 	}
+}
 
+MavlinkStream*
+MavlinkFTP::new_instance(Mavlink *mavlink)
+{
+	return new MavlinkFTP(mavlink);
 }
 
 #ifdef MAVLINK_FTP_UNIT_TEST
 void
-MavlinkFTP::set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, MavlinkFtpTest *ftp_test)
+MavlinkFTP::set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, void *worker_data)
 {
 	_utRcvMsgFunc = rcvMsgFunc;
-	_ftp_test = ftp_test;
+	_worker_data = worker_data;
 }
 #endif
 
-void
-MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg)
+uint8_t
+MavlinkFTP::_getServerSystemId(void)
 {
-	// get a free request
-	struct Request* req = _get_request();
-
-	// if we couldn't get a request slot, just drop it
-	if (req == nullptr) {
-		warnx("Dropping FTP request: queue full\n");
-		return;
-	}
-
-	if (msg->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
-		mavlink_msg_file_transfer_protocol_decode(msg, &req->message);
-		
 #ifdef MAVLINK_FTP_UNIT_TEST
-		if (!_utRcvMsgFunc) {
-			warnx("Incorrectly written unit test\n");
+	// We use fake ids when unit testing
+	return MavlinkFtpTest::serverSystemId;
+#else
+	// Not unit testing, use the real thing
+	return _mavlink->get_system_id();
+#endif
+}
+
+uint8_t
+MavlinkFTP::_getServerComponentId(void)
+{
+#ifdef MAVLINK_FTP_UNIT_TEST
+	// We use fake ids when unit testing
+	return MavlinkFtpTest::serverComponentId;
+#else
+	// Not unit testing, use the real thing
+	return _mavlink->get_component_id();
+#endif
+}
+
+uint8_t
+MavlinkFTP::_getServerChannel(void)
+{
+#ifdef MAVLINK_FTP_UNIT_TEST
+	// We use fake ids when unit testing
+	return MavlinkFtpTest::serverChannel;
+#else
+	// Not unit testing, use the real thing
+	return _mavlink->get_channel();
+#endif
+}
+
+void
+MavlinkFTP::handle_message(const mavlink_message_t *msg)
+{
+	//warnx("MavlinkFTP::handle_message %d %d", buf_size_1, buf_size_2);
+	
+	if (msg->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
+		mavlink_file_transfer_protocol_t ftp_request;
+		mavlink_msg_file_transfer_protocol_decode(msg, &ftp_request);
+		
+#ifdef MAVLINK_FTP_DEBUG
+		warnx("FTP: received ftp protocol message target_system: %d", ftp_request.target_system);
+#endif
+		
+		if (ftp_request.target_system == _getServerSystemId()) {
+			_process_request(&ftp_request, msg->sysid);
 			return;
 		}
-		// We use fake ids when unit testing
-		req->serverSystemId = MavlinkFtpTest::serverSystemId;
-		req->serverComponentId = MavlinkFtpTest::serverComponentId;
-		req->serverChannel = MavlinkFtpTest::serverChannel;
-#else
-		// Not unit testing, use the real thing
-		req->serverSystemId = mavlink->get_system_id();
-		req->serverComponentId = mavlink->get_component_id();
-		req->serverChannel = mavlink->get_channel();
-#endif
-		
-		// This is the system id we want to target when sending
-		req->targetSystemId = msg->sysid;
-			
-		if (req->message.target_system == req->serverSystemId) {
-			req->mavlink = mavlink;
-#ifdef MAVLINK_FTP_UNIT_TEST
-			// We are running in Unit Test mode. Don't queue, just call _worket directly.
-			_process_request(req);
-#else
-			// We are running in normal mode. Queue the request to the worker
-			work_queue(LPWORK, &req->work, &MavlinkFTP::_worker_trampoline, req, 0);
-#endif
-            return;
-		}
 	}
-
-	_return_request(req);
-}
-
-/// @brief Queued static work queue routine to handle mavlink messages
-void
-MavlinkFTP::_worker_trampoline(void *arg)
-{
-	Request* req = reinterpret_cast<Request *>(arg);
-	MavlinkFTP* server = MavlinkFTP::get_server();
-
-	// call the server worker with the work item
-	server->_process_request(req);
 }
 
 /// @brief Processes an FTP message
 void
-MavlinkFTP::_process_request(Request *req)
+MavlinkFTP::_process_request(mavlink_file_transfer_protocol_t* ftp_req, uint8_t target_system_id)
 {
-	PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&req->message.payload[0]);
+	bool stream_send = false;
+	PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&ftp_req->payload[0]);
 
 	ErrorCode errorCode = kErrNone;
 
@@ -162,7 +176,7 @@ MavlinkFTP::_process_request(Request *req)
 	}
 
 #ifdef MAVLINK_FTP_DEBUG
-	printf("ftp: channel %u opc %u size %u offset %u\n", req->serverChannel, payload->opcode, payload->size, payload->offset);
+	printf("ftp: channel %u opc %u size %u offset %u\n", _getServerChannel(), payload->opcode, payload->size, payload->offset);
 #endif
 
 	switch (payload->opcode) {
@@ -197,6 +211,11 @@ MavlinkFTP::_process_request(Request *req)
 		errorCode = _workRead(payload);
 		break;
 
+	case kCmdBurstReadFile:
+		errorCode = _workBurst(payload, target_system_id);
+		stream_send = true;
+		break;
+			
 	case kCmdWriteFile:
 		errorCode = _workWrite(payload);
 		break;
@@ -221,7 +240,6 @@ MavlinkFTP::_process_request(Request *req)
 		errorCode = _workRemoveDirectory(payload);
 		break;
 			
-
 	case kCmdCalcFileCRC32:
 		errorCode = _workCalcFileCRC32(payload);
 		break;
@@ -232,16 +250,14 @@ MavlinkFTP::_process_request(Request *req)
 	}
 
 out:
+	payload->seq_number++;
+	
 	// handle success vs. error
 	if (errorCode == kErrNone) {
 		payload->req_opcode = payload->opcode;
 		payload->opcode = kRspAck;
-#ifdef MAVLINK_FTP_DEBUG
-		warnx("FTP: ack\n");
-#endif
 	} else {
 		int r_errno = errno;
-		warnx("FTP: nak %u", errorCode);
 		payload->req_opcode = payload->opcode;
 		payload->opcode = kRspNak;
 		payload->size = 1;
@@ -252,60 +268,33 @@ out:
 		}
 	}
 
-	
-	// respond to the request
-	_reply(req);
-
-	_return_request(req);
+	// Stream download replies are sent through mavlink stream mechanism. Unless we need to Nak.
+	if (!stream_send || errorCode != kErrNone) {
+		// respond to the request
+		ftp_req->target_system = target_system_id;
+		_reply(ftp_req);
+	}
 }
 
-/// @brief Sends the specified FTP reponse message out through mavlink
+/// @brief Sends the specified FTP response message out through mavlink
 void
-MavlinkFTP::_reply(Request *req)
+MavlinkFTP::_reply(mavlink_file_transfer_protocol_t* ftp_req)
 {
-	PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&req->message.payload[0]);
 	
-	payload->seqNumber = payload->seqNumber + 1;
-
-	mavlink_message_t msg;
-	msg.checksum = 0;
-#ifndef MAVLINK_FTP_UNIT_TEST
-	uint16_t len =
+#ifdef MAVLINK_FTP_DEBUG
+	PayloadHeader *payload = reinterpret_cast<PayloadHeader *>(&ftp_req->payload[0]);
+	warnx("FTP: %s seq_number: %d", payload->opcode == kRspAck ? "Ack" : "Nak", payload->seq_number);
 #endif
-	mavlink_msg_file_transfer_protocol_pack_chan(req->serverSystemId,	// Sender system id
-								    req->serverComponentId,	// Sender component id
-								    req->serverChannel,		// Channel to send on
-								    &msg,			// Message to pack payload into
-								    0,				// Target network
-								    req->targetSystemId,	// Target system id
-								    0,				// Target component id
-								    (const uint8_t*)payload);	// Payload to pack into message
 	
-	bool success = true;
+	ftp_req->target_network = 0;
+	ftp_req->target_component = 0;
 #ifdef MAVLINK_FTP_UNIT_TEST
 	// Unit test hook is set, call that instead
-	_utRcvMsgFunc(&msg, _ftp_test);
+	_utRcvMsgFunc(ftp_req, _worker_data);
 #else
-	Mavlink	*mavlink = req->mavlink;
-	
-	mavlink->lockMessageBufferMutex();
-	success = mavlink->message_buffer_write(&msg, len);
-	mavlink->unlockMessageBufferMutex();
-		
+	_mavlink->send_message(MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL, ftp_req);
 #endif
 
-	if (!success) {
-		warnx("FTP TX ERR");
-	}
-#ifdef MAVLINK_FTP_DEBUG
-	else {
-		warnx("wrote: sys: %d, comp: %d, chan: %d, checksum: %d",
-		      req->serverSystemId,
-		      req->serverComponentId,
-		      req->serverChannel,
-		      msg.checksum);
-	}
-#endif
 }
 
 /// @brief Responds to a List command
@@ -425,13 +414,16 @@ MavlinkFTP::_workList(PayloadHeader* payload)
 MavlinkFTP::ErrorCode
 MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag)
 {
-	int session_index = _find_unused_session();
-	if (session_index < 0) {
+	if (_session_info.fd >= 0) {
 		warnx("FTP: Open failed - out of sessions\n");
 		return kErrNoSessionsAvailable;
 	}
 
 	char *filename = _data_as_cstring(payload);
+	
+#ifdef MAVLINK_FTP_DEBUG
+	warnx("FTP: open '%s'", filename);
+#endif
 
 	uint32_t fileSize = 0;
 	struct stat st;
@@ -448,9 +440,11 @@ MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag)
 	if (fd < 0) {
 		return kErrFailErrno;
 	}
-	_session_fds[session_index] = fd;
+	_session_info.fd = fd;
+	_session_info.file_size = fileSize;
+	_session_info.stream_download = false;
 
-	payload->session = session_index;
+	payload->session = 0;
 	payload->size = sizeof(uint32_t);
 	*((uint32_t*)payload->data) = fileSize;
 
@@ -461,23 +455,25 @@ MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag)
 MavlinkFTP::ErrorCode
 MavlinkFTP::_workRead(PayloadHeader* payload)
 {
-	int session_index = payload->session;
-
-	if (!_valid_session(session_index)) {
+	if (payload->session != 0 || _session_info.fd < 0) {
 		return kErrInvalidSession;
 	}
 
-	// Seek to the specified position
 #ifdef MAVLINK_FTP_DEBUG
-	warnx("seek %d", payload->offset);
+	warnx("FTP: read offset:%d", payload->offset);
 #endif
-	if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) {
-		// Unable to see to the specified location
-		warnx("seek fail");
+	// We have to test seek past EOF ourselves, lseek will allow seek past EOF
+	if (payload->offset >= _session_info.file_size) {
+		warnx("request past EOF");
 		return kErrEOF;
 	}
+		
+	if (lseek(_session_info.fd, payload->offset, SEEK_SET) < 0) {
+		warnx("seek fail");
+		return kErrFailErrno;
+	}
 
-	int bytes_read = ::read(_session_fds[session_index], &payload->data[0], kMaxDataLength);
+	int bytes_read = ::read(_session_info.fd, &payload->data[0], kMaxDataLength);
 	if (bytes_read < 0) {
 		// Negative return indicates error other than eof
 		warnx("read fail %d", bytes_read);
@@ -489,27 +485,41 @@ MavlinkFTP::_workRead(PayloadHeader* payload)
 	return kErrNone;
 }
 
+/// @brief Responds to a Stream command
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workBurst(PayloadHeader* payload, uint8_t target_system_id)
+{
+	if (payload->session != 0 && _session_info.fd < 0) {
+		return kErrInvalidSession;
+	}
+	
+#ifdef MAVLINK_FTP_DEBUG
+	warnx("FTP: burst offset:%d", payload->offset);
+#endif
+	// Setup for streaming sends
+	_session_info.stream_download = true;
+	_session_info.stream_offset = payload->offset;
+	_session_info.stream_seq_number = payload->seq_number + 1;
+	_session_info.stream_target_system_id = target_system_id;
+
+	return kErrNone;
+}
+
 /// @brief Responds to a Write command
 MavlinkFTP::ErrorCode
 MavlinkFTP::_workWrite(PayloadHeader* payload)
 {
-	int session_index = payload->session;
-
-	if (!_valid_session(session_index)) {
+	if (payload->session != 0 && _session_info.fd < 0) {
 		return kErrInvalidSession;
 	}
 
-	// Seek to the specified position
-#ifdef MAVLINK_FTP_DEBUG
-	warnx("seek %d", payload->offset);
-#endif
-	if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) {
+	if (lseek(_session_info.fd, payload->offset, SEEK_SET) < 0) {
 		// Unable to see to the specified location
 		warnx("seek fail");
 		return kErrFailErrno;
 	}
 
-	int bytes_written = ::write(_session_fds[session_index], &payload->data[0], payload->size);
+	int bytes_written = ::write(_session_info.fd, &payload->data[0], payload->size);
 	if (bytes_written < 0) {
 		// Negative return indicates error other than eof
 		warnx("write fail %d", bytes_written);
@@ -616,12 +626,13 @@ MavlinkFTP::_workTruncateFile(PayloadHeader* payload)
 MavlinkFTP::ErrorCode
 MavlinkFTP::_workTerminate(PayloadHeader* payload)
 {
-	if (!_valid_session(payload->session)) {
+	if (payload->session != 0 || _session_info.fd < 0) {
 		return kErrInvalidSession;
 	}
-    
-	::close(_session_fds[payload->session]);
-	_session_fds[payload->session] = -1;
+	
+	::close(_session_info.fd);
+	_session_info.fd = -1;
+	_session_info.stream_download = false;
 	
 	payload->size = 0;
 
@@ -632,11 +643,10 @@ MavlinkFTP::_workTerminate(PayloadHeader* payload)
 MavlinkFTP::ErrorCode
 MavlinkFTP::_workReset(PayloadHeader* payload)
 {
-	for (size_t i=0; i<kMaxSession; i++) {
-		if (_session_fds[i] != -1) {
-			::close(_session_fds[i]);
-			_session_fds[i] = -1;
-		}
+	if (_session_info.fd != -1) {
+		::close(_session_info.fd);
+		_session_info.fd = -1;
+		_session_info.stream_download = false;
 	}
 
 	payload->size = 0;
@@ -734,29 +744,6 @@ MavlinkFTP::_workCalcFileCRC32(PayloadHeader* payload)
 	return kErrNone;
 }
 
-/// @brief Returns true if the specified session is a valid open session
-bool
-MavlinkFTP::_valid_session(unsigned index)
-{
-	if ((index >= kMaxSession) || (_session_fds[index] < 0)) {
-		return false;
-	}
-	return true;
-}
-
-/// @brief Returns an unused session index
-int
-MavlinkFTP::_find_unused_session(void)
-{
-	for (size_t i=0; i<kMaxSession; i++) {
-		if (_session_fds[i] == -1) {
-			return i;
-		}
-	}
-
-	return -1;
-}
-
 /// @brief Guarantees that the payload data is null terminated.
 ///     @return Returns a pointer to the payload data as a char *
 char *
@@ -773,40 +760,6 @@ MavlinkFTP::_data_as_cstring(PayloadHeader* payload)
 	return (char *)&(payload->data[0]);
 }
 
-/// @brief Returns a unused Request entry. NULL if none available.
-MavlinkFTP::Request *
-MavlinkFTP::_get_request(void)
-{
-	_lock_request_queue();
-	Request* req = reinterpret_cast<Request *>(dq_remfirst(&_request_queue));
-	_unlock_request_queue();
-	return req;
-}
-
-/// @brief Locks a semaphore to provide exclusive access to the request queue
-void
-MavlinkFTP::_lock_request_queue(void)
-{
-	do {}
-	while (sem_wait(&_request_queue_sem) != 0);
-}
-
-/// @brief Unlocks the semaphore providing exclusive access to the request queue
-void
-MavlinkFTP::_unlock_request_queue(void)
-{
-	sem_post(&_request_queue_sem);
-}
-
-/// @brief Returns a no longer needed request to the queue
-void
-MavlinkFTP::_return_request(Request *req)
-{
-	_lock_request_queue();
-	dq_addlast(&req->work.dq, &_request_queue);
-	_unlock_request_queue();
-}
-
 /// @brief Copy file (with limited space)
 int
 MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length)
@@ -864,3 +817,106 @@ MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length
 	errno = op_errno;
 	return (length > 0)? -1 : 0;
 }
+
+void MavlinkFTP::send(const hrt_abstime t)
+{
+	// Anything to stream?
+	if (!_session_info.stream_download) {
+		return;
+	}
+	
+#ifndef MAVLINK_FTP_UNIT_TEST
+	// Skip send if not enough room
+	unsigned max_bytes_to_send = _mavlink->get_free_tx_buf();
+#ifdef MAVLINK_FTP_DEBUG
+    warnx("MavlinkFTP::send max_bytes_to_send(%d) get_free_tx_buf(%d)", max_bytes_to_send, _mavlink->get_free_tx_buf());
+#endif
+	if (max_bytes_to_send < get_size()) {
+		return;
+	}
+#endif
+	
+	// Send stream packets until buffer is full
+
+	bool more_data;
+	do {
+		more_data = false;
+		
+		ErrorCode error_code = kErrNone;
+		
+		mavlink_file_transfer_protocol_t ftp_msg;
+		PayloadHeader* payload = reinterpret_cast<PayloadHeader *>(&ftp_msg.payload[0]);
+		
+		payload->seq_number = _session_info.stream_seq_number;
+		payload->session = 0;
+		payload->opcode = kRspAck;
+		payload->req_opcode = kCmdBurstReadFile;
+		payload->offset = _session_info.stream_offset;
+		_session_info.stream_seq_number++;
+
+#ifdef MAVLINK_FTP_DEBUG
+		warnx("stream send: offset %d", _session_info.stream_offset);
+#endif
+		// We have to test seek past EOF ourselves, lseek will allow seek past EOF
+		if (_session_info.stream_offset >= _session_info.file_size) {
+			error_code = kErrEOF;
+#ifdef MAVLINK_FTP_DEBUG
+			warnx("stream download: sending Nak EOF");
+#endif
+		}
+		
+		if (error_code == kErrNone) {
+			if (lseek(_session_info.fd, payload->offset, SEEK_SET) < 0) {
+				error_code = kErrFailErrno;
+#ifdef MAVLINK_FTP_DEBUG
+				warnx("stream download: seek fail");
+#endif
+			}
+		}
+		
+		if (error_code == kErrNone) {
+			int bytes_read = ::read(_session_info.fd, &payload->data[0], kMaxDataLength);
+			if (bytes_read < 0) {
+				// Negative return indicates error other than eof
+				error_code = kErrFailErrno;
+#ifdef MAVLINK_FTP_DEBUG
+				warnx("stream download: read fail");
+#endif
+			} else {
+				payload->size = bytes_read;
+				_session_info.stream_offset += bytes_read;
+			}
+		}
+		
+		if (error_code != kErrNone) {
+			payload->opcode = kRspNak;
+			payload->size = 1;
+			uint8_t* pData = &payload->data[0];
+			*pData = error_code; // Straight reference to data[0] is causing bogus gcc array subscript error
+			if (error_code == kErrFailErrno) {
+				int r_errno = errno;
+				payload->size = 2;
+				payload->data[1] = r_errno;
+			}
+			_session_info.stream_download = false;
+		} else {
+#ifndef MAVLINK_FTP_UNIT_TEST
+			if (max_bytes_to_send < (get_size()*2)) {
+				more_data = false;
+				payload->burst_complete = true;
+				_session_info.stream_download = false;
+			} else {
+#endif
+				more_data = true;
+				payload->burst_complete = false;
+#ifndef MAVLINK_FTP_UNIT_TEST
+				max_bytes_to_send -= get_size();
+			}
+#endif
+		}
+		
+		ftp_msg.target_system = _session_info.stream_target_system_id;
+		_reply(&ftp_msg);
+	} while (more_data);
+}
+

src/modules/mavlink/mavlink_ftp.h

@@ -39,45 +39,44 @@
 #include <dirent.h>
 #include <queue.h>
 
-#include <px4_workqueue.h>
 #include <systemlib/err.h>
 
-#include "mavlink_messages.h"
-#include "mavlink_main.h"
+#include "mavlink_stream.h"
+#include "mavlink_bridge_header.h"
 
 class MavlinkFtpTest;
 
-/// @brief MAVLink remote file server. Support FTP like commands using MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL message.
-/// A limited number of requests (kRequestQueueSize) may be outstanding at a time. Additional messages will be discarded.
-class MavlinkFTP
+/// MAVLink remote file server. Support FTP like commands using MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL message.
+class MavlinkFTP : public MavlinkStream
 {
 public:
-	/// @brief Returns the one Mavlink FTP server in the system.
-	static MavlinkFTP* get_server(void);
-    
 	/// @brief Contructor is only public so unit test code can new objects.
-	MavlinkFTP();
+	MavlinkFTP(Mavlink *mavlink);
+	~MavlinkFTP();
+
+	static MavlinkStream *new_instance(Mavlink *mavlink);
 	
-	/// @brief Adds the specified message to the work queue.
-	void handle_message(Mavlink* mavlink, mavlink_message_t *msg);
-    
-	typedef void (*ReceiveMessageFunc_t)(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest);
+	/// Handle possible FTP message
+	void handle_message(const mavlink_message_t *msg);
+
+	typedef void (*ReceiveMessageFunc_t)(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data);
 	
 	/// @brief Sets up the server to run in unit test mode.
 	///	@param rcvmsgFunc Function which will be called to handle outgoing mavlink messages.
-	///	@param ftp_test MavlinkFtpTest object which the function is associated with
-	void set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, MavlinkFtpTest *ftp_test);
+	///	@param worker_data Data to pass to worker
+	void set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, void *worker_data);
 
 	/// @brief This is the payload which is in mavlink_file_transfer_protocol_t.payload. We pad the structure ourselves to
 	/// 32 bit alignment to avoid usage of any pack pragmas.
 	struct PayloadHeader
         {
-		uint16_t	seqNumber;	///< sequence number for message
+		uint16_t	seq_number;	///< sequence number for message
 		uint8_t		session;	///< Session id for read and write commands
 		uint8_t		opcode;		///< Command opcode
 		uint8_t		size;		///< Size of data
 		uint8_t		req_opcode;	///< Request opcode returned in kRspAck, kRspNak message
-		uint8_t		padding[2];	///< 32 bit aligment padding
+		uint8_t		burst_complete; ///< Only used if req_opcode=kCmdBurstReadFile - 1: set of burst packets complete, 0: More burst packets coming.
+		uint8_t		padding;        ///< 32 bit aligment padding
 		uint32_t	offset;		///< Offsets for List and Read commands
 		uint8_t		data[];		///< command data, varies by Opcode
         };
@@ -100,6 +99,7 @@ public:
 		kCmdTruncateFile,	///< Truncate file at <path> to <offset> length
 		kCmdRename,		///< Rename <path1> to <path2>
 		kCmdCalcFileCRC32,	///< Calculate CRC32 for file at <path>
+		kCmdBurstReadFile,	///< Burst download session file
 		
 		kRspAck = 128,		///< Ack response
 		kRspNak			///< Nak response
@@ -118,35 +118,22 @@ public:
 		kErrUnknownCommand		///< Unknown command opcode
         };
 	
+	// MavlinkStream overrides
+	virtual const char *get_name(void) const;
+	virtual uint8_t get_id(void);
+	virtual unsigned get_size(void);
+	
 private:
-	/// @brief Unit of work which is queued to work_queue
-	struct Request
-	{
-		work_s	work;			///< work queue entry
-		Mavlink	*mavlink;		///< Mavlink to reply to
-		uint8_t serverSystemId;		///< System ID to send from
-		uint8_t serverComponentId;	///< Component ID to send from
-		uint8_t serverChannel;		///< Channel to send to
-		uint8_t targetSystemId;		///< System ID to target reply to
-
-		mavlink_file_transfer_protocol_t message;	///< Protocol message
-	};
-	
-	Request		*_get_request(void);
-	void		_return_request(Request *req);
-	void		_lock_request_queue(void);
-	void		_unlock_request_queue(void);
-	
 	char		*_data_as_cstring(PayloadHeader* payload);
 	
-	static void	_worker_trampoline(void *arg);
-	void		_process_request(Request *req);
-	void		_reply(Request *req);
+	void		_process_request(mavlink_file_transfer_protocol_t* ftp_req, uint8_t target_system_id);
+	void		_reply(mavlink_file_transfer_protocol_t* ftp_req);
 	int		_copy_file(const char *src_path, const char *dst_path, size_t length);
 
 	ErrorCode	_workList(PayloadHeader *payload);
 	ErrorCode	_workOpen(PayloadHeader *payload, int oflag);
 	ErrorCode	_workRead(PayloadHeader *payload);
+	ErrorCode	_workBurst(PayloadHeader* payload, uint8_t target_system_id);
 	ErrorCode	_workWrite(PayloadHeader *payload);
 	ErrorCode	_workTerminate(PayloadHeader *payload);
 	ErrorCode	_workReset(PayloadHeader* payload);
@@ -156,14 +143,13 @@ private:
 	ErrorCode	_workTruncateFile(PayloadHeader *payload);
 	ErrorCode	_workRename(PayloadHeader *payload);
 	ErrorCode	_workCalcFileCRC32(PayloadHeader *payload);
-
-	static const unsigned	kRequestQueueSize = 2;			///< Max number of queued requests
-	Request			_request_bufs[kRequestQueueSize];	///< Request buffers which hold work
-	dq_queue_t		_request_queue;				///< Queue of available Request buffers
-	sem_t			_request_queue_sem;			///< Semaphore for locking access to _request_queue
 	
-	int _find_unused_session(void);
-	bool _valid_session(unsigned index);
+	uint8_t _getServerSystemId(void);
+	uint8_t _getServerComponentId(void);
+	uint8_t _getServerChannel(void);
+
+	// Overrides from MavlinkStream
+	virtual void send(const hrt_abstime t);
 	
 	static const char	kDirentFile = 'F';	///< Identifies File returned from List command
 	static const char	kDirentDir = 'D';	///< Identifies Directory returned from List command
@@ -172,9 +158,24 @@ private:
 	/// @brief Maximum data size in RequestHeader::data
 	static const uint8_t	kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(PayloadHeader);
 	
-	static const unsigned kMaxSession = 2;	///< Max number of active sessions
-	int	_session_fds[kMaxSession];	///< Session file descriptors, 0 for empty slot
+	struct SessionInfo {
+		int		fd;
+		uint32_t	file_size;
+		bool		stream_download;
+		uint32_t	stream_offset;
+		uint16_t	stream_seq_number;
+		uint8_t		stream_target_system_id;
+	};
+	struct SessionInfo _session_info;	///< Session info, fd=-1 for no active session
 	
-	ReceiveMessageFunc_t _utRcvMsgFunc;	///< Unit test override for mavlink message sending
-	MavlinkFtpTest *_ftp_test;		///< Additional parameter to _utRcvMsgFunc;
+	ReceiveMessageFunc_t	_utRcvMsgFunc;	///< Unit test override for mavlink message sending
+	void			*_worker_data;	///< Additional parameter to _utRcvMsgFunc;
+	
+	/* do not allow copying this class */
+	MavlinkFTP(const MavlinkFTP&);
+	MavlinkFTP operator=(const MavlinkFTP&);
+	
+	
+	// Mavlink test needs to be able to call send
+	friend class MavlinkFtpTest;
 };

src/modules/mavlink/mavlink_main.cpp

@@ -139,6 +139,7 @@ Mavlink::Mavlink() :
 	_streams(nullptr),
 	_mission_manager(nullptr),
 	_parameters_manager(nullptr),
+	_mavlink_ftp(nullptr),
 	_mode(MAVLINK_MODE_NORMAL),
 	_channel(MAVLINK_COMM_0),
 	_logbuffer {},
@@ -905,6 +906,9 @@ Mavlink::handle_message(const mavlink_message_t *msg)
 
 	/* handle packet with parameter component */
 	_parameters_manager->handle_message(msg);
+	
+	/* handle packet with ftp component */
+	_mavlink_ftp->handle_message(msg);
 
 	if (get_forwarding_on()) {
 		/* forward any messages to other mavlink instances */
@@ -1412,6 +1416,11 @@ Mavlink::task_main(int argc, char *argv[])
 	_parameters_manager->set_interval(interval_from_rate(120.0f));
 	LL_APPEND(_streams, _parameters_manager);
 
+	/* MAVLINK_FTP stream */
+	_mavlink_ftp = (MavlinkFTP *) MavlinkFTP::new_instance(this);
+	_mavlink_ftp->set_interval(interval_from_rate(120.0f));
+	LL_APPEND(_streams, _mavlink_ftp);
+	
 	/* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on
 	 * remote requests rate. Rate specified here controls how much bandwidth we will reserve for
 	 * mission messages. */

src/modules/mavlink/mavlink_main.h

@@ -63,6 +63,7 @@
 #include "mavlink_messages.h"
 #include "mavlink_mission.h"
 #include "mavlink_parameters.h"
+#include "mavlink_ftp.h"
 
 #ifdef __PX4_NUTTX
 class Mavlink
@@ -308,8 +309,9 @@ private:
 	MavlinkOrbSubscription	*_subscriptions;
 	MavlinkStream		*_streams;
 
-	MavlinkMissionManager	*_mission_manager;
-	MavlinkParametersManager *_parameters_manager;
+	MavlinkMissionManager		*_mission_manager;
+	MavlinkParametersManager	*_parameters_manager;
+	MavlinkFTP			*_mavlink_ftp;
 
 	MAVLINK_MODE 		_mode;
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -136,8 +136,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_time_offset(0)
 {
 
-	// make sure the FTP server is started
-	(void)MavlinkFTP::get_server();
 }
 
 MavlinkReceiver::~MavlinkReceiver()
@@ -204,10 +202,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_request_data_stream(msg);
 		break;
 
-	case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
-		MavlinkFTP::get_server()->handle_message(_mavlink, msg);
-		break;
-
 	case MAVLINK_MSG_ID_SYSTEM_TIME:
 		handle_message_system_time(msg);
 		break;

src/modules/mavlink/mavlink_stream.h

@@ -73,8 +73,6 @@ public:
 	 * @return 0 if updated / sent, -1 if unchanged
 	 */
 	int update(const hrt_abstime t);
-	static MavlinkStream *new_instance(const Mavlink *mavlink);
-	static const char *get_name_static();
 	virtual const char *get_name() const = 0;
 	virtual uint8_t get_id() = 0;
 

src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp

@@ -43,19 +43,19 @@
 #include "../mavlink_ftp.h"
 
 /// @brief Test case file name for Read command. File are generated using mavlink_ftp_test_data.py
-const MavlinkFtpTest::ReadTestCase MavlinkFtpTest::_rgReadTestCases[] = {
-	{ "/etc/unit_test_data/mavlink_tests/test_238.data",	MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) - 1},	// Read takes less than single packet
-	{ "/etc/unit_test_data/mavlink_tests/test_239.data",	MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) },		// Read completely fills single packet
-	{ "/etc/unit_test_data/mavlink_tests/test_240.data",	MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) + 1 },	// Read take two packets
+const MavlinkFtpTest::DownloadTestCase MavlinkFtpTest::_rgDownloadTestCases[] = {
+	{ "/etc/unit_test_data/mavlink_tests/test_238.data",	MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) - 1,	true, false },	// Read takes less than single packet
+	{ "/etc/unit_test_data/mavlink_tests/test_239.data",	MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader),	true, true },	// Read completely fills single packet
+	{ "/etc/unit_test_data/mavlink_tests/test_240.data",	MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) + 1,	false, false },	// Read take two packets
 };
 
 const char MavlinkFtpTest::_unittest_microsd_dir[] = "/fs/microsd/ftp_unit_test_dir";
 const char MavlinkFtpTest::_unittest_microsd_file[] = "/fs/microsd/ftp_unit_test_dir/file";
 
 MavlinkFtpTest::MavlinkFtpTest() :
-	_ftp_server{},
-	_reply_msg{},
-	_lastOutgoingSeqNumber{}
+	_ftp_server(nullptr),
+	_expected_seq_number(0),
+	_reply_msg{}
 {
 }
 
@@ -67,8 +67,9 @@ MavlinkFtpTest::~MavlinkFtpTest()
 /// @brief Called before every test to initialize the FTP Server.
 void MavlinkFtpTest::_init(void)
 {
-	_ftp_server = new MavlinkFTP;;
-	_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message, this);
+	_expected_seq_number = 0;
+	_ftp_server = new MavlinkFTP(NULL);
+	_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message_handler_generic, this);
 
 	_cleanup_microsd();
 }
@@ -85,15 +86,13 @@ void MavlinkFtpTest::_cleanup(void)
 bool MavlinkFtpTest::_ack_test(void)
 {
 	MavlinkFTP::PayloadHeader		payload;
-	mavlink_file_transfer_protocol_t	ftp_msg;
-	MavlinkFTP::PayloadHeader		*reply;
+	const MavlinkFTP::PayloadHeader		*reply;
 	
 	payload.opcode = MavlinkFTP::kCmdNone;
 
 	bool success = _send_receive_msg(&payload,	// FTP payload header
 					 0,		// size in bytes of data
 					 nullptr,	// Data to start into FTP message payload
-					 &ftp_msg,	// Response from server
 					 &reply);	// Payload inside FTP message response
 	if (!success) {
 		return false;
@@ -109,15 +108,13 @@ bool MavlinkFtpTest::_ack_test(void)
 bool MavlinkFtpTest::_bad_opcode_test(void)
 {
 	MavlinkFTP::PayloadHeader		payload;
-	mavlink_file_transfer_protocol_t	ftp_msg;
-	MavlinkFTP::PayloadHeader		*reply;
+	const MavlinkFTP::PayloadHeader		*reply;
 	
 	payload.opcode = 0xFF;	// bogus opcode
 
 	bool success = _send_receive_msg(&payload,	// FTP payload header
 					 0,		// size in bytes of data
 					 nullptr,	// Data to start into FTP message payload
-					 &ftp_msg,	// Response from server
 					 &reply);	// Payload inside FTP message response
 	if (!success) {
 		return false;
@@ -135,8 +132,7 @@ bool MavlinkFtpTest::_bad_datasize_test(void)
 {
 	mavlink_message_t			msg;
 	MavlinkFTP::PayloadHeader		payload;
-	mavlink_file_transfer_protocol_t	ftp_msg;
-	MavlinkFTP::PayloadHeader		*reply;
+	const MavlinkFTP::PayloadHeader		*reply;
 	
 	payload.opcode = MavlinkFTP::kCmdListDirectory;
 	
@@ -145,9 +141,9 @@ bool MavlinkFtpTest::_bad_datasize_test(void)
 	// Set the data size to be one larger than is legal
 	((MavlinkFTP::PayloadHeader*)((mavlink_file_transfer_protocol_t*)msg.payload64)->payload)->size = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN + 1;
 	
-	_ftp_server->handle_message(nullptr /* mavlink */, &msg);
+	_ftp_server->handle_message(&msg);
 	
-	if (!_decode_message(&_reply_msg, &ftp_msg, &reply)) {
+	if (!_decode_message(&_reply_msg, &reply)) {
 		return false;
 	}
 	
@@ -161,8 +157,7 @@ bool MavlinkFtpTest::_bad_datasize_test(void)
 bool MavlinkFtpTest::_list_test(void)
 {
 	MavlinkFTP::PayloadHeader		payload;
-	mavlink_file_transfer_protocol_t	ftp_msg;
-	MavlinkFTP::PayloadHeader		*reply;
+	const MavlinkFTP::PayloadHeader		*reply;
 	
 	char response1[] = "Dempty_dir|Ftest_238.data\t238|Ftest_239.data\t239|Ftest_240.data\t240";
 	char response2[] = "Ddev|Detc|Dfs|Dobj";
@@ -188,7 +183,6 @@ bool MavlinkFtpTest::_list_test(void)
 		bool success = _send_receive_msg(&payload,		// FTP payload header
 						strlen(test->dir)+1,	// size in bytes of data
 						(uint8_t*)test->dir,	// Data to start into FTP message payload
-						&ftp_msg,		// Response from server
 						&reply);		// Payload inside FTP message response
 		if (!success) {
 			return false;
@@ -202,16 +196,20 @@ bool MavlinkFtpTest::_list_test(void)
 			// to a hardcoded return result string.
 			
 			// Convert null terminators to seperator char so we can use strok to parse returned data
+			char list_entry[256];
 			for (uint8_t j=0; j<reply->size-1; j++) {
 				if (reply->data[j] == 0) {
-					reply->data[j] = '|';
+					list_entry[j] = '|';
+				} else {
+					list_entry[j] = reply->data[j];
 				}
 			}
+			list_entry[reply->size-1] = 0;
 			
 			// Loop over returned directory entries trying to find then in the response list
 			char *dir;
 			int response_count = 0;
-			dir = strtok((char *)&reply->data[0], "|");
+			dir = strtok(list_entry, "|");
 			while (dir != nullptr) {
 				ut_assert("Returned directory not found in expected response", strstr(test->response, dir));
 				response_count++;
@@ -234,8 +232,7 @@ bool MavlinkFtpTest::_list_test(void)
 bool MavlinkFtpTest::_list_eof_test(void)
 {
 	MavlinkFTP::PayloadHeader		payload;
-	mavlink_file_transfer_protocol_t	ftp_msg;
-	MavlinkFTP::PayloadHeader		*reply;
+	const MavlinkFTP::PayloadHeader		*reply;
 	const char				*dir = "/";
 	
 	payload.opcode = MavlinkFTP::kCmdListDirectory;
@@ -244,7 +241,6 @@ bool MavlinkFtpTest::_list_eof_test(void)
 	bool success = _send_receive_msg(&payload,	// FTP payload header
 					strlen(dir)+1,	// size in bytes of data
 					(uint8_t*)dir,	// Data to start into FTP message payload
-					&ftp_msg,	// Response from server
 					&reply);	// Payload inside FTP message response
 	if (!success) {
 		return false;
@@ -261,8 +257,7 @@ bool MavlinkFtpTest::_list_eof_test(void)
 bool MavlinkFtpTest::_open_badfile_test(void)
 {
 	MavlinkFTP::PayloadHeader		payload;
-	mavlink_file_transfer_protocol_t	ftp_msg;
-	MavlinkFTP::PayloadHeader		*reply;
+	const MavlinkFTP::PayloadHeader		*reply;
 	const char				*dir = "/foo";	// non-existent file
 	
 	payload.opcode = MavlinkFTP::kCmdOpenFileRO;
@@ -271,7 +266,6 @@ bool MavlinkFtpTest::_open_badfile_test(void)
 	bool success = _send_receive_msg(&payload,	// FTP payload header
 					strlen(dir)+1,	// size in bytes of data
 					(uint8_t*)dir,	// Data to start into FTP message payload
-					&ftp_msg,	// Response from server
 					&reply);	// Payload inside FTP message response
 	if (!success) {
 		return false;
@@ -288,12 +282,11 @@ bool MavlinkFtpTest::_open_badfile_test(void)
 bool MavlinkFtpTest::_open_terminate_test(void)
 {
 	MavlinkFTP::PayloadHeader		payload;
-	mavlink_file_transfer_protocol_t	ftp_msg;
-	MavlinkFTP::PayloadHeader		*reply;
+	const MavlinkFTP::PayloadHeader		*reply;
 	
-	for (size_t i=0; i<sizeof(_rgReadTestCases)/sizeof(_rgReadTestCases[0]); i++) {
+	for (size_t i=0; i<sizeof(_rgDownloadTestCases)/sizeof(_rgDownloadTestCases[0]); i++) {
 		struct stat st;
-		const ReadTestCase *test = &_rgReadTestCases[i];
+		const DownloadTestCase *test = &_rgDownloadTestCases[i];
 		
 		payload.opcode = MavlinkFTP::kCmdOpenFileRO;
 		payload.offset = 0;
@@ -301,7 +294,6 @@ bool MavlinkFtpTest::_open_terminate_test(void)
 		bool success = _send_receive_msg(&payload,		// FTP payload header
 						 strlen(test->file)+1,	// size in bytes of data
 						 (uint8_t*)test->file,	// Data to start into FTP message payload
-						 &ftp_msg,		// Response from server
 						 &reply);		// Payload inside FTP message response
 		if (!success) {
 			return false;
@@ -321,7 +313,6 @@ bool MavlinkFtpTest::_open_terminate_test(void)
 		success = _send_receive_msg(&payload,	// FTP payload header
 					    0,		// size in bytes of data
 					    nullptr,	// Data to start into FTP message payload
-					    &ftp_msg,	// Response from server
 					    &reply);	// Payload inside FTP message response
 		if (!success) {
 			return false;
@@ -338,9 +329,8 @@ bool MavlinkFtpTest::_open_terminate_test(void)
 bool MavlinkFtpTest::_terminate_badsession_test(void)
 {
 	MavlinkFTP::PayloadHeader		payload;
-	mavlink_file_transfer_protocol_t	ftp_msg;
-	MavlinkFTP::PayloadHeader		*reply;
-	const char				*file = _rgReadTestCases[0].file;
+	const MavlinkFTP::PayloadHeader		*reply;
+	const char				*file = _rgDownloadTestCases[0].file;
 	
 	payload.opcode = MavlinkFTP::kCmdOpenFileRO;
 	payload.offset = 0;
@@ -348,7 +338,6 @@ bool MavlinkFtpTest::_terminate_badsession_test(void)
 	bool success = _send_receive_msg(&payload,	// FTP payload header
 					strlen(file)+1,	// size in bytes of data
 					(uint8_t*)file,	// Data to start into FTP message payload
-					&ftp_msg,	// Response from server
 					&reply);	// Payload inside FTP message response
 	if (!success) {
 		return false;
@@ -363,7 +352,6 @@ bool MavlinkFtpTest::_terminate_badsession_test(void)
 	success = _send_receive_msg(&payload,	// FTP payload header
 				   0,		// size in bytes of data
 				   nullptr,	// Data to start into FTP message payload
-				   &ftp_msg,	// Response from server
 				   &reply);	// Payload inside FTP message response
 	if (!success) {
 		return false;
@@ -380,12 +368,11 @@ bool MavlinkFtpTest::_terminate_badsession_test(void)
 bool MavlinkFtpTest::_read_test(void)
 {
 	MavlinkFTP::PayloadHeader		payload;
-	mavlink_file_transfer_protocol_t	ftp_msg;
-	MavlinkFTP::PayloadHeader		*reply;
+	const MavlinkFTP::PayloadHeader		*reply;
 	
-	for (size_t i=0; i<sizeof(_rgReadTestCases)/sizeof(_rgReadTestCases[0]); i++) {
+	for (size_t i=0; i<sizeof(_rgDownloadTestCases)/sizeof(_rgDownloadTestCases[0]); i++) {
 		struct stat st;
-		const ReadTestCase *test = &_rgReadTestCases[i];
+		const DownloadTestCase *test = &_rgDownloadTestCases[i];
 		
 		// Read in the file so we can compare it to what we get back
 		ut_compare("stat failed", stat(test->file, &st), 0);
@@ -406,7 +393,6 @@ bool MavlinkFtpTest::_read_test(void)
 		bool success = _send_receive_msg(&payload,		// FTP payload header
 						 strlen(test->file)+1,	// size in bytes of data
 						 (uint8_t*)test->file,	// Data to start into FTP message payload
-						 &ftp_msg,		// Response from server
 						 &reply);		// Payload inside FTP message response
 		if (!success) {
 			return false;
@@ -421,7 +407,6 @@ bool MavlinkFtpTest::_read_test(void)
 		success = _send_receive_msg(&payload,	// FTP payload header
 					    0,		// size in bytes of data
 					    nullptr,	// Data to start into FTP message payload
-					    &ftp_msg,	// Response from server
 					    &reply);	// Payload inside FTP message response
 		if (!success) {
 			return false;
@@ -429,10 +414,40 @@ bool MavlinkFtpTest::_read_test(void)
 		
 		ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
 		ut_compare("Offset incorrect", reply->offset, 0);
+
+		uint32_t full_packet_bytes = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader);
+		uint32_t expected_bytes = test->singlePacketRead ? (uint32_t)st.st_size : full_packet_bytes;
+		ut_compare("Payload size incorrect", reply->size, expected_bytes);
+		ut_compare("File contents differ", memcmp(reply->data, bytes, expected_bytes), 0);
 		
-		if (test->length <= MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader)) {
-			ut_compare("Payload size incorrect", reply->size, (uint32_t)st.st_size);
-			ut_compare("File contents differ", memcmp(reply->data, bytes, st.st_size), 0);
+		payload.offset += expected_bytes;
+		
+		if (test->singlePacketRead) {
+			// Try going past EOF
+			success = _send_receive_msg(&payload,	// FTP payload header
+						    0,		// size in bytes of data
+						    nullptr,	// Data to start into FTP message payload
+						    &reply);	// Payload inside FTP message response
+			if (!success) {
+				return false;
+			}
+			
+			ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak);
+		} else {
+			success = _send_receive_msg(&payload,	// FTP payload header
+						    0,		// size in bytes of data
+						    nullptr,	// Data to start into FTP message payload
+						    &reply);	// Payload inside FTP message response
+			if (!success) {
+				return false;
+			}
+			
+			ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+			ut_compare("Offset incorrect", reply->offset, payload.offset);
+
+			expected_bytes = (uint32_t)st.st_size - full_packet_bytes;
+			ut_compare("Payload size incorrect", reply->size, expected_bytes);
+			ut_compare("File contents differ", memcmp(reply->data, &bytes[payload.offset], expected_bytes), 0);
 		}
 		
 		payload.opcode = MavlinkFTP::kCmdTerminateSession;
@@ -442,7 +457,91 @@ bool MavlinkFtpTest::_read_test(void)
 		success = _send_receive_msg(&payload,	// FTP payload header
 					    0,		// size in bytes of data
 					    nullptr,	// Data to start into FTP message payload
-					    &ftp_msg,	// Response from server
+					    &reply);	// Payload inside FTP message response
+		if (!success) {
+			return false;
+		}
+		
+		ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+		ut_compare("Incorrect payload size", reply->size, 0);
+	}
+	
+	return true;
+}
+
+/// @brief Tests for correct reponse to a Read command on an open session.
+bool MavlinkFtpTest::_burst_test(void)
+{
+	MavlinkFTP::PayloadHeader		payload;
+	const MavlinkFTP::PayloadHeader		*reply;
+	BurstInfo				burst_info;
+
+
+	
+	for (size_t i=0; i<sizeof(_rgDownloadTestCases)/sizeof(_rgDownloadTestCases[0]); i++) {
+		struct stat st;
+		const DownloadTestCase *test = &_rgDownloadTestCases[i];
+		
+		// Read in the file so we can compare it to what we get back
+		ut_compare("stat failed", stat(test->file, &st), 0);
+		uint8_t *bytes = new uint8_t[st.st_size];
+		ut_assert("new failed", bytes != nullptr);
+		int fd = ::open(test->file, O_RDONLY);
+		ut_assert("open failed", fd != -1);
+		int bytes_read = ::read(fd, bytes, st.st_size);
+		ut_compare("read failed", bytes_read, st.st_size);
+		::close(fd);
+		
+		// Test case data files are created for specific boundary conditions
+		ut_compare("Test case data files are out of date", test->length, st.st_size);
+		
+		payload.opcode = MavlinkFTP::kCmdOpenFileRO;
+		payload.offset = 0;
+		
+		bool success = _send_receive_msg(&payload,		// FTP payload header
+						 strlen(test->file)+1,	// size in bytes of data
+						 (uint8_t*)test->file,	// Data to start into FTP message payload
+						 &reply);		// Payload inside FTP message response
+		if (!success) {
+			return false;
+		}
+
+		ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+		
+		// Setup for burst response handler
+		burst_info.burst_state = burst_state_first_ack;
+		burst_info.single_packet_file = test->singlePacketRead;
+		burst_info.file_size = st.st_size;
+		burst_info.file_bytes = bytes;
+		burst_info.ftp_test_class = this;
+		_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message_handler_burst, &burst_info);
+
+		// Send the burst command, message response will be handled by _receive_message_handler_stream
+		payload.opcode = MavlinkFTP::kCmdBurstReadFile;
+		payload.session = reply->session;
+		payload.offset = 0;
+		
+		mavlink_message_t msg;
+		_setup_ftp_msg(&payload, 0, nullptr, &msg);
+		_ftp_server->handle_message(&msg);
+
+		// First packet is sent using stream mechanism, so we need to force it out ourselves
+		hrt_abstime t = 0;
+		_ftp_server->send(t);
+		
+		ut_compare("Incorrect sequence of messages", burst_info.burst_state, burst_state_complete);
+		
+		// Put back generic message handler
+		_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message_handler_generic, this);
+		
+		// Terminate session
+		payload.opcode = MavlinkFTP::kCmdTerminateSession;
+		payload.session = reply->session;
+		payload.size = 0;
+		
+		success = _send_receive_msg(&payload,	// FTP payload header
+					    0,		// size in bytes of data
+					    nullptr,	// Data to start into FTP message payload
 					    &reply);	// Payload inside FTP message response
 		if (!success) {
 			return false;
@@ -459,18 +558,16 @@ bool MavlinkFtpTest::_read_test(void)
 bool MavlinkFtpTest::_read_badsession_test(void)
 {
 	MavlinkFTP::PayloadHeader		payload;
-	mavlink_file_transfer_protocol_t	ftp_msg;
-	MavlinkFTP::PayloadHeader		*reply;
-	const char				*file = _rgReadTestCases[0].file;
+	const MavlinkFTP::PayloadHeader		*reply;
+	const char				*file = _rgDownloadTestCases[0].file;
 	
 	payload.opcode = MavlinkFTP::kCmdOpenFileRO;
 	payload.offset = 0;
 	
-	bool success = _send_receive_msg(&payload,	// FTP payload header
+	bool success = _send_receive_msg(&payload,		// FTP payload header
 					 strlen(file)+1,	// size in bytes of data
 					 (uint8_t*)file,	// Data to start into FTP message payload
-					 &ftp_msg,	// Response from server
-					 &reply);	// Payload inside FTP message response
+					 &reply);		// Payload inside FTP message response
 	if (!success) {
 		return false;
 	}
@@ -484,7 +581,6 @@ bool MavlinkFtpTest::_read_badsession_test(void)
 	success = _send_receive_msg(&payload,	// FTP payload header
 				    0,		// size in bytes of data
 				    nullptr,	// Data to start into FTP message payload
-				    &ftp_msg,	// Response from server
 				    &reply);	// Payload inside FTP message response
 	if (!success) {
 		return false;
@@ -500,8 +596,7 @@ bool MavlinkFtpTest::_read_badsession_test(void)
 bool MavlinkFtpTest::_removedirectory_test(void)
 {
 	MavlinkFTP::PayloadHeader		payload;
-	mavlink_file_transfer_protocol_t	ftp_msg;
-	MavlinkFTP::PayloadHeader		*reply;
+	const MavlinkFTP::PayloadHeader		*reply;
 	int					fd;
 	
 	struct _testCase {
@@ -534,7 +629,6 @@ bool MavlinkFtpTest::_removedirectory_test(void)
 		bool success = _send_receive_msg(&payload,		// FTP payload header
 						 strlen(test->dir)+1,	// size in bytes of data
 						 (uint8_t*)test->dir,	// Data to start into FTP message payload
-						 &ftp_msg,		// Response from server
 						 &reply);		// Payload inside FTP message response
 		if (!success) {
 			return false;
@@ -556,8 +650,7 @@ bool MavlinkFtpTest::_removedirectory_test(void)
 bool MavlinkFtpTest::_createdirectory_test(void)
 {
 	MavlinkFTP::PayloadHeader		payload;
-	mavlink_file_transfer_protocol_t	ftp_msg;
-	MavlinkFTP::PayloadHeader		*reply;
+	const MavlinkFTP::PayloadHeader		*reply;
 	
 	struct _testCase {
 		const char	*dir;
@@ -579,7 +672,6 @@ bool MavlinkFtpTest::_createdirectory_test(void)
 		bool success = _send_receive_msg(&payload,		// FTP payload header
 						 strlen(test->dir)+1,	// size in bytes of data
 						 (uint8_t*)test->dir,	// Data to start into FTP message payload
-						 &ftp_msg,		// Response from server
 						 &reply);		// Payload inside FTP message response
 		if (!success) {
 			return false;
@@ -601,8 +693,7 @@ bool MavlinkFtpTest::_createdirectory_test(void)
 bool MavlinkFtpTest::_removefile_test(void)
 {
 	MavlinkFTP::PayloadHeader		payload;
-	mavlink_file_transfer_protocol_t	ftp_msg;
-	MavlinkFTP::PayloadHeader		*reply;
+	const MavlinkFTP::PayloadHeader		*reply;
 	int					fd;
 	
 	struct _testCase {
@@ -611,7 +702,7 @@ bool MavlinkFtpTest::_removefile_test(void)
 	};
 	static const struct _testCase rgTestCases[] = {
 		{ "/bogus",			false },
-		{ _rgReadTestCases[0].file,	false },
+		{ _rgDownloadTestCases[0].file,	false },
 		{ _unittest_microsd_dir,	false },
 		{ _unittest_microsd_file,	true },
 		{ _unittest_microsd_file,	false },
@@ -630,7 +721,6 @@ bool MavlinkFtpTest::_removefile_test(void)
 		bool success = _send_receive_msg(&payload,		// FTP payload header
 						 strlen(test->file)+1,	// size in bytes of data
 						 (uint8_t*)test->file,	// Data to start into FTP message payload
-						 &ftp_msg,		// Response from server
 						 &reply);		// Payload inside FTP message response
 		if (!success) {
 			return false;
@@ -649,62 +739,119 @@ bool MavlinkFtpTest::_removefile_test(void)
 	return true;
 }
 
-/// @brief Static method used as callback from MavlinkFTP. This method will be called by MavlinkFTP when
+/// Static method used as callback from MavlinkFTP for generic use. This method will be called by MavlinkFTP when
 /// it needs to send a message out on Mavlink.
-void MavlinkFtpTest::receive_message(const mavlink_message_t *msg, MavlinkFtpTest *ftp_test)
+void MavlinkFtpTest::receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data)
 {
-	ftp_test->_receive_message(msg);
+	((MavlinkFtpTest*)worker_data)->_receive_message_handler_generic(ftp_req);
 }
 
-/// @brief Non-Static version of receive_message
-void MavlinkFtpTest::_receive_message(const mavlink_message_t *msg)
+void MavlinkFtpTest::_receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req)
 {
 	// Move the message into our own member variable
-	memcpy(&_reply_msg, msg, sizeof(mavlink_message_t));
+	memcpy(&_reply_msg, ftp_req, sizeof(mavlink_file_transfer_protocol_t));
+}
+
+/// Static method used as callback from MavlinkFTP for stream download testing. This method will be called by MavlinkFTP when
+/// it needs to send a message out on Mavlink.
+void MavlinkFtpTest::receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data)
+{
+	BurstInfo* burst_info = (BurstInfo*)worker_data;
+	burst_info->ftp_test_class->_receive_message_handler_burst(ftp_req, burst_info);
+}
+
+bool MavlinkFtpTest::_receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_msg, BurstInfo* burst_info)
+{
+	hrt_abstime t = 0;
+	const MavlinkFTP::PayloadHeader* reply;
+	uint32_t full_packet_bytes = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader);
+	uint32_t expected_bytes;
+	
+	_decode_message(ftp_msg, &reply);
+
+	switch (burst_info->burst_state) {
+		case burst_state_first_ack:
+			ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+			ut_compare("Offset incorrect", reply->offset, 0);
+			
+			expected_bytes = burst_info->single_packet_file ? burst_info->file_size : full_packet_bytes;
+			ut_compare("Payload size incorrect", reply->size, expected_bytes);
+			ut_compare("burst_complete incorrect", reply->burst_complete, 0);
+			ut_compare("File contents differ", memcmp(reply->data, burst_info->file_bytes, expected_bytes), 0);
+			
+			// Setup for next expected message
+			burst_info->burst_state = burst_info->single_packet_file ? burst_state_nak_eof : burst_state_last_ack;
+			
+			ut_assert("Remaining stream packets missing", _ftp_server->get_size());
+			_ftp_server->send(t);
+			break;
+			
+		case burst_state_last_ack:
+			ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
+			ut_compare("Offset incorrect", reply->offset, full_packet_bytes);
+			
+			expected_bytes = burst_info->file_size - full_packet_bytes;
+			ut_compare("Payload size incorrect", reply->size, expected_bytes);
+			ut_compare("burst_complete incorrect", reply->burst_complete, 0);
+			ut_compare("File contents differ", memcmp(reply->data, &burst_info->file_bytes[full_packet_bytes], expected_bytes), 0);
+			
+			// Setup for next expected message
+			burst_info->burst_state = burst_state_nak_eof;
+			
+			ut_assert("Remaining stream packets missing", _ftp_server->get_size());
+			_ftp_server->send(t);
+			break;
+			
+		case burst_state_nak_eof:
+			// Signal complete
+			burst_info->burst_state = burst_state_complete;
+			ut_compare("All packets should have been seent", _ftp_server->get_size(), 0);
+			break;
+		
+	}
+	
+	return true;
 }
 
 /// @brief Decode and validate the incoming message
-bool MavlinkFtpTest::_decode_message(const mavlink_message_t		*msg,		///< Mavlink message to decode
-				     mavlink_file_transfer_protocol_t	*ftp_msg,	///< Decoded FTP message
-				     MavlinkFTP::PayloadHeader		**payload)	///< Payload inside FTP message response
+bool MavlinkFtpTest::_decode_message(const mavlink_file_transfer_protocol_t	*ftp_msg,	///< Incoming FTP message
+				     const MavlinkFTP::PayloadHeader		**payload)	///< Payload inside FTP message response
 {
-	mavlink_msg_file_transfer_protocol_decode(msg, ftp_msg);
+	//warnx("_decode_message");
 	
 	// Make sure the targets are correct
 	ut_compare("Target network non-zero", ftp_msg->target_network, 0);
 	ut_compare("Target system id mismatch", ftp_msg->target_system, clientSystemId);
 	ut_compare("Target component id mismatch", ftp_msg->target_component, clientComponentId);
 	
-	*payload = reinterpret_cast<MavlinkFTP::PayloadHeader *>(ftp_msg->payload);
+	*payload = reinterpret_cast<const MavlinkFTP::PayloadHeader *>(ftp_msg->payload);
 	
 	// Make sure we have a good sequence number
-	ut_compare("Sequence number mismatch", (*payload)->seqNumber, _lastOutgoingSeqNumber + 1);
-	
-	// Bump sequence number for next outgoing message
-	_lastOutgoingSeqNumber++;
+	ut_compare("Sequence number mismatch", (*payload)->seq_number, _expected_seq_number);
+	_expected_seq_number++;
 	
 	return true;
 }
 
 /// @brief Initializes an FTP message into a mavlink message
-void MavlinkFtpTest::_setup_ftp_msg(MavlinkFTP::PayloadHeader	*payload_header,	///< FTP payload header
-				    uint8_t			size,			///< size in bytes of data
-				    const uint8_t		*data,			///< Data to start into FTP message payload
-				    mavlink_message_t		*msg)			///< Returned mavlink message
+void MavlinkFtpTest::_setup_ftp_msg(const MavlinkFTP::PayloadHeader	*payload_header,	///< FTP payload header
+				    uint8_t				size,			///< size in bytes of data
+				    const uint8_t			*data,			///< Data to start into FTP message payload
+				    mavlink_message_t			*msg)			///< Returned mavlink message
 {
 	uint8_t payload_bytes[MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN];
 	MavlinkFTP::PayloadHeader *payload = reinterpret_cast<MavlinkFTP::PayloadHeader *>(payload_bytes);
 			     
 	memcpy(payload, payload_header, sizeof(MavlinkFTP::PayloadHeader));
 	
-	payload->seqNumber = _lastOutgoingSeqNumber;
+	payload->seq_number = _expected_seq_number++;
 	payload->size = size;
 	if (size != 0) {
 		memcpy(payload->data, data, size);
 	}
     
-	payload->padding[0] = 0;
-	payload->padding[1] = 0;
+	payload->burst_complete = 0;
+	payload->padding = 0;
 	
 	msg->checksum = 0;
 	mavlink_msg_file_transfer_protocol_pack(clientSystemId,		// Sender system id
@@ -720,14 +867,13 @@ void MavlinkFtpTest::_setup_ftp_msg(MavlinkFTP::PayloadHeader	*payload_header,	/
 bool MavlinkFtpTest::_send_receive_msg(MavlinkFTP::PayloadHeader	*payload_header,	///< FTP payload header
 				      uint8_t				size,			///< size in bytes of data
 				      const uint8_t			*data,			///< Data to start into FTP message payload
-				      mavlink_file_transfer_protocol_t	*ftp_msg_reply,		///< Response from server
-				      MavlinkFTP::PayloadHeader		**payload_reply)	///< Payload inside FTP message response
+				      const MavlinkFTP::PayloadHeader	**payload_reply)	///< Payload inside FTP message response
 {
 	mavlink_message_t msg;
 
 	_setup_ftp_msg(payload_header, size, data, &msg);
-	_ftp_server->handle_message(nullptr /* mavlink */, &msg);
-	return _decode_message(&_reply_msg, ftp_msg_reply, payload_reply);
+	_ftp_server->handle_message(&msg);
+	return _decode_message(&_reply_msg, payload_reply);
 }
 	
 /// @brief Cleans up an files created on microsd during testing
@@ -743,14 +889,14 @@ bool MavlinkFtpTest::run_tests(void)
 	ut_run_test(_ack_test);
 	ut_run_test(_bad_opcode_test);
 	ut_run_test(_bad_datasize_test);
-	printf("WARNING! list test commented out, but needs proper resolution!\n");
-	//ut_run_test(_list_test);
+	ut_run_test(_list_test);
 	ut_run_test(_list_eof_test);
 	ut_run_test(_open_badfile_test);
 	ut_run_test(_open_terminate_test);
 	ut_run_test(_terminate_badsession_test);
 	ut_run_test(_read_test);
 	ut_run_test(_read_badsession_test);
+	ut_run_test(_burst_test);
 	ut_run_test(_removedirectory_test);
 	ut_run_test(_createdirectory_test);
 	ut_run_test(_removefile_test);

src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h

@@ -48,7 +48,18 @@ public:
 	
 	virtual bool run_tests(void);
 	
-	static void receive_message(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest);
+	static void receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data);
+	
+	/// Worker data for stream handler
+	struct BurstInfo {
+		MavlinkFtpTest*		ftp_test_class;
+		int			burst_state;
+		bool			single_packet_file;
+		uint32_t		file_size;
+		uint8_t*		file_bytes;
+	};
+	
+	static void receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_req, void *worker_data);
 	
 	static const uint8_t serverSystemId = 50;	///< System ID for server
 	static const uint8_t serverComponentId = 1;	///< Component ID for server
@@ -75,31 +86,45 @@ private:
 	bool _terminate_badsession_test(void);
 	bool _read_test(void);
 	bool _read_badsession_test(void);
+	bool _burst_test(void);
 	bool _removedirectory_test(void);
 	bool _createdirectory_test(void);
 	bool _removefile_test(void);
 	
-	void _receive_message(const mavlink_message_t *msg);
-	void _setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, mavlink_message_t *msg);
-	bool _decode_message(const mavlink_message_t *msg, mavlink_file_transfer_protocol_t *ftp_msg, MavlinkFTP::PayloadHeader **payload);
+	void _receive_message_handler_generic(const mavlink_file_transfer_protocol_t* ftp_req);
+	void _setup_ftp_msg(const MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, mavlink_message_t *msg);
+	bool _decode_message(const mavlink_file_transfer_protocol_t *ftp_msg, const MavlinkFTP::PayloadHeader **payload);
 	bool _send_receive_msg(MavlinkFTP::PayloadHeader	*payload_header,
                            uint8_t				size,
                            const uint8_t			*data,
-                           mavlink_file_transfer_protocol_t	*ftp_msg_reply,
-                           MavlinkFTP::PayloadHeader		**payload_reply);
+                           const MavlinkFTP::PayloadHeader	**payload_reply);
 	void _cleanup_microsd(void);
 	
-	MavlinkFTP *_ftp_server;
-    
-	mavlink_message_t _reply_msg;
-    
-	uint16_t _lastOutgoingSeqNumber;
-	
-	struct ReadTestCase {
+	/// A single download test case
+	struct DownloadTestCase {
 		const char	*file;
 		const uint16_t	length;
+		bool		singlePacketRead;
+		bool		exactlyFillPacket;
 	};
-	static const ReadTestCase _rgReadTestCases[];
+	
+	/// The set of test cases for download testing
+	static const DownloadTestCase _rgDownloadTestCases[];
+	
+	/// States for stream download handler
+	enum {
+		burst_state_first_ack,
+		burst_state_last_ack,
+		burst_state_nak_eof,
+		burst_state_complete
+	};
+	
+	bool _receive_message_handler_burst(const mavlink_file_transfer_protocol_t* ftp_req, BurstInfo* burst_info);
+	
+	MavlinkFTP*	_ftp_server;
+	uint16_t	_expected_seq_number;
+	
+	mavlink_file_transfer_protocol_t _reply_msg;
 	
 	static const char _unittest_microsd_dir[];
 	static const char _unittest_microsd_file[];

src/modules/mavlink/mavlink_tests/module.mk

@@ -38,7 +38,8 @@
 MODULE_COMMAND		= mavlink_tests
 SRCS			= mavlink_tests.cpp \
 			mavlink_ftp_test.cpp \
-	 		../mavlink_ftp.cpp \
+			../mavlink_stream.cpp \
+			../mavlink_ftp.cpp \
 			../mavlink.c
 
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink

src/platforms/ros/perf_counter.cpp

@@ -170,7 +170,5 @@ void		perf_reset_all(void)
  */
 uint64_t	perf_event_count(perf_counter_t handle)
 {
-
+	return 0;
 }
-
-