dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

This commit is contained in:
Lorenz Meier 2014-10-07 09:25:59 +02:00
parent c2687a7774 c0eefc983a
commit 082a0c7aa5
17 changed files with 483 additions and 178 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
ROMFS/px4fmu_test/init.d/rcS
View File

@ -75,3 +75,33 @@ if [ -f /fs/microsd/mount_test_cmds.txt ]
then
tests mount
fi
#
# Run unit tests at board boot, reporting failure as needed.
# Add new unit tests using the same pattern as below.
#
set unit_test_failure 0
if mavlink_tests
then
else
set unit_test_failure 1
set unit_test_failure_list "${unit_test_failure_list} mavlink_tests"
fi
if commander_tests
then
else
set unit_test_failure 1
set unit_test_failure_list "${unit_test_failure_list} commander_tests"
fi
if [ $unit_test_failure == 0 ]
then
echo
echo "All Unit Tests PASSED"
else
echo
echo "Some Unit Tests FAILED:${unit_test_failure_list}"
fi

1
makefiles/config_px4fmu-v2_test.mk
View File

@ -56,6 +56,7 @@ LIBRARIES += lib/mathlib/CMSIS
MODULES += modules/unit_test
MODULES += modules/mavlink/mavlink_tests
MODULES += modules/commander/commander_tests
#
# Transitional support - add commands from the NuttX export archive.

4
src/modules/commander/commander_tests/commander_tests.cpp
View File

@ -48,7 +48,5 @@ extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]);
int commander_tests_main(int argc, char *argv[])
{
stateMachineHelperTest();
return 0;
return stateMachineHelperTest() ? 0 : -1;
}

13
src/modules/commander/commander_tests/state_machine_helper_test.cpp
View File

@ -49,7 +49,7 @@ public:
StateMachineHelperTest();
virtual ~StateMachineHelperTest();
virtual void runTests(void);
virtual bool run_tests(void);
private:
bool armingStateTransitionTest();
@ -488,16 +488,13 @@ bool StateMachineHelperTest::isSafeTest(void)
return true;
}
void StateMachineHelperTest::runTests(void)
bool StateMachineHelperTest::run_tests(void)
{
ut_run_test(armingStateTransitionTest);
ut_run_test(mainStateTransitionTest);
ut_run_test(isSafeTest);
return (_tests_failed == 0);
}
void stateMachineHelperTest(void)
{
StateMachineHelperTest* test = new StateMachineHelperTest();
test->runTests();
test->printResults();
}
ut_declare_test(stateMachineHelperTest, StateMachineHelperTest)

2
src/modules/commander/commander_tests/state_machine_helper_test.h
View File

@ -39,6 +39,6 @@
#ifndef STATE_MACHINE_HELPER_TEST_H_
#define STATE_MACHINE_HELPER_TEST_
void stateMachineHelperTest(void);
bool stateMachineHelperTest(void);
#endif /* STATE_MACHINE_HELPER_TEST_H_ */

2
src/modules/dataman/module.mk
View File

@ -40,3 +40,5 @@ MODULE_COMMAND = dataman
SRCS = dataman.c
MODULE_STACKSIZE = 1200
MAXOPTIMIZATION = -Os

292
src/modules/mavlink/mavlink_ftp.cpp
View File

