From 7db57310d5cdd7295da6c919dafa92d3831296db Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:32:16 -0700 Subject: New mavlink_ftp unit test data --- .../unit_test_data/mavlink_tests/empty_dir/.gitignore | 0 .../unit_test_data/mavlink_tests/test_234.data | Bin 0 -> 234 bytes .../unit_test_data/mavlink_tests/test_235.data | Bin 0 -> 235 bytes .../unit_test_data/mavlink_tests/test_236.data | Bin 0 -> 236 bytes 4 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 ROMFS/px4fmu_test/unit_test_data/mavlink_tests/empty_dir/.gitignore create mode 100644 ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_234.data create mode 100644 ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_235.data create mode 100644 ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_236.data diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/empty_dir/.gitignore b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/empty_dir/.gitignore new file mode 100644 index 000000000..e69de29bb diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_234.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_234.data new file mode 100644 index 000000000..3e075aa4f Binary files /dev/null and b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_234.data differ diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_235.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_235.data new file mode 100644 index 000000000..61372f769 Binary files /dev/null and b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_235.data differ diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_236.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_236.data new file mode 100644 index 000000000..eb7fcb11a Binary files /dev/null and b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_236.data differ -- cgit v1.2.3 From 81275e624e87d5cbb10535ffa2fcc2adbccc793f Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:32:33 -0700 Subject: Added mavlink_ftp unit test --- makefiles/config_px4fmu-v2_test.mk | 3 +++ 1 file changed, 3 insertions(+) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 932f92261..a41670a77 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -54,6 +54,9 @@ MODULES += lib/conversion # LIBRARIES += lib/mathlib/CMSIS +MODULES += modules/unit_test +MODULES += modules/mavlink/mavlink_tests + # # Transitional support - add commands from the NuttX export archive. # -- cgit v1.2.3 From 6f0160bb5db552c921773f278e93d33862811a2a Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:32:50 -0700 Subject: New mavlink_ftp unit test --- .../mavlink/mavlink_tests/mavlink_ftp_test.cpp | 788 +++++++++++++++++++++ .../mavlink/mavlink_tests/mavlink_ftp_test.h | 109 +++ .../mavlink/mavlink_tests/mavlink_tests.cpp | 52 ++ src/modules/mavlink/mavlink_tests/module.mk | 48 ++ 4 files changed, 997 insertions(+) create mode 100644 src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp create mode 100644 src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h create mode 100644 src/modules/mavlink/mavlink_tests/mavlink_tests.cpp create mode 100644 src/modules/mavlink/mavlink_tests/module.mk diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp new file mode 100644 index 000000000..ebfce6d74 --- /dev/null +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp @@ -0,0 +1,788 @@ +/**************************************************************************** + * + * Copyright (C) 2014 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 mavlink_ftp_test.cpp +/// @author Don Gagne + +#include +#include +#include +#include + +#include "mavlink_ftp_test.h" +#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_234.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_235.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) }, // Read completely fills single packet + { "/etc/unit_test_data/mavlink_tests/test_236.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) + 1 }, // 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{} +{ +} + +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); + + _cleanup_microsd(); +} + +/// @brief Called after every test to take down the FTP Server. +void MavlinkFtpTest::cleanup(void) +{ + delete _ftp_server; + + _cleanup_microsd(); +} + +/// @brief Tests for correct behavior of an Ack response. +bool MavlinkFtpTest::_ack_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + 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; + } + + 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 response to a message sent with an invalid CRC. +bool MavlinkFtpTest::_bad_crc_test(void) +{ + mavlink_message_t msg; + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + payload.opcode = MavlinkFTP::kCmdNone; + + _setup_ftp_msg(&payload, 0, nullptr, &msg); + + ((MavlinkFTP::PayloadHeader*)((mavlink_file_transfer_protocol_t*)msg.payload64)->payload)->crc32++; + + _ftp_server->handle_message(nullptr /* mavlink */, &msg); + + if (!_decode_message(&_reply_msg, &ftp_msg, &reply)) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrCrc); + + return true; +} + +/// @brief Tests for correct response to an invalid opcpde. +bool MavlinkFtpTest::_bad_opcode_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + 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; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrUnknownCommand); + + return true; +} + +/// @brief Tests for correct reponse to a payload which an invalid data size field. +bool MavlinkFtpTest::_bad_datasize_test(void) +{ + mavlink_message_t msg; + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + payload.opcode = MavlinkFTP::kCmdListDirectory; + + _setup_ftp_msg(&payload, 0, nullptr, &msg); + + // 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); + + if (!_decode_message(&_reply_msg, &ftp_msg, &reply)) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidDataSize); + + return true; +} + +bool MavlinkFtpTest::_list_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + char response1[] = "D.|Dempty_dir|Ftest_234.data\t234|Ftest_235.data\t235|Ftest_236.data\t236"; + char response2[] = "Ddev|Detc|Dfs|Dobj"; + + struct _testCase { + const char *dir; + char *response; + bool success; + }; + struct _testCase rgTestCases[] = { + { "/bogus", nullptr, false }, + { "/etc/unit_test_data/mavlink_tests", response1, true }, + { "/", response2, true }, + }; + + for (size_t i=0; iresponse) + 1; + + char *ptr = strtok (test->response, "|"); + while (ptr != nullptr) + { + ptr = strtok (nullptr, "|"); + } + + payload.opcode = MavlinkFTP::kCmdListDirectory; + payload.offset = 0; + + 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; + } + + if (test->success) { + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, expected_data_size); + ut_compare("Ack payload contents incorrect", memcmp(reply->data, test->response, expected_data_size), 0); + } else { + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 2); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); + } + } + + return true; +} + +/// @brief Tests for correct reponse to a List command on a valid directory, but with an offset that +/// is beyond the last directory entry. +bool MavlinkFtpTest::_list_eof_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + const char *dir = "/"; + + payload.opcode = MavlinkFTP::kCmdListDirectory; + payload.offset = 4; // offset past top level dirs + + 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; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrEOF); + + return true; +} + +/// @brief Tests for correct reponse to an Open command on a file which does not exist. +bool MavlinkFtpTest::_open_badfile_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + const char *dir = "/foo"; // non-existent file + + payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.offset = 0; + + 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; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 2); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); + + return true; +} + +/// @brief Tests for correct reponse to an Open command on a file, followed by Terminate +bool MavlinkFtpTest::_open_terminate_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + 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; + } + + ut_compare("stat failed", stat(test->file, &st), 0); + + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, sizeof(uint32_t)); + ut_compare("File size incorrect", *((uint32_t*)&reply->data[0]), (uint32_t)st.st_size); + + 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 + &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 Terminate command on an invalid session. +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; + + payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.offset = 0; + + 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; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + + payload.opcode = MavlinkFTP::kCmdTerminateSession; + payload.session = reply->session + 1; + 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 + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession); + + return true; +} + +/// @brief Tests for correct reponse to a Read command on an open session. +bool MavlinkFtpTest::_read_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + 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::kCmdOpenFile; + 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 + &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); + + payload.opcode = MavlinkFTP::kCmdReadFile; + payload.session = reply->session; + payload.offset = 0; + + 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("Offset incorrect", reply->offset, 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.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 + &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 invalid session. +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; + + payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.offset = 0; + + 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; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + + payload.opcode = MavlinkFTP::kCmdReadFile; + payload.session = reply->session + 1; // Invalid session + payload.offset = 0; + + 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 Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession); + + return true; +} + +bool MavlinkFtpTest::_removedirectory_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + int fd; + + struct _testCase { + const char *dir; + bool success; + bool deleteFile; + }; + static const struct _testCase rgTestCases[] = { + { "/bogus", false, false }, + { "/etc/unit_test_data/mavlink_tests/empty_dir", false, false }, + { _unittest_microsd_dir, false, false }, + { _unittest_microsd_file, false, false }, + { _unittest_microsd_dir, true, true }, + }; + + ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0); + ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL)) != -1); + ::close(fd); + + for (size_t i=0; ideleteFile) { + ut_compare("unlink failed", ::unlink(_unittest_microsd_file), 0); + } + + payload.opcode = MavlinkFTP::kCmdRemoveDirectory; + payload.offset = 0; + + 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; + } + + if (test->success) { + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + } else { + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 2); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); + } + } + + return true; +} + +bool MavlinkFtpTest::_createdirectory_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + struct _testCase { + const char *dir; + bool success; + }; + static const struct _testCase rgTestCases[] = { + { "/etc/bogus", false }, + { _unittest_microsd_dir, true }, + { _unittest_microsd_dir, false }, + { "/fs/microsd/bogus/bogus", false }, + }; + + for (size_t i=0; idir)+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; + } + + if (test->success) { + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + } else { + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 2); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); + } + } + + return true; +} + +bool MavlinkFtpTest::_removefile_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + int fd; + + struct _testCase { + const char *file; + bool success; + }; + static const struct _testCase rgTestCases[] = { + { "/bogus", false }, + { _rgReadTestCases[0].file, false }, + { _unittest_microsd_dir, false }, + { _unittest_microsd_file, true }, + { _unittest_microsd_file, false }, + }; + + ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0); + ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL)) != -1); + ::close(fd); + + 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; + } + + if (test->success) { + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + } else { + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 2); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); + } + } + + return true; +} + +/// @brief Static method used as callback from MavlinkFTP. 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) +{ + ftp_test->_receive_message(msg); +} + +/// @brief Non-Static version of receive_message +void MavlinkFtpTest::_receive_message(const mavlink_message_t *msg) +{ + // Move the message into our own member variable + memcpy(&_reply_msg, msg, sizeof(mavlink_message_t)); +} + +/// @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 +{ + mavlink_msg_file_transfer_protocol_decode(msg, ftp_msg); + + // 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); + + // Make sure we have a good CRC + ut_compare("Incoming CRC mismatch", (*payload)->crc32, _payload_crc32((*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++; + + return true; +} + +/// @brief Returns the 32 bit CRC for the payload, crc32 and padding are set to 0 for calculation. +uint32_t MavlinkFtpTest::_payload_crc32(struct MavlinkFTP::PayloadHeader *payload) +{ + // We calculate CRC with crc and padding set to 0. + uint32_t saveCRC = payload->crc32; + payload->crc32 = 0; + payload->padding[0] = 0; + payload->padding[1] = 0; + payload->padding[2] = 0; + uint32_t retCRC = crc32((const uint8_t*)payload, payload->size + sizeof(MavlinkFTP::PayloadHeader)); + payload->crc32 = saveCRC; + + return retCRC; +} + +/// @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 +{ + 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->size = size; + if (size != 0) { + memcpy(payload->data, data, size); + } + + payload->crc32 = _payload_crc32(payload); + + msg->checksum = 0; + mavlink_msg_file_transfer_protocol_pack(clientSystemId, // Sender system id + clientComponentId, // Sender component id + msg, // Message to pack payload into + 0, // Target network + serverSystemId, // Target system id + serverComponentId, // Target component id + payload_bytes); // Payload to pack into message +} + +/// @brief Sends the specified FTP message to the server and returns response +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 +{ + 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); +} + +/// @brief Cleans up an files created on microsd during testing +void MavlinkFtpTest::_cleanup_microsd(void) +{ + ::unlink(_unittest_microsd_file); + ::rmdir(_unittest_microsd_dir); +} + +/// @brief Runs all the unit tests +void MavlinkFtpTest::runTests(void) +{ + ut_run_test(_ack_test); + ut_run_test(_bad_crc_test); + ut_run_test(_bad_opcode_test); + ut_run_test(_bad_datasize_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(_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 new file mode 100644 index 000000000..bbb095a08 --- /dev/null +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h @@ -0,0 +1,109 @@ +/**************************************************************************** + * + * Copyright (C) 2014 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 mavlink_ftp_test.h +/// @author Don Gagne + +#pragma once + +#include +#include "../mavlink_bridge_header.h" +#include "../mavlink_ftp.h" + +class MavlinkFtpTest : public UnitTest +{ +public: + MavlinkFtpTest(); + virtual ~MavlinkFtpTest(); + + virtual void init(void); + virtual void cleanup(void); + + virtual void runTests(void); + + static void receive_message(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest); + + static const uint8_t serverSystemId = 50; ///< System ID for server + static const uint8_t serverComponentId = 1; ///< Component ID for server + static const uint8_t serverChannel = 0; ///< Channel to send to + + static const uint8_t clientSystemId = 1; ///< System ID for client + static const uint8_t clientComponentId = 0; ///< Component ID for client + + // We don't want any of these + MavlinkFtpTest(const MavlinkFtpTest&); + MavlinkFtpTest& operator=(const MavlinkFtpTest&); + +private: + bool _ack_test(void); + bool _bad_crc_test(void); + bool _bad_opcode_test(void); + bool _bad_datasize_test(void); + bool _list_test(void); + bool _list_eof_test(void); + bool _open_badfile_test(void); + bool _open_terminate_test(void); + bool _terminate_badsession_test(void); + bool _read_test(void); + bool _read_badsession_test(void); + bool _removedirectory_test(void); + bool _createdirectory_test(void); + bool _removefile_test(void); + + void _receive_message(const mavlink_message_t *msg); + uint32_t _payload_crc32(struct MavlinkFTP::PayloadHeader *payload); + 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); + 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); + void _cleanup_microsd(void); + + MavlinkFTP *_ftp_server; + + mavlink_message_t _reply_msg; + + uint16_t _lastOutgoingSeqNumber; + + struct ReadTestCase { + const char *file; + const uint16_t length; + }; + static const ReadTestCase _rgReadTestCases[]; + + static const char _unittest_microsd_dir[]; + static const char _unittest_microsd_file[]; +}; + diff --git a/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp b/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp new file mode 100644 index 000000000..d9c1e413a --- /dev/null +++ b/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (C) 2014 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 mavlink_ftp_tests.cpp + */ + +#include + +#include "mavlink_ftp_test.h" + +extern "C" __EXPORT int mavlink_tests_main(int argc, char *argv[]); + +int mavlink_tests_main(int argc, char *argv[]) +{ + MavlinkFtpTest* test = new MavlinkFtpTest; + + test->runTests(); + test->printResults(); + + return 0; +} diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk new file mode 100644 index 000000000..07cfce1dc --- /dev/null +++ b/src/modules/mavlink/mavlink_tests/module.mk @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2014 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. +# +############################################################################ + +# +# System state machine tests. +# + +MODULE_COMMAND = mavlink_tests +SRCS = mavlink_tests.cpp \ + mavlink_ftp_test.cpp \ + ../mavlink_ftp.cpp \ + ../mavlink.c + +INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink + +MODULE_STACKSIZE = 5000 + +EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST -- cgit v1.2.3 From 1969c9ccf6797c4d86f32b9f57332a9e59071251 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:33:43 -0700 Subject: Improved unit test framework - Added init/cleanup calls before after test - Added ut_compare macro with better failure reporting --- src/modules/unit_test/unit_test.cpp | 5 +++++ src/modules/unit_test/unit_test.h | 18 ++++++++++++++++++ 2 files changed, 23 insertions(+) diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp index 02d1af481..be3b9461a 100644 --- a/src/modules/unit_test/unit_test.cpp +++ b/src/modules/unit_test/unit_test.cpp @@ -56,3 +56,8 @@ void UnitTest::printAssert(const char* msg, const char* test, const char* file, { warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line); } + +void UnitTest::printCompare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line) +{ + warnx("Compare failed: %s - (%s:%d) (%s:%d) (%s:%d)", msg, v1_text, v1, v2_text, v2, file, line); +} diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h index 32eb8c308..1a5489709 100644 --- a/src/modules/unit_test/unit_test.h +++ b/src/modules/unit_test/unit_test.h @@ -52,11 +52,15 @@ INLINE_GLOBAL(const char*, mu_last_test) UnitTest(); virtual ~UnitTest(); + + virtual void init(void) { }; + virtual void cleanup(void) { }; virtual void runTests(void) = 0; void printResults(void); void printAssert(const char* msg, const char* test, const char* file, int line); + void printCompare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line); #define ut_assert(message, test) \ do { \ @@ -68,10 +72,23 @@ INLINE_GLOBAL(const char*, mu_last_test) } \ } while (0) +#define ut_compare(message, v1, v2) \ + do { \ + int _v1 = v1; \ + int _v2 = v2; \ + if (_v1 != _v2) { \ + printCompare(message, #v1, _v1, #v2, _v2, __FILE__, __LINE__); \ + return false; \ + } else { \ + mu_assertion()++; \ + } \ + } while (0) + #define ut_run_test(test) \ do { \ warnx("RUNNING TEST: %s", #test); \ mu_tests_run()++; \ + init(); \ if (!test()) { \ warnx("TEST FAILED: %s", #test); \ mu_tests_failed()++; \ @@ -79,6 +96,7 @@ INLINE_GLOBAL(const char*, mu_last_test) warnx("TEST PASSED: %s", #test); \ mu_tests_passed()++; \ } \ + cleanup(); \ } while (0) }; -- cgit v1.2.3 From e7165d6cbf364cb95e45dad998daa7c494de409e Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:34:02 -0700 Subject: Generator for mavlink_ftp test files --- src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py | 7 +++++++ 1 file changed, 7 insertions(+) create mode 100644 src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py new file mode 100644 index 000000000..a7e68f2a3 --- /dev/null +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py @@ -0,0 +1,7 @@ +import sys +print 'Arguments: file - ' + sys.argv[1] + ', length - ' + sys.argv[2] +file = open(sys.argv[1], 'w') +for i in range(int(sys.argv[2])): + b = bytearray([i & 0xFF]) + file.write(b) +file.close() \ No newline at end of file -- cgit v1.2.3 From 37ea85968da111f7dc63cdfa8ca02ad6e601a8c3 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:34:20 -0700 Subject: Tweak for mavlink_ftp api change --- src/modules/mavlink/mavlink_receiver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bed1bd789..823de5172 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -121,7 +121,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : { // make sure the FTP server is started - (void)MavlinkFTP::getServer(); + (void)MavlinkFTP::get_server(); } MavlinkReceiver::~MavlinkReceiver() @@ -173,7 +173,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) break; case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL: - MavlinkFTP::getServer()->handle_message(_mavlink, msg); + MavlinkFTP::get_server()->handle_message(_mavlink, msg); break; default: -- cgit v1.2.3 From 828fb5efd10355d0d33b05fcf597bc4c05cd4db4 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:36:32 -0700 Subject: Additional FTP command support plus unit test - Restructured to a flatter class implementation - Added support for create/remove directory, remove file - Refactored error codes and command opcodes - Added unit test support - Fixed bugs found by unit test --- src/modules/mavlink/mavlink_ftp.cpp | 445 +++++++++++++++++++++++++----------- src/modules/mavlink/mavlink_ftp.h | 293 ++++++++++-------------- 2 files changed, 427 insertions(+), 311 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 5b65dc369..72ede8797 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -31,38 +31,39 @@ * ****************************************************************************/ +/// @file mavlink_ftp.cpp +/// @author px4dev, Don Gagne + #include #include #include #include #include +#include #include "mavlink_ftp.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::_server; - MavlinkFTP * -MavlinkFTP::getServer() +MavlinkFTP::get_server(void) { - // XXX this really cries out for some locking... - if (_server == nullptr) { - _server = new MavlinkFTP; - } - return _server; + static MavlinkFTP server; + return &server; } MavlinkFTP::MavlinkFTP() : - _session_fds{}, - _workBufs{}, - _workFree{}, - _lock{} + _request_bufs{}, + _request_queue{}, + _request_queue_sem{}, + _utRcvMsgFunc{}, + _ftp_test{} { // initialise the request freelist - dq_init(&_workFree); - sem_init(&_lock, 0, 1); + dq_init(&_request_queue); + sem_init(&_request_queue_sem, 0, 1); // initialize session list for (size_t i=0; idecode(mavlink, msg)) { + if (req == nullptr) { + warnx("Dropping FTP request: queue full\n"); + return; + } - // and queue it for the worker - work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0); - } else { - _qFree(req); + 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"); + 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::_workerTrampoline(void *arg) +MavlinkFTP::_worker_trampoline(void *arg) { - auto req = reinterpret_cast(arg); - auto server = MavlinkFTP::getServer(); + Request* req = reinterpret_cast(arg); + MavlinkFTP* server = MavlinkFTP::get_server(); // call the server worker with the work item - server->_worker(req); + server->_process_request(req); } +/// @brief Processes an FTP message void -MavlinkFTP::_worker(Request *req) +MavlinkFTP::_process_request(Request *req) { - auto hdr = req->header(); + PayloadHeader *payload = reinterpret_cast(&req->message.payload[0]); + ErrorCode errorCode = kErrNone; - uint32_t messageCRC; // basic sanity checks; must validate length before use - if (hdr->size > kMaxDataLength) { - errorCode = kErrNoRequest; + if (payload->size > kMaxDataLength) { + errorCode = kErrInvalidDataSize; goto out; } // check request CRC to make sure this is one of ours - messageCRC = hdr->crc32; - hdr->crc32 = 0; - hdr->padding[0] = 0; - hdr->padding[1] = 0; - hdr->padding[2] = 0; - if (crc32(req->rawData(), req->dataSize()) != messageCRC) { - errorCode = kErrNoRequest; + if (_payload_crc32(payload) != payload->crc32) { + errorCode = kErrCrc; goto out; warnx("ftp: bad crc"); } #ifdef MAVLINK_FTP_DEBUG - printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset); + printf("ftp: channel %u opc %u size %u offset %u\n", req->serverChannel, payload->opcode, payload->size, payload->offset); #endif - switch (hdr->opcode) { + switch (payload->opcode) { case kCmdNone: break; - case kCmdTerminate: - errorCode = _workTerminate(req); + case kCmdTerminateSession: + errorCode = _workTerminate(payload); break; - case kCmdReset: - errorCode = _workReset(); + case kCmdResetSessions: + errorCode = _workReset(payload); break; - case kCmdList: - errorCode = _workList(req); + case kCmdListDirectory: + errorCode = _workList(payload); break; - case kCmdOpen: - errorCode = _workOpen(req, false); + case kCmdOpenFile: + errorCode = _workOpen(payload, false); break; - case kCmdCreate: - errorCode = _workOpen(req, true); + case kCmdCreateFile: + errorCode = _workOpen(payload, true); break; - case kCmdRead: - errorCode = _workRead(req); + case kCmdReadFile: + errorCode = _workRead(payload); break; - case kCmdWrite: - errorCode = _workWrite(req); + case kCmdWriteFile: + errorCode = _workWrite(payload); break; - case kCmdRemove: - errorCode = _workRemove(req); + case kCmdRemoveFile: + errorCode = _workRemoveFile(payload); break; + case kCmdCreateDirectory: + errorCode = _workCreateDirectory(payload); + break; + + case kCmdRemoveDirectory: + errorCode = _workRemoveDirectory(payload); + break; + default: - errorCode = kErrNoRequest; + errorCode = kErrUnknownCommand; break; } out: // handle success vs. error if (errorCode == kErrNone) { - hdr->opcode = kRspAck; + payload->opcode = kRspAck; #ifdef MAVLINK_FTP_DEBUG warnx("FTP: ack\n"); #endif } else { warnx("FTP: nak %u", errorCode); - hdr->opcode = kRspNak; - hdr->size = 1; - hdr->data[0] = errorCode; + payload->opcode = kRspNak; + payload->size = 1; + payload->data[0] = errorCode; + if (errorCode == kErrFailErrno) { + payload->size = 2; + payload->data[1] = errno; + } } + // respond to the request _reply(req); - // free the request buffer back to the freelist - _qFree(req); + _return_request(req); } +/// @brief Sends the specified FTP reponse message out through mavlink void MavlinkFTP::_reply(Request *req) { - auto hdr = req->header(); + PayloadHeader *payload = reinterpret_cast(&req->message.payload[0]); - hdr->seqNumber = req->header()->seqNumber + 1; + payload->seqNumber = payload->seqNumber + 1; - // message is assumed to be already constructed in the request buffer, so generate the CRC - hdr->crc32 = 0; - hdr->padding[0] = 0; - hdr->padding[1] = 0; - hdr->padding[2] = 0; - hdr->crc32 = crc32(req->rawData(), req->dataSize()); + payload->crc32 = _payload_crc32(payload); - // then pack and send the reply back to the request source - req->reply(); + mavlink_message_t msg; + msg.checksum = 0; +#ifndef MAVLINK_FTP_UNIT_TEST + uint16_t len = +#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; +#ifdef MAVLINK_FTP_UNIT_TEST + // Unit test hook is set, call that instead + _utRcvMsgFunc(&msg, _ftp_test); +#else + Mavlink *mavlink = req->mavlink; + + mavlink->lockMessageBufferMutex(); + success = mavlink->message_buffer_write(&msg, len); + mavlink->unlockMessageBufferMutex(); + +#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 MavlinkFTP::ErrorCode -MavlinkFTP::_workList(Request *req) +MavlinkFTP::_workList(PayloadHeader* payload) { - auto hdr = req->header(); - char dirPath[kMaxDataLength]; - strncpy(dirPath, req->dataAsCString(), kMaxDataLength); + strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength); DIR *dp = opendir(dirPath); if (dp == nullptr) { warnx("FTP: can't open path '%s'", dirPath); - return kErrNotDir; + return kErrFailErrno; } #ifdef MAVLINK_FTP_DEBUG - warnx("FTP: list %s offset %d", dirPath, hdr->offset); + warnx("FTP: list %s offset %d", dirPath, payload->offset); #endif ErrorCode errorCode = kErrNone; @@ -239,19 +320,19 @@ MavlinkFTP::_workList(Request *req) unsigned offset = 0; // move to the requested offset - seekdir(dp, hdr->offset); + seekdir(dp, payload->offset); for (;;) { // read the directory entry if (readdir_r(dp, &entry, &result)) { warnx("FTP: list %s readdir_r failure\n", dirPath); - errorCode = kErrIO; + errorCode = kErrFailErrno; break; } // no more entries? if (result == nullptr) { - if (hdr->offset != 0 && offset == 0) { + if (payload->offset != 0 && offset == 0) { // User is requesting subsequent dir entries but there were none. This means the user asked // to seek past EOF. errorCode = kErrEOF; @@ -299,94 +380,95 @@ MavlinkFTP::_workList(Request *req) } // Move the data into the buffer - hdr->data[offset++] = direntType; - strcpy((char *)&hdr->data[offset], buf); + payload->data[offset++] = direntType; + strcpy((char *)&payload->data[offset], buf); #ifdef MAVLINK_FTP_DEBUG - printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]); + printf("FTP: list %s %s\n", dirPath, (char *)&payload->data[offset-1]); #endif offset += nameLen + 1; } closedir(dp); - hdr->size = offset; + payload->size = offset; return errorCode; } +/// @brief Responds to an Open command MavlinkFTP::ErrorCode -MavlinkFTP::_workOpen(Request *req, bool create) +MavlinkFTP::_workOpen(PayloadHeader* payload, bool create) { - auto hdr = req->header(); - - int session_index = _findUnusedSession(); + int session_index = _find_unused_session(); if (session_index < 0) { - return kErrNoSession; + warnx("FTP: Open failed - out of sessions\n"); + return kErrNoSessionsAvailable; } - + + char *filename = _data_as_cstring(payload); uint32_t fileSize = 0; if (!create) { struct stat st; - if (stat(req->dataAsCString(), &st) != 0) { - return kErrNotFile; + if (stat(filename, &st) != 0) { + return kErrFailErrno; } fileSize = st.st_size; } int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY; - int fd = ::open(req->dataAsCString(), oflag); + int fd = ::open(filename, oflag); if (fd < 0) { - return create ? kErrPerm : kErrNotFile; + return kErrFailErrno; } _session_fds[session_index] = fd; - hdr->session = session_index; + payload->session = session_index; if (create) { - hdr->size = 0; + payload->size = 0; } else { - hdr->size = sizeof(uint32_t); - *((uint32_t*)hdr->data) = fileSize; + payload->size = sizeof(uint32_t); + *((uint32_t*)payload->data) = fileSize; } return kErrNone; } +/// @brief Responds to a Read command MavlinkFTP::ErrorCode -MavlinkFTP::_workRead(Request *req) +MavlinkFTP::_workRead(PayloadHeader* payload) { - auto hdr = req->header(); + int session_index = payload->session; - int session_index = hdr->session; - - if (!_validSession(session_index)) { - return kErrNoSession; + if (!_valid_session(session_index)) { + return kErrInvalidSession; } // Seek to the specified position #ifdef MAVLINK_FTP_DEBUG - warnx("seek %d", hdr->offset); + warnx("seek %d", payload->offset); #endif - if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) { + if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) { // Unable to see to the specified location warnx("seek fail"); return kErrEOF; } - int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength); + int bytes_read = ::read(_session_fds[session_index], &payload->data[0], kMaxDataLength); if (bytes_read < 0) { // Negative return indicates error other than eof warnx("read fail %d", bytes_read); - return kErrIO; + return kErrFailErrno; } - hdr->size = bytes_read; + payload->size = bytes_read; return kErrNone; } +/// @brief Responds to a Write command MavlinkFTP::ErrorCode -MavlinkFTP::_workWrite(Request *req) +MavlinkFTP::_workWrite(PayloadHeader* payload) { #if 0 // NYI: Coming soon @@ -409,35 +491,44 @@ MavlinkFTP::_workWrite(Request *req) hdr->size = result; return kErrNone; #else - return kErrPerm; + return kErrUnknownCommand; #endif } +/// @brief Responds to a RemoveFile command MavlinkFTP::ErrorCode -MavlinkFTP::_workRemove(Request *req) +MavlinkFTP::_workRemoveFile(PayloadHeader* payload) { - //auto hdr = req->header(); - - // for now, send error reply - return kErrPerm; + char file[kMaxDataLength]; + strncpy(file, _data_as_cstring(payload), kMaxDataLength); + + if (unlink(file) == 0) { + payload->size = 0; + return kErrNone; + } else { + return kErrFailErrno; + } } +/// @brief Responds to a Terminate command MavlinkFTP::ErrorCode -MavlinkFTP::_workTerminate(Request *req) +MavlinkFTP::_workTerminate(PayloadHeader* payload) { - auto hdr = req->header(); - - if (!_validSession(hdr->session)) { - return kErrNoSession; + if (!_valid_session(payload->session)) { + return kErrInvalidSession; } - ::close(_session_fds[hdr->session]); + ::close(_session_fds[payload->session]); + _session_fds[payload->session] = -1; + + payload->size = 0; return kErrNone; } +/// @brief Responds to a Reset command MavlinkFTP::ErrorCode -MavlinkFTP::_workReset(void) +MavlinkFTP::_workReset(PayloadHeader* payload) { for (size_t i=0; isize = 0; + return kErrNone; } +/// @brief Responds to a RemoveDirectory command +MavlinkFTP::ErrorCode +MavlinkFTP::_workRemoveDirectory(PayloadHeader* payload) +{ + char dir[kMaxDataLength]; + strncpy(dir, _data_as_cstring(payload), kMaxDataLength); + + if (rmdir(dir) == 0) { + payload->size = 0; + return kErrNone; + } else { + return kErrFailErrno; + } +} + +/// @brief Responds to a CreateDirectory command +MavlinkFTP::ErrorCode +MavlinkFTP::_workCreateDirectory(PayloadHeader* payload) +{ + char dir[kMaxDataLength]; + strncpy(dir, _data_as_cstring(payload), kMaxDataLength); + + if (mkdir(dir, S_IRWXU | S_IRWXG | S_IRWXO) == 0) { + payload->size = 0; + return kErrNone; + } else { + return kErrFailErrno; + } +} + +/// @brief Returns true if the specified session is a valid open session bool -MavlinkFTP::_validSession(unsigned index) +MavlinkFTP::_valid_session(unsigned index) { if ((index >= kMaxSession) || (_session_fds[index] < 0)) { return false; @@ -458,8 +582,9 @@ MavlinkFTP::_validSession(unsigned index) return true; } +/// @brief Returns an unused session index int -MavlinkFTP::_findUnusedSession(void) +MavlinkFTP::_find_unused_session(void) { for (size_t i=0; isize < kMaxDataLength) { - requestData()[header()->size] = '\0'; + if (payload->size < kMaxDataLength) { + payload->data[payload->size] = '\0'; } else { - requestData()[kMaxDataLength - 1] = '\0'; + payload->data[kMaxDataLength - 1] = '\0'; } // and return data - return (char *)&(header()->data[0]); + 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(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 Returns the 32 bit CRC for the payload, crc32 and padding members are set to 0 for calculation. +uint32_t +MavlinkFTP::_payload_crc32(PayloadHeader *payload) +{ + // We calculate CRC with crc and padding set to 0. + uint32_t saveCRC = payload->crc32; + payload->crc32 = 0; + payload->padding[0] = 0; + payload->padding[1] = 0; + payload->padding[2] = 0; + uint32_t retCRC = crc32((const uint8_t*)payload, payload->size + sizeof(PayloadHeader)); + payload->crc32 = saveCRC; + + return retCRC; } diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 1dd8f102e..b4cc154d1 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -33,17 +33,8 @@ #pragma once -/** - * @file mavlink_ftp.h - * - * MAVLink remote file server. - * - * A limited number of requests (currently 2) may be outstanding at a time. - * Additional messages will be discarded. - * - * Messages consist of a fixed header, followed by a data area. - * - */ +/// @file mavlink_ftp.h +/// @author px4dev, Don Gagne #include #include @@ -54,183 +45,131 @@ #include "mavlink_messages.h" #include "mavlink_main.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 { 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(); - - static MavlinkFTP *getServer(); - - // static interface - void handle_message(Mavlink* mavlink, - mavlink_message_t *msg); - -private: - - static const unsigned kRequestQueueSize = 2; - - static MavlinkFTP *_server; - - /// @brief Trying to pack structures across differing compilers is questionable for Clients, so we pad the - /// structure ourselves to 32 bit alignment which should get us what we want. - struct RequestHeader + + /// @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); + + /// @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); + + /// @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 - uint8_t session; ///< Session id for read and write commands - uint8_t opcode; ///< Command opcode - uint8_t size; ///< Size of data - uint8_t padding[3]; - uint32_t crc32; ///< CRC for entire Request structure, with crc32 and padding set to 0 - uint32_t offset; ///< Offsets for List and Read commands - uint8_t data[]; + uint16_t seqNumber; ///< 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 padding[3]; ///< 32 bit aligment padding + uint32_t crc32; ///< CRC for entire Request structure, with crc32 and padding set to 0 + uint32_t offset; ///< Offsets for List and Read commands + uint8_t data[]; ///< command data, varies by Opcode }; - + + /// @brief Command opcodes enum Opcode : uint8_t { - kCmdNone, // ignored, always acked - kCmdTerminate, // releases sessionID, closes file - kCmdReset, // terminates all sessions - kCmdList, // list files in from - kCmdOpen, // opens for reading, returns - kCmdRead, // reads bytes from in - kCmdCreate, // creates for writing, returns - kCmdWrite, // appends bytes at in - kCmdRemove, // remove file (only if created by server?) - - kRspAck, - kRspNak + kCmdNone, ///< ignored, always acked + kCmdTerminateSession, ///< Terminates open Read session + kCmdResetSessions, ///< Terminates all open Read sessions + kCmdListDirectory, ///< List files in from + kCmdOpenFile, ///< Opens file at for reading, returns + kCmdReadFile, ///< Reads bytes from in + kCmdCreateFile, ///< Creates file at for writing, returns + kCmdWriteFile, ///< Appends bytes to file in + kCmdRemoveFile, ///< Remove file at + kCmdCreateDirectory, ///< Creates directory at + kCmdRemoveDirectory, ///< Removes Directory at , must be empty + + kRspAck, ///< Ack response + kRspNak ///< Nak response }; - + + /// @brief Error codes returned in Nak response PayloadHeader.data[0]. enum ErrorCode : uint8_t - { + { kErrNone, - kErrNoRequest, - kErrNoSession, - kErrSequence, - kErrNotDir, - kErrNotFile, - kErrEOF, - kErrNotAppend, - kErrTooBig, - kErrIO, - kErrPerm - }; - - int _findUnusedSession(void); - bool _validSession(unsigned index); - - static const unsigned kMaxSession = 2; - int _session_fds[kMaxSession]; - - class Request + kErrFail, ///< Unknown failure + kErrFailErrno, ///< Command failed, errno sent back in PayloadHeader.data[1] + kErrInvalidDataSize, ///< PayloadHeader.size is invalid + kErrInvalidSession, ///< Session is not currently open + kErrNoSessionsAvailable, ///< All available Sessions in use + kErrEOF, ///< Offset past end of file for List and Read commands + kErrUnknownCommand, ///< Unknown command opcode + kErrCrc ///< CRC on Payload is incorrect + }; + +private: + /// @brief Unit of work which is queued to work_queue + struct Request { - public: - union { - dq_entry_t entry; - work_s work; - }; - - bool decode(Mavlink *mavlink, mavlink_message_t *fromMessage) { - if (fromMessage->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) { - _systemId = fromMessage->sysid; - _mavlink = mavlink; - mavlink_msg_file_transfer_protocol_decode(fromMessage, &_message); - return _message.target_system == _mavlink->get_system_id(); - } - return false; - } - - void reply() { - - // XXX the proper way would be an IOCTL / uORB call, rather than exploiting the - // flat memory architecture, as we're operating between threads here. - mavlink_message_t msg; - msg.checksum = 0; - unsigned len = mavlink_msg_file_transfer_protocol_pack_chan(_mavlink->get_system_id(), // Sender system id - _mavlink->get_component_id(), // Sender component id - _mavlink->get_channel(), // Channel to send on - &msg, // Message to pack payload into - 0, // Target network - _systemId, // Target system id - 0, // Target component id - rawData()); // Payload to pack into message - - _mavlink->lockMessageBufferMutex(); - bool success = _mavlink->message_buffer_write(&msg, len); - _mavlink->unlockMessageBufferMutex(); - - if (!success) { - warnx("FTP TX ERR"); - } -#ifdef MAVLINK_FTP_DEBUG - else { - warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d", - _mavlink->get_system_id(), - _mavlink->get_component_id(), - _mavlink->get_channel(), - len, - msg.checksum); - } -#endif - } - - uint8_t *rawData() { return &_message.payload[0]; } - RequestHeader *header() { return reinterpret_cast(&_message.payload[0]); } - uint8_t *requestData() { return &(header()->data[0]); } - unsigned dataSize() { return header()->size + sizeof(RequestHeader); } - mavlink_channel_t channel() { return _mavlink->get_channel(); } - - char *dataAsCString(); - - private: - Mavlink *_mavlink; - mavlink_file_transfer_protocol_t _message; - uint8_t _systemId; - + 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 }; - - static const char kDirentFile = 'F'; - static const char kDirentDir = 'D'; - static const char kDirentUnknown = 'U'; - static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(RequestHeader); - - /// Request worker; runs on the low-priority work queue to service - /// remote requests. - /// - static void _workerTrampoline(void *arg); - void _worker(Request *req); - - /// Reply to a request (XXX should be a Request method) - /// - void _reply(Request *req); - - ErrorCode _workList(Request *req); - ErrorCode _workOpen(Request *req, bool create); - ErrorCode _workRead(Request *req); - ErrorCode _workWrite(Request *req); - ErrorCode _workRemove(Request *req); - ErrorCode _workTerminate(Request *req); - ErrorCode _workReset(); - - // work freelist - Request _workBufs[kRequestQueueSize]; - dq_queue_t _workFree; - sem_t _lock; - - void _qLock() { do {} while (sem_wait(&_lock) != 0); } - void _qUnlock() { sem_post(&_lock); } - - void _qFree(Request *req) { - _qLock(); - dq_addlast(&req->entry, &_workFree); - _qUnlock(); - } - - Request *_dqFree() { - _qLock(); - auto req = reinterpret_cast(dq_remfirst(&_workFree)); - _qUnlock(); - return req; - } - + + Request *_get_request(void); + void _return_request(Request *req); + void _lock_request_queue(void); + void _unlock_request_queue(void); + + uint32_t _payload_crc32(PayloadHeader *hdr); + + char *_data_as_cstring(PayloadHeader* payload); + + static void _worker_trampoline(void *arg); + void _process_request(Request *req); + void _reply(Request *req); + + ErrorCode _workList(PayloadHeader *payload); + ErrorCode _workOpen(PayloadHeader *payload, bool create); + ErrorCode _workRead(PayloadHeader *payload); + ErrorCode _workWrite(PayloadHeader *payload); + ErrorCode _workTerminate(PayloadHeader *payload); + ErrorCode _workReset(PayloadHeader* payload); + ErrorCode _workRemoveDirectory(PayloadHeader *payload); + ErrorCode _workCreateDirectory(PayloadHeader *payload); + ErrorCode _workRemoveFile(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); + + static const char kDirentFile = 'F'; ///< Identifies File returned from List command + static const char kDirentDir = 'D'; ///< Identifies Directory returned from List command + static const char kDirentUnknown = 'U'; ///< Identifies Unknown entry returned from List command + + /// @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 + + ReceiveMessageFunc_t _utRcvMsgFunc; ///< Unit test override for mavlink message sending + MavlinkFtpTest *_ftp_test; ///< Additional parameter to _utRcvMsgFunc; }; -- cgit v1.2.3 From 1d038eed047358eee30513673bbcb29bf93dd78e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 3 Sep 2014 22:44:16 +0400 Subject: UAVCAN: GNSS fix message update --- src/modules/uavcan/sensors/gnss.cpp | 8 +++----- uavcan | 2 +- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 0d67aad47..24afe6aaf 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -43,8 +43,6 @@ #include #include -#define MM_PER_CM 10 // Millimeters per centimeter - const char *const UavcanGnssBridge::NAME = "gnss"; UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : @@ -95,9 +93,9 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure Date: Fri, 5 Sep 2014 10:04:42 +0200 Subject: loiter mission items: better reached check for FW --- src/modules/navigator/mission_block.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 4adf77dce..157a1cf71 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -113,6 +113,19 @@ MissionBlock::is_mission_item_reached() if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) { _waypoint_position_reached = true; } + } else if (!_navigator->get_vstatus()->is_rotary_wing && + (_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT)) { + /* Loiter mission item on a non rotary wing: the aircraft is going to circle the + * coordinates with a radius equal to the loiter_radius field + * Therefore the item is marked as reached once the system reaches the loiter + * radius (+ some margin). Time inside and turn count is handled elsewhere. + */ + if (dist >= 0.0f && dist <= _mission_item.loiter_radius * 1.2f) { + _waypoint_position_reached = true; + } + } else { /* for normal mission items used their acceptance radius */ if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { -- cgit v1.2.3 From 81837dcdde4bdef31fc153cd1f893e2fefc74c58 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Sep 2014 10:18:25 +0200 Subject: update comment --- src/modules/navigator/mission_block.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 157a1cf71..723caec7c 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -118,14 +118,14 @@ MissionBlock::is_mission_item_reached() _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT)) { /* Loiter mission item on a non rotary wing: the aircraft is going to circle the - * coordinates with a radius equal to the loiter_radius field + * coordinates with a radius equal to the loiter_radius field. It is not flying + * through the waypoint center. * Therefore the item is marked as reached once the system reaches the loiter * radius (+ some margin). Time inside and turn count is handled elsewhere. */ if (dist >= 0.0f && dist <= _mission_item.loiter_radius * 1.2f) { _waypoint_position_reached = true; } - } else { /* for normal mission items used their acceptance radius */ if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { -- cgit v1.2.3 From f315be54162c3284742cd2195d9c78c153289e66 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 6 Sep 2014 10:36:51 +0200 Subject: FW: in seatbelt/althold on ground reset integrators --- src/modules/fw_att_control/fw_att_control_main.cpp | 33 ++++++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index ad873203e..c60d8d348 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -63,6 +63,7 @@ #include #include #include +#include #include #include #include @@ -124,6 +125,7 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_sub; /**< notification of manual control updates */ int _global_pos_sub; /**< global position subscription */ + int _vehicle_status_sub; /**< vehicle status subscription */ orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ @@ -139,6 +141,7 @@ private: struct actuator_controls_s _actuators; /**< actuator control inputs */ struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ struct vehicle_global_position_s _global_pos; /**< global position */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ @@ -275,6 +278,11 @@ private: */ void global_pos_poll(); + /** + * Check for vehicle status updates. + */ + void vehicle_status_poll(); + /** * Shim for calling task_main from task_create. */ @@ -313,6 +321,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _params_sub(-1), _manual_sub(-1), _global_pos_sub(-1), + _vehicle_status_sub(-1), /* publications */ _rate_sp_pub(-1), @@ -338,6 +347,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _actuators = {}; _actuators_airframe = {}; _global_pos = {}; + _vehicle_status = {}; _parameter_handles.tconst = param_find("FW_ATT_TC"); @@ -560,6 +570,18 @@ FixedwingAttitudeControl::global_pos_poll() } } +void +FixedwingAttitudeControl::vehicle_status_poll() +{ + /* check if there is new status information */ + bool vehicle_status_updated; + orb_check(_vehicle_status_sub, &vehicle_status_updated); + + if (vehicle_status_updated) { + orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + } +} + void FixedwingAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -585,6 +607,7 @@ FixedwingAttitudeControl::task_main() _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vcontrol_mode_sub, 200); @@ -599,6 +622,7 @@ FixedwingAttitudeControl::task_main() vehicle_accel_poll(); vehicle_control_mode_poll(); vehicle_manual_poll(); + vehicle_status_poll(); /* wakeup source(s) */ struct pollfd fds[2]; @@ -667,6 +691,8 @@ FixedwingAttitudeControl::task_main() global_pos_poll(); + vehicle_status_poll(); + /* lock integrator until control is started */ bool lock_integrator; @@ -779,6 +805,13 @@ FixedwingAttitudeControl::task_main() } } + /* If the aircraft is on ground reset the integrators */ + if (_vehicle_status.condition_landed) { + _roll_ctrl.reset_integrator(); + _pitch_ctrl.reset_integrator(); + _yaw_ctrl.reset_integrator(); + } + /* Prepare speed_body_u and speed_body_w */ float speed_body_u = 0.0f; float speed_body_v = 0.0f; -- cgit v1.2.3