diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 2c7c0d68e2..0dd4837644 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/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 diff --git a/ROMFS/px4fmu_common/mixers/pass.aux.mix b/ROMFS/px4fmu_common/mixers/pass.aux.mix new file mode 100644 index 0000000000..8e7011f0ed --- /dev/null +++ b/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 diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py index 0d2413a75f..048836a4eb 100644 --- a/Tools/px4params/srcparser.py +++ b/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*;') diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/xmlout.py index 07cced4786..b072ab79f8 100644 --- a/Tools/px4params/xmlout.py +++ b/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") diff --git a/msg/actuator_armed.msg b/msg/actuator_armed.msg index b83adb8f24..c8098724e8 100644 --- a/msg/actuator_armed.msg +++ b/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 diff --git a/src/drivers/gimbal/gimbal.cpp b/src/drivers/gimbal/gimbal.cpp index 3e1767002b..db016b363a 100644 --- a/src/drivers/gimbal/gimbal.cpp +++ b/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; } diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index a493813388..92e6dde7ff 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/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; diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 160dc5cadb..1c43e7ebf7 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -38,7 +38,7 @@ * Authors and acknowledgements in header. */ -#include "../ecl.h" +#include #include "ecl_roll_controller.h" #include #include diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index f3379b245d..4dd409283a 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -45,7 +45,7 @@ #include #include #include -#include +#include 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(); } } diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index cddf2ab2c7..cecf22a0c3 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -47,12 +47,12 @@ #include #include #include -#include #include #include #include #include +#include #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; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e4644c03a3..a349cfca95 100644 --- a/src/modules/commander/commander.cpp +++ b/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, ¶m); @@ -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) { diff --git a/src/modules/commander/esc_calibration.cpp b/src/modules/commander/esc_calibration.cpp new file mode 100644 index 0000000000..541aca0531 --- /dev/null +++ b/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 + */ + +#include "esc_calibration.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "drivers/drv_pwm_output.h" +#include +#include +#include +#include +#include + +/* 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; + } \ No newline at end of file diff --git a/src/modules/commander/esc_calibration.h b/src/modules/commander/esc_calibration.h new file mode 100644 index 0000000000..5539955ebc --- /dev/null +++ b/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 + */ + +#ifndef ESC_CALIBRATION_H_ +#define ESC_CALIBRATION_H_ +int check_if_batt_disconnected(int mavlink_fd); +int do_esc_calibration(int mavlink_fd); + +#endif \ No newline at end of file diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index f739e4caec..c16a606711 100644 --- a/src/modules/commander/module.mk +++ b/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 diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 4d644a8627..6913fbb55c 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -42,116 +42,130 @@ #include #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; imsgid == 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(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(&req->message.payload[0]); + bool stream_send = false; + PayloadHeader *payload = reinterpret_cast(&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(&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(&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; isize = 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; idata[0]); } -/// @brief Returns a unused Request entry. NULL if none available. -MavlinkFTP::Request * -MavlinkFTP::_get_request(void) -{ - _lock_request_queue(); - Request* req = reinterpret_cast(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(&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); +} + diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index eca1710174..af8740e481 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -39,45 +39,44 @@ #include #include -#include #include -#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 to length kCmdRename, ///< Rename to kCmdCalcFileCRC32, ///< Calculate CRC32 for file at + 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; }; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 947ee08229..030aa64105 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/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. */ diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 87e8be4362..4d2a833a0d 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/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; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 13d6743063..180a923774 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/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; diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index 6d0d0b6775..c37b791b04 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/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; diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp index 7b5b9228c8..6fe0ed9c8f 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp +++ b/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; jsize-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; ifile)+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; ifile, &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; ifile, &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(ftp_msg->payload); + *payload = reinterpret_cast(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(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); diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h index 2696192cc6..14c9369b05 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h +++ b/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[]; diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk index a2793c6365..e104860937 100644 --- a/src/modules/mavlink/mavlink_tests/module.mk +++ b/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 diff --git a/src/platforms/ros/perf_counter.cpp b/src/platforms/ros/perf_counter.cpp index a718013975..8e9670737b 100755 --- a/src/platforms/ros/perf_counter.cpp +++ b/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; } - -