@ -34,6 +34,7 @@
/// @file mavlink_ftp.cpp
/// @author px4dev, Don Gagne <don@thegagnes.com>
#include <crc32.h>
#include <unistd.h>
#include <stdio.h>
#include <fcntl.h>
@ -180,12 +181,16 @@ MavlinkFTP::_process_request(Request *req)
errorCode = _workList(payload);
break;
case kCmdOpenFile:
errorCode = _workOpen(payload, false);
case kCmdOpenFileRO:
errorCode = _workOpen(payload, O_RDONLY);
break;
case kCmdCreateFile:
errorCode = _workOpen(payload, true);
errorCode = _workOpen(payload, O_CREAT | O_EXCL | O_WRONLY);
break;
case kCmdOpenFileWO:
errorCode = _workOpen(payload, O_CREAT | O_WRONLY);
break;
case kCmdReadFile:
@ -200,6 +205,14 @@ MavlinkFTP::_process_request(Request *req)
errorCode = _workRemoveFile(payload);
break;
case kCmdRename:
errorCode = _workRename(payload);
break;
case kCmdTruncateFile:
errorCode = _workTruncateFile(payload);
break;
case kCmdCreateDirectory:
errorCode = _workCreateDirectory(payload);
break;
@ -208,6 +221,11 @@ MavlinkFTP::_process_request(Request *req)
errorCode = _workRemoveDirectory(payload);
break;
case kCmdCalcFileCRC32:
errorCode = _workCalcFileCRC32(payload);
break;
default:
errorCode = kErrUnknownCommand;
break;
@ -222,6 +240,7 @@ out:
warnx("FTP: ack\n");
#endif
} else {
int r_errno = errno;
warnx("FTP: nak %u", errorCode);
payload->req_opcode = payload->opcode;
payload->opcode = kRspNak;
@ -229,7 +248,7 @@ out:
payload->data[0] = errorCode;
if (errorCode == kErrFailErrno) {
payload->size = 2;
payload->data[1] = errno;
payload->data[1] = r_errno;
}
}
@ -396,27 +415,27 @@ MavlinkFTP::_workList(PayloadHeader* payload)
/// @brief Responds to an Open command
MavlinkFTP::ErrorCode
MavlinkFTP::_workOpen(PayloadHeader* payload, bool create)
MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag)
{
int session_index = _find_unused_session();
if (session_index < 0) {
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(filename, &st) != 0) {
return kErrFailErrno;
}
fileSize = st.st_size;
}
int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
char *filename = _data_as_cstring(payload);
uint32_t fileSize = 0;
struct stat st;
if (stat(filename, &st) != 0) {
// fail only if requested open for read
if (oflag & O_RDONLY)
return kErrFailErrno;
else
st.st_size = 0;
}
fileSize = st.st_size;
int fd = ::open(filename, oflag);
if (fd < 0) {
return kErrFailErrno;
@ -424,12 +443,8 @@ MavlinkFTP::_workOpen(PayloadHeader* payload, bool create)
_session_fds[session_index] = fd;
payload->session = session_index;
if (create) {
payload->size = 0;
} else {
payload->size = sizeof(uint32_t);
*((uint32_t*)payload->data) = fileSize;
}
payload->size = sizeof(uint32_t);
*((uint32_t*)payload->data) = fileSize;
return kErrNone;
}
@ -470,29 +485,33 @@ MavlinkFTP::_workRead(PayloadHeader* payload)
MavlinkFTP::ErrorCode
MavlinkFTP::_workWrite(PayloadHeader* payload)
{
#if 0
// NYI: Coming soon
auto hdr = req->header();
int session_index = payload->session;
// look up session
auto session = getSession(hdr->session);
if (session == nullptr) {
return kErrNoSession;
if (!_valid_session(session_index)) {
return kErrInvalidSession;
}
// append to file
int result = session->append(hdr->offset, &hdr->data[0], hdr->size);
if (result < 0) {
// XXX might also be no space, I/O, etc.
return kErrNotAppend;
}
hdr->size = result;
return kErrNone;
#else
return kErrUnknownCommand;
// 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) {
// 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);
if (bytes_written < 0) {
// Negative return indicates error other than eof
warnx("write fail %d", bytes_written);
return kErrFailErrno;
}
payload->size = sizeof(uint32_t);
*((uint32_t*)payload->data) = bytes_written;
return kErrNone;
}
/// @brief Responds to a RemoveFile command
@ -510,6 +529,81 @@ MavlinkFTP::_workRemoveFile(PayloadHeader* payload)
}
}
/// @brief Responds to a TruncateFile command
MavlinkFTP::ErrorCode
MavlinkFTP::_workTruncateFile(PayloadHeader* payload)
{
char file[kMaxDataLength];
const char temp_file[] = "/fs/microsd/.trunc.tmp";
strncpy(file, _data_as_cstring(payload), kMaxDataLength);
payload->size = 0;
// emulate truncate(file, payload->offset) by
// copying to temp and overwrite with O_TRUNC flag.
struct stat st;
if (stat(file, &st) != 0) {
return kErrFailErrno;
}
if (!S_ISREG(st.st_mode)) {
errno = EISDIR;
return kErrFailErrno;
}
// check perms allow us to write (not romfs)
if (!(st.st_mode & (S_IWUSR | S_IWGRP | S_IWOTH))) {
errno = EROFS;
return kErrFailErrno;
}
if (payload->offset == (unsigned)st.st_size) {
// nothing to do
return kErrNone;
}
else if (payload->offset == 0) {
// 1: truncate all data
int fd = ::open(file, O_TRUNC | O_WRONLY);
if (fd < 0) {
return kErrFailErrno;
}
::close(fd);
return kErrNone;
}
else if (payload->offset > (unsigned)st.st_size) {
// 2: extend file
int fd = ::open(file, O_WRONLY);
if (fd < 0) {
return kErrFailErrno;
}
if (lseek(fd, payload->offset - 1, SEEK_SET) < 0) {
::close(fd);
return kErrFailErrno;
}
bool ok = 1 == ::write(fd, "", 1);
::close(fd);
return (ok)? kErrNone : kErrFailErrno;
}
else {
// 3: truncate
if (_copy_file(file, temp_file, payload->offset) != 0) {
return kErrFailErrno;
}
if (_copy_file(temp_file, file, payload->offset) != 0) {
return kErrFailErrno;
}
if (::unlink(temp_file) != 0) {
return kErrFailErrno;
}
return kErrNone;
}
}
/// @brief Responds to a Terminate command
MavlinkFTP::ErrorCode
MavlinkFTP::_workTerminate(PayloadHeader* payload)
@ -542,6 +636,33 @@ MavlinkFTP::_workReset(PayloadHeader* payload)
return kErrNone;
}
/// @brief Responds to a Rename command
MavlinkFTP::ErrorCode
MavlinkFTP::_workRename(PayloadHeader* payload)
{
char oldpath[kMaxDataLength];
char newpath[kMaxDataLength];
char *ptr = _data_as_cstring(payload);
size_t oldpath_sz = strlen(ptr);
if (oldpath_sz == payload->size) {
// no newpath
errno = EINVAL;
return kErrFailErrno;
}
strncpy(oldpath, ptr, kMaxDataLength);
strncpy(newpath, ptr + oldpath_sz + 1, kMaxDataLength);
if (rename(oldpath, newpath) == 0) {
payload->size = 0;
return kErrNone;
} else {
return kErrFailErrno;
}
}
/// @brief Responds to a RemoveDirectory command
MavlinkFTP::ErrorCode
MavlinkFTP::_workRemoveDirectory(PayloadHeader* payload)
@ -572,6 +693,39 @@ MavlinkFTP::_workCreateDirectory(PayloadHeader* payload)
}
}
/// @brief Responds to a CalcFileCRC32 command
MavlinkFTP::ErrorCode
MavlinkFTP::_workCalcFileCRC32(PayloadHeader* payload)
{
char file_buf[256];
uint32_t checksum = 0;
ssize_t bytes_read;
strncpy(file_buf, _data_as_cstring(payload), kMaxDataLength);
int fd = ::open(file_buf, O_RDONLY);
if (fd < 0) {
return kErrFailErrno;
}
do {
bytes_read = ::read(fd, file_buf, sizeof(file_buf));
if (bytes_read < 0) {
int r_errno = errno;
::close(fd);
errno = r_errno;
return kErrFailErrno;
}
checksum = crc32part((uint8_t*)file_buf, bytes_read, checksum);
} while (bytes_read == sizeof(file_buf));
::close(fd);
payload->size = sizeof(uint32_t);
*((uint32_t*)payload->data) = checksum;
return kErrNone;
}
/// @brief Returns true if the specified session is a valid open session
bool
MavlinkFTP::_valid_session(unsigned index)
@ -645,3 +799,55 @@ MavlinkFTP::_return_request(Request *req)
_unlock_request_queue();
}
/// @brief Copy file (with limited space)
int
MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t length)
{
char buff[512];
int src_fd = -1, dst_fd = -1;
int op_errno = 0;
src_fd = ::open(src_path, O_RDONLY);
if (src_fd < 0) {
return -1;
}
dst_fd = ::open(dst_path, O_CREAT | O_TRUNC | O_WRONLY);
if (dst_fd < 0) {
op_errno = errno;
::close(src_fd);
errno = op_errno;
return -1;
}
while (length > 0) {
ssize_t bytes_read, bytes_written;
size_t blen = (length > sizeof(buff))? sizeof(buff) : length;
bytes_read = ::read(src_fd, buff, blen);
if (bytes_read == 0) {
// EOF
break;
}
else if (bytes_read < 0) {
warnx("cp: read");
op_errno = errno;
break;
}
bytes_written = ::write(dst_fd, buff, bytes_read);
if (bytes_written != bytes_read) {
warnx("cp: short write");
op_errno = errno;
break;
}
length -= bytes_written;
}
::close(src_fd);
::close(dst_fd);
errno = op_errno;
return (length > 0)? -1 : 0;
}

14
src/modules/mavlink/mavlink_ftp.h
View File

@ -89,13 +89,17 @@ public:
kCmdTerminateSession, ///< Terminates open Read session
kCmdResetSessions, ///< Terminates all open Read sessions
kCmdListDirectory, ///< List files in <path> from <offset>
kCmdOpenFile, ///< Opens file at <path> for reading, returns <session>
kCmdOpenFileRO, ///< Opens file at <path> for reading, returns <session>
kCmdReadFile, ///< Reads <size> bytes from <offset> in <session>
kCmdCreateFile, ///< Creates file at <path> for writing, returns <session>
kCmdWriteFile, ///< Appends <size> bytes to file in <session>
kCmdWriteFile, ///< Writes <size> bytes to <offset> in <session>
kCmdRemoveFile, ///< Remove file at <path>
kCmdCreateDirectory, ///< Creates directory at <path>
kCmdRemoveDirectory, ///< Removes Directory at <path>, must be empty
kCmdOpenFileWO, ///< Opens file at <path> for writing, returns <session>
kCmdTruncateFile, ///< Truncate file at <path> to <offset> length
kCmdRename, ///< Rename <path1> to <path2>
kCmdCalcFileCRC32, ///< Calculate CRC32 for file at <path>
kRspAck = 128, ///< Ack response
kRspNak ///< Nak response
@ -138,9 +142,10 @@ private:
static void _worker_trampoline(void *arg);
void _process_request(Request *req);
void _reply(Request *req);
int _copy_file(const char *src_path, const char *dst_path, ssize_t length);
ErrorCode _workList(PayloadHeader *payload);
ErrorCode _workOpen(PayloadHeader *payload, bool create);
ErrorCode _workOpen(PayloadHeader *payload, int oflag);
ErrorCode _workRead(PayloadHeader *payload);
ErrorCode _workWrite(PayloadHeader *payload);
ErrorCode _workTerminate(PayloadHeader *payload);
@ -148,6 +153,9 @@ private:
ErrorCode _workRemoveDirectory(PayloadHeader *payload);
ErrorCode _workCreateDirectory(PayloadHeader *payload);
ErrorCode _workRemoveFile(PayloadHeader *payload);
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

20
src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp
View File

@ -65,7 +65,7 @@ MavlinkFtpTest::~MavlinkFtpTest()
}
/// @brief Called before every test to initialize the FTP Server.
void MavlinkFtpTest::init(void)
void MavlinkFtpTest::_init(void)
{
_ftp_server = new MavlinkFTP;;
_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message, this);
@ -74,7 +74,7 @@ void MavlinkFtpTest::init(void)
}
/// @brief Called after every test to take down the FTP Server.
void MavlinkFtpTest::cleanup(void)
void MavlinkFtpTest::_cleanup(void)
{
delete _ftp_server;
@ -265,7 +265,7 @@ bool MavlinkFtpTest::_open_badfile_test(void)
MavlinkFTP::PayloadHeader *reply;
const char *dir = "/foo"; // non-existent file
payload.opcode = MavlinkFTP::kCmdOpenFile;
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
payload.offset = 0;
bool success = _send_receive_msg(&payload, // FTP payload header
@ -295,7 +295,7 @@ bool MavlinkFtpTest::_open_terminate_test(void)
struct stat st;
const ReadTestCase *test = &_rgReadTestCases[i];
payload.opcode = MavlinkFTP::kCmdOpenFile;
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
payload.offset = 0;
bool success = _send_receive_msg(&payload, // FTP payload header
@ -342,7 +342,7 @@ bool MavlinkFtpTest::_terminate_badsession_test(void)
MavlinkFTP::PayloadHeader *reply;
const char *file = _rgReadTestCases[0].file;
payload.opcode = MavlinkFTP::kCmdOpenFile;
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
payload.offset = 0;
bool success = _send_receive_msg(&payload, // FTP payload header
@ -400,7 +400,7 @@ bool MavlinkFtpTest::_read_test(void)
// 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.opcode = MavlinkFTP::kCmdOpenFileRO;
payload.offset = 0;
bool success = _send_receive_msg(&payload, // FTP payload header
@ -463,7 +463,7 @@ bool MavlinkFtpTest::_read_badsession_test(void)
MavlinkFTP::PayloadHeader *reply;
const char *file = _rgReadTestCases[0].file;
payload.opcode = MavlinkFTP::kCmdOpenFile;
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
payload.offset = 0;
bool success = _send_receive_msg(&payload, // FTP payload header
@ -738,7 +738,7 @@ void MavlinkFtpTest::_cleanup_microsd(void)
}
/// @brief Runs all the unit tests
void MavlinkFtpTest::runTests(void)
bool MavlinkFtpTest::run_tests(void)
{
ut_run_test(_ack_test);
ut_run_test(_bad_opcode_test);
@ -753,5 +753,9 @@ void MavlinkFtpTest::runTests(void)
ut_run_test(_removedirectory_test);
ut_run_test(_createdirectory_test);
ut_run_test(_removefile_test);
return (_tests_failed == 0);
}
ut_declare_test(mavlink_ftp_test, MavlinkFtpTest)

9
src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h
View File

@ -46,10 +46,7 @@ public:
MavlinkFtpTest();
virtual ~MavlinkFtpTest();
virtual void init(void);
virtual void cleanup(void);
virtual void runTests(void);
virtual bool run_tests(void);
static void receive_message(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest);
@ -65,6 +62,9 @@ public:
MavlinkFtpTest& operator=(const MavlinkFtpTest&);
private:
virtual void _init(void);
virtual void _cleanup(void);
bool _ack_test(void);
bool _bad_opcode_test(void);
bool _bad_datasize_test(void);
@ -105,3 +105,4 @@ private:
static const char _unittest_microsd_file[];
};
bool mavlink_ftp_test(void);

7
src/modules/mavlink/mavlink_tests/mavlink_tests.cpp
View File

@ -43,10 +43,5 @@ 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;
return mavlink_ftp_test() ? 0 : -1;
}

2
src/modules/mavlink/mavlink_tests/module.mk
View File

@ -45,4 +45,6 @@ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
MODULE_STACKSIZE = 5000
MAXOPTIMIZATION = -Os
EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST

97
src/modules/navigator/mission.cpp
View File

@ -37,6 +37,7 @@
*
* @author Julian Oes <julian@oes.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#include <sys/types.h>
@ -113,6 +114,7 @@ Mission::on_inactive()
update_offboard_mission();
}
/* require takeoff after non-loiter or landing */
if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) {
_need_takeoff = true;
}
@ -147,8 +149,19 @@ Mission::on_active()
/* lets check if we reached the current mission item */
if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
advance_mission();
set_mission_items();
if (_mission_item.autocontinue) {
/* switch to next waypoint if 'autocontinue' flag set */
advance_mission();
set_mission_items();
} else {
/* else just report that item reached */
if (_mission_type == MISSION_TYPE_OFFBOARD) {
if (!(_navigator->get_mission_result()->seq_reached == _current_offboard_mission_index && _navigator->get_mission_result()->reached)) {
set_mission_item_reached();
}
}
}
} else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) {
altitude_sp_foh_update();
@ -221,7 +234,7 @@ Mission::update_offboard_mission()
_current_offboard_mission_index = 0;
}
report_current_offboard_mission_item();
set_current_offboard_mission_item();
}
@ -359,6 +372,10 @@ Mission::set_mission_items()
/* no mission available or mission finished, switch to loiter */
if (_mission_type != MISSION_TYPE_NONE) {
mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
/* use last setpoint for loiter */
_navigator->set_can_loiter_at_sp(true);
} else if (!user_feedback_done) {
/* only tell users that we got no mission if there has not been any
* better, more specific feedback yet
@ -375,10 +392,11 @@ Mission::set_mission_items()
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
/* reuse setpoint for LOITER only if it's not IDLE */
_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
reset_mission_item_reached();
report_mission_finished();
set_mission_finished();
_navigator->set_position_setpoint_triplet_updated();
return;
@ -415,7 +433,7 @@ Mission::set_mission_items()
takeoff_alt += _navigator->get_home_position()->alt;
}
/* perform takeoff at least to NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
/* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
if (_navigator->get_vstatus()->condition_landed) {
takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get());
@ -423,32 +441,46 @@ Mission::set_mission_items()
takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get());
}
mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
/* check if we already above takeoff altitude */
if (_navigator->get_global_position()->alt < takeoff_alt - _navigator->get_acceptance_radius()) {
mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude = takeoff_alt;
_mission_item.altitude_is_relative = false;
_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
_mission_item.altitude = takeoff_alt;
_mission_item.altitude_is_relative = false;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0;
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
} else {
/* set current position setpoint from mission item */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
_navigator->set_position_setpoint_triplet_updated();
return;
/* require takeoff after landing or idle */
if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) {
_need_takeoff = true;
} else {
/* skip takeoff */
_takeoff = false;
}
}
_navigator->set_can_loiter_at_sp(false);
reset_mission_item_reached();
/* set current position setpoint from mission item */
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
if (_mission_type == MISSION_TYPE_OFFBOARD) {
report_current_offboard_mission_item();
}
// TODO: report onboard mission item somehow
/* require takeoff after landing or idle */
if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) {
_need_takeoff = true;
}
_navigator->set_can_loiter_at_sp(false);
reset_mission_item_reached();
if (_mission_type == MISSION_TYPE_OFFBOARD) {
set_current_offboard_mission_item();
}
// TODO: report onboard mission item somehow
if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) {
/* try to read next mission item */
struct mission_item_s mission_item_next;
@ -460,6 +492,10 @@ Mission::set_mission_items()
/* next mission item is not available */
pos_sp_triplet->next.valid = false;
}
} else {
/* vehicle will be paused on current waypoint, don't set next item */
pos_sp_triplet->next.valid = false;
}
/* Save the distance between the current sp and the previous one */
@ -666,19 +702,19 @@ Mission::save_offboard_mission_state()
}
void
Mission::report_mission_item_reached()
Mission::set_mission_item_reached()
{
if (_mission_type == MISSION_TYPE_OFFBOARD) {
_navigator->get_mission_result()->reached = true;
_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
}
_navigator->get_mission_result()->reached = true;
_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
_navigator->publish_mission_result();
}
void
Mission::report_current_offboard_mission_item()
Mission::set_current_offboard_mission_item()
{
warnx("current offboard mission index: %d", _current_offboard_mission_index);
_navigator->get_mission_result()->reached = false;
_navigator->get_mission_result()->finished = false;
_navigator->get_mission_result()->seq_current = _current_offboard_mission_index;
_navigator->publish_mission_result();
@ -686,9 +722,8 @@ Mission::report_current_offboard_mission_item()
}
void
Mission::report_mission_finished()
Mission::set_mission_finished()
{
_navigator->get_mission_result()->finished = true;
_navigator->publish_mission_result();
}

17
src/modules/navigator/mission.h
View File

@ -37,6 +37,7 @@
*
* @author Julian Oes <julian@oes.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef NAVIGATOR_MISSION_H
@ -130,19 +131,19 @@ private:
void save_offboard_mission_state();
/**
* Report that a mission item has been reached
* Set a mission item as reached
*/
void report_mission_item_reached();
void set_mission_item_reached();
/**
* Rport the current mission item
* Set the current offboard mission item
*/
void report_current_offboard_mission_item();
void set_current_offboard_mission_item();
/**
* Report that the mission is finished if one exists or that none exists
* Set that the mission is finished if one exists or that none exists
*/
void report_mission_finished();
void set_mission_finished();
control::BlockParamInt _param_onboard_enabled;
control::BlockParamFloat _param_takeoff_alt;
@ -154,8 +155,8 @@ private:
int _current_onboard_mission_index;
int _current_offboard_mission_index;
bool _need_takeoff;
bool _takeoff;
bool _need_takeoff; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */
bool _takeoff; /**< takeoff state flag */
enum {
MISSION_TYPE_NONE,

2
src/modules/navigator/module.mk
View File

@ -63,3 +63,5 @@ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink
MODULE_STACKSIZE = 1200
EXTRACXXFLAGS = -Weffc++
MAXOPTIMIZATION = -Os

24
src/modules/unit_test/unit_test.cpp
View File

@ -36,7 +36,11 @@
#include <systemlib/err.h>
UnitTest::UnitTest()
UnitTest::UnitTest() :
_tests_run(0),
_tests_failed(0),
_tests_passed(0),
_assertions(0)
{
}
@ -44,20 +48,22 @@ UnitTest::~UnitTest()
{
}
void UnitTest::printResults(void)
void UnitTest::print_results(void)
{
warnx(mu_tests_failed() ? "SOME TESTS FAILED" : "ALL TESTS PASSED");
warnx(" Tests passed : %d", mu_tests_passed());
warnx(" Tests failed : %d", mu_tests_failed());
warnx(" Assertions : %d", mu_assertion());
warnx(_tests_failed ? "SOME TESTS FAILED" : "ALL TESTS PASSED");
warnx(" Tests passed : %d", _tests_passed);
warnx(" Tests failed : %d", _tests_failed);
warnx(" Assertions : %d", _assertions);
}
void UnitTest::printAssert(const char* msg, const char* test, const char* file, int line)
/// @brief Used internally to the ut_assert macro to print assert failures.
void UnitTest::_print_assert(const char* msg, const char* test, const char* file, int line)
{
warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line);
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)
/// @brief Used internally to the ut_compare macro to print assert failures.
void UnitTest::_print_compare(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);
}

125
src/modules/unit_test/unit_test.h
View File

@ -37,68 +37,85 @@
#include <systemlib/err.h>
/// @brief Base class to be used for unit tests.
class __EXPORT UnitTest
{
public:
#define INLINE_GLOBAL(type,func) inline type& func() { static type x; return x; }
INLINE_GLOBAL(int, mu_tests_run)
INLINE_GLOBAL(int, mu_tests_failed)
INLINE_GLOBAL(int, mu_tests_passed)
INLINE_GLOBAL(int, mu_assertion)
INLINE_GLOBAL(int, mu_line)
INLINE_GLOBAL(const char*, mu_last_test)
UnitTest();
virtual ~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 { \
if (!(test)) { \
printAssert(message, #test, __FILE__, __LINE__); \
return false; \
} else { \
mu_assertion()++; \
} \
} 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()++; \
} \
/// @brief Override to run your unit tests. Unit tests should be called using ut_run_test macro.
/// @return true: all unit tests succeeded, false: one or more unit tests failed
virtual bool run_tests(void) = 0;
/// @brief Prints results from running of unit tests.
void print_results(void);
/// @brief Macro to create a function which will run a unit test class and print results.
#define ut_declare_test(test_function, test_class) \
bool test_function(void) \
{ \
test_class* test = new test_class(); \
bool success = test->run_tests(); \
test->print_results(); \
return success; \
}
protected:
/// @brief Runs a single unit test. Unit tests must have the function signature of bool test(void). The unit
/// test should return true if it succeeded, false for fail.
#define ut_run_test(test) \
do { \
warnx("RUNNING TEST: %s", #test); \
_tests_run++; \
_init(); \
if (!test()) { \
warnx("TEST FAILED: %s", #test); \
_tests_failed++; \
} else { \
warnx("TEST PASSED: %s", #test); \
_tests_passed++; \
} \
_cleanup(); \
} 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()++; \
} else { \
warnx("TEST PASSED: %s", #test); \
mu_tests_passed()++; \
} \
cleanup(); \
} while (0)
/// @brief Used to assert a value within a unit test.
#define ut_assert(message, test) \
do { \
if (!(test)) { \
_print_assert(message, #test, __FILE__, __LINE__); \
return false; \
} else { \
_assertions++; \
} \
} while (0)
/// @brief Used to compare two integer values within a unit test. If possible use ut_compare instead of ut_assert
/// since it will give you better error reporting of the actual values being compared.
#define ut_compare(message, v1, v2) \
do { \
int _v1 = v1; \
int _v2 = v2; \
if (_v1 != _v2) { \
_print_compare(message, #v1, _v1, #v2, _v2, __FILE__, __LINE__); \
return false; \
} else { \
_assertions++; \
} \
} while (0)
virtual void _init(void) { }; ///< Run before each unit test. Override to provide custom behavior.
virtual void _cleanup(void) { }; ///< Run after each unit test. Override to provide custom behavior.
void _print_assert(const char* msg, const char* test, const char* file, int line);
void _print_compare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line);
int _tests_run; ///< The number of individual unit tests run
int _tests_failed; ///< The number of unit tests which failed
int _tests_passed; ///< The number of unit tests which passed
int _assertions; ///< Total number of assertions tested by all unit tests
};
#endif /* UNIT_TEST_H_ */