/// @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;
}
/// @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::kCmdOpenFileRO;
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 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;
}
bool ParameterTest::_assert_parameter_int_value(param_t param, int32_t expected)
{
int32_t value;
int result = param_get(param, &value);
ut_compare("param_get did not return parameter", 0, result);
ut_compare("value for param doesn't match default value", expected, value);
return true;
}
bool ParameterTest::SimpleFind()
{
param_t param = param_find("TEST_2");
ut_assert_true(PARAM_INVALID != param);
int32_t value;
int result = param_get(param, &value);
ut_compare("param_get did not return parameter", 0, result);
ut_compare("value of returned parameter does not match", 4, value);
return true;
}
static int
compareUnits(
const void* const entry1,
const void* const entry2)
{
return ut_compare(((const UnitAndId*)entry1)->unit,
((const UnitAndId*)entry2)->unit);
}
/// @brief Decode and validate the incoming message
bool MavlinkFtpTest::_decode_message(const mavlink_file_transfer_protocol_t *ftp_msg, ///< Incoming FTP message
const MavlinkFTP::PayloadHeader **payload) ///< Payload inside FTP message response
{
//warnx("_decode_message");
// Make sure the targets are correct
ut_compare("Target network non-zero", ftp_msg->target_network, 0);
ut_compare("Target system id mismatch", ftp_msg->target_system, clientSystemId);
ut_compare("Target component id mismatch", ftp_msg->target_component, clientComponentId);
*payload = reinterpret_cast<const MavlinkFTP::PayloadHeader *>(ftp_msg->payload);
// Make sure we have a good sequence number
ut_compare("Sequence number mismatch", (*payload)->seq_number, _expected_seq_number);
_expected_seq_number++;
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; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
const struct _testCase *test = &rgTestCases[i];
payload.opcode = MavlinkFTP::kCmdCreateDirectory;
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;
}
/// @brief Tests for correct behavior of an Ack response.
bool MavlinkFtpTest::_ack_test()
{
MavlinkFTP::PayloadHeader payload;
const MavlinkFTP::PayloadHeader *reply;
payload.opcode = MavlinkFTP::kCmdNone;
bool success = _send_receive_msg(&payload, // FTP payload header
0, // size in bytes of data
nullptr, // Data to start into FTP message payload
&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 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<MavlinkFTP::PayloadHeader *>(ftp_msg->payload);
// Make sure we have a good sequence number
ut_compare("Sequence number mismatch", (*payload)->seqNumber, _lastOutgoingSeqNumber + 1);
// Bump sequence number for next outgoing message
_lastOutgoingSeqNumber++;
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::kCmdOpenFileRO;
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; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
const struct _testCase *test = &rgTestCases[i];
if (test->deleteFile) {
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;
}
/// @brief Tests for correct reponse to a Terminate command on an invalid session.
bool MavlinkFtpTest::_terminate_badsession_test()
{
MavlinkFTP::PayloadHeader payload;
const MavlinkFTP::PayloadHeader *reply;
const char *file = _rgDownloadTestCases[0].file;
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
payload.offset = 0;
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(file) + 1, // size in bytes of data
(uint8_t *)file, // Data to start into FTP message payload
&reply); // Payload inside FTP message response
if (!success) {
return false;
}
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
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
&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 response to an invalid opcpde.
bool MavlinkFtpTest::_bad_opcode_test()
{
MavlinkFTP::PayloadHeader payload;
const MavlinkFTP::PayloadHeader *reply;
payload.opcode = 0xFF; // bogus opcode
bool success = _send_receive_msg(&payload, // FTP payload header
0, // size in bytes of data
nullptr, // Data to start into FTP message payload
&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;
}
bool StateMachineHelperTest::isSafeTest(void)
{
struct vehicle_status_s current_state;
struct safety_s safety;
struct actuator_armed_s armed;
armed.armed = false;
armed.lockdown = false;
safety.safety_switch_available = true;
safety.safety_off = false;
ut_compare("is safe: not armed", is_safe(¤t_state, &safety, &armed), true);
armed.armed = false;
armed.lockdown = true;
safety.safety_switch_available = true;
safety.safety_off = true;
ut_compare("is safe: software lockdown", is_safe(¤t_state, &safety, &armed), true);
armed.armed = true;
armed.lockdown = false;
safety.safety_switch_available = true;
safety.safety_off = true;
ut_compare("not safe: safety off", is_safe(¤t_state, &safety, &armed), false);
armed.armed = true;
armed.lockdown = false;
safety.safety_switch_available = true;
safety.safety_off = false;
ut_compare("is safe: safety off", is_safe(¤t_state, &safety, &armed), true);
armed.armed = true;
armed.lockdown = false;
safety.safety_switch_available = false;
safety.safety_off = false;
ut_compare("not safe: no safety switch", is_safe(¤t_state, &safety, &armed), false);
return true;
}
bool IntrusiveQueueTest::test_push()
{
IntrusiveQueue<testContainer *> q1;
// size should be 0 initially
ut_compare("size initially 0", q1.size(), 0);
ut_assert_true(q1.empty());
// insert 100
for (int i = 0; i < 100; i++) {
testContainer *t = new testContainer();
t->i = i;
ut_compare("size increasing with i", q1.size(), i);
q1.push(t);
ut_compare("size increasing with i", q1.size(), i + 1);
ut_assert_true(!q1.empty());
}
// verify full size (100)
ut_compare("size 100", q1.size(), 100);
// pop all elements
for (int i = 0; i < 100; i++) {
auto node = q1.front();
q1.pop();
delete node;
}
// verify list has been cleared
ut_compare("size 0", q1.size(), 0);
ut_assert_true(q1.empty());
return true;
}
bool MavlinkFtpTest::_removefile_test()
{
MavlinkFTP::PayloadHeader payload;
const MavlinkFTP::PayloadHeader *reply;
int fd;
struct _testCase {
const char *file;
bool success;
};
static const struct _testCase rgTestCases[] = {
{ "/bogus", false },
#ifdef __PX4_NUTTX
// file can actually be deleted on linux
{ _rgDownloadTestCases[0].file, false },
#endif /* __PX4_NUTTX */
{ _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, S_IRWXU | S_IRWXG | S_IRWXO)) != -1);
::close(fd);
for (size_t i = 0; i < sizeof(rgTestCases) / sizeof(rgTestCases[0]); i++) {
const struct _testCase *test = &rgTestCases[i];
payload.opcode = MavlinkFTP::kCmdRemoveFile;
payload.offset = 0;
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(test->file) + 1, // size in bytes of data
(uint8_t *)test->file, // Data to start into FTP message payload
&reply); // Payload inside FTP message response
if (!success) {
return false;
}
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;
}
/*
* Adds an entry to an identifier-to-unit map.
*
* Arguments:
* map The database.
* id The identifier. May be freed upon return.
* unit The unit. May be freed upon return.
* Returns:
* UT_OS Operating-system error. See "errno".
* UT_EXISTS "id" already maps to a different unit.
* UT_SUCCESS Success.
*/
static ut_status
itumAdd(
IdToUnitMap* map,
const char* const id,
const ut_unit* const unit)
{
ut_status status;
UnitAndId* targetEntry;
assert(map != NULL);
assert(id != NULL);
assert(unit != NULL);
targetEntry = uaiNew(unit, id);
if (targetEntry == NULL) {
status = ut_get_status();
}
else {
UnitAndId** treeEntry = tsearch(targetEntry, &map->tree,
map->compare);
if (treeEntry == NULL) {
uaiFree(targetEntry);
status = UT_OS;
}
else {
if (ut_compare((*treeEntry)->unit, unit) == 0) {
status = UT_SUCCESS;
}
else {
status = UT_EXISTS;
ut_set_status(status);
ut_handle_error_message(
"\"%s\" already maps to existing but different unit", id);
}
if (targetEntry != *treeEntry)
uaiFree(targetEntry);
} /* found entry */
} /* "targetEntry" allocated */
return status;
}
/// @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; i<sizeof(_rgReadTestCases)/sizeof(_rgReadTestCases[0]); i++) {
struct stat st;
const ReadTestCase *test = &_rgReadTestCases[i];
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
payload.offset = 0;
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(test->file)+1, // size in bytes of data
(uint8_t*)test->file, // Data to start into FTP message payload
&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;
}
bool IntrusiveQueueTest::test_pop()
{
IntrusiveQueue<testContainer *> q1;
// size should be 0 initially
ut_compare("size initially 0", q1.size(), 0);
ut_assert_true(q1.empty());
// insert 100
for (int i = 0; i < 100; i++) {
testContainer *t = new testContainer();
t->i = i;
q1.push(t);
}
// verify full size (100)
ut_assert_true(q1.size() == 100);
for (int i = 0; i < 100; i++) {
auto node = q1.front();
ut_compare("stored i", i, node->i);
ut_compare("size check", q1.size(), 100 - i);
q1.pop();
ut_compare("size check", q1.size(), 100 - i - 1);
delete node;
ut_compare("size check", q1.size(), 100 - i - 1);
}
// verify list has been cleared
ut_assert_true(q1.empty());
ut_compare("size check", q1.size(), 0);
// pop an empty queue
auto T = q1.pop();
ut_assert_true(T == nullptr);
ut_assert_true(q1.empty());
ut_compare("size check", q1.size(), 0);
return true;
}
/**
* Compare the two specified units. Units can compare equal even if their string representations are not, e.g. consider
* "W" (Watt) and "J/s" (Joule per second).
* \return
* \arg \c <0, \a unit_a is considered less than \a unit_b.
* \arg \c 0, \a unit_a and \a unit_b are considered equal.
* \arg \c >0, \a unit_a is considered greater than \a unit_b.
*/
int harp_unit_compare(const char *unit_a, const char *unit_b)
{
ut_unit *udunit_a;
ut_unit *udunit_b;
int result;
if (parse_unit(unit_a, &udunit_a) != 0)
{
return -1;
}
if (parse_unit(unit_b, &udunit_b) != 0)
{
ut_free(udunit_a);
return -1;
}
result = ut_compare(udunit_a, udunit_b);
ut_free(udunit_b);
ut_free(udunit_a);
return result;
}
/*========================================================================================
* Turns the passed value into a Y/M/D date
*/
int utCalendar2_cal( double val, ut_unit *dataunits, int *year, int *month, int *day, int *hour,
int *minute, double *second, const char *calendar_name )
{
int jdnew, ndinc, ierr, iorig, iround;
double fdays, extra_seconds, tot_extra_seconds;
int ndays;
calcalcs_cal *cal2use;
/* Following vars are saved between invocations and reused if the
* passed units are the same as last time.
*/
static ut_unit *prev_units=NULL;
static cv_converter *conv_user_units_to_days=NULL;
static int y0, mon0, d0, h0, min0, jday0;
static double s0, extra_seconds0;
static char *prev_calendar=NULL;
if( dataunits == NULL ) {
fprintf( stderr, "Error, utCalendar2 passed a NULL units\n" );
return( UT_ENOINIT );
}
if( have_initted == 0 )
initialize( ut_get_system(dataunits) );
/* Get the calendar we will be using, based on the passed name
*/
cal2use = getcal( calendar_name );
if( cal2use == NULL ) {
unknown_cal_emit_warning( calendar_name );
cal2use = getcal( "Standard" );
}
/* See if we are being passed the same units and calendar as last time. If so,
* we can optimize by not recomputing all this junk
*/
if( (prev_units != NULL) && (prev_calendar != NULL)
&& (strcmp(prev_calendar,cal2use->name)==0)
&& (ut_compare( prev_units, dataunits ) == 0)) {
/* Units and calendar are same as used last call */
}
else
{
/* Units/calendar combo are different from previously saved units, must redo calcs */
if( prev_units != NULL )
ut_free( prev_units );
if( prev_calendar != NULL )
free( prev_calendar );
/* Get origin day of the data units */
get_origin( dataunits, &y0, &mon0, &d0, &h0, &min0, &s0 ); /* Note: static vars */
/* Number of seconds into the specified origin day */
extra_seconds0 = h0*3600.0 + min0*60.0 + s0; /* Note: static vars */
/* Convert the origin day to Julian Day number in the specified calendar */
if( (ierr = ccs_date2jday( cal2use, y0, mon0, d0, &jday0 )) != 0 ) {
fprintf( stderr, "Error in utCalendar2: %s\n", ccs_err_str(ierr) );
return( UT_EINVALID );
}
/* Get converter from user-specified units to "days" */
if( conv_user_units_to_days != NULL )
cv_free( conv_user_units_to_days );
conv_user_units_to_days = get_user_to_day_converter( dataunits, y0, mon0, d0, h0, min0, s0 );
/* Save these units so we can reuse our time-consuming
* calculations next time if they are the same units
*/
prev_units = ut_clone( dataunits );
if( ut_compare( prev_units, dataunits ) != 0 ) {
fprintf( stderr, "error, internal error in udunits2 library found in routine utCalendar2: a clone of the user's units does not equal the original units!\n" );
exit(-1);
}
prev_calendar = (char *)malloc( sizeof(char) * (strlen(cal2use->name)+1 ));
strcpy( prev_calendar, cal2use->name );
}
/* Convert user value of offset to floating point days */
fdays = cv_convert_double( conv_user_units_to_days, val );
/* Get integer number of days and seconds past that */
ndays = fdays;
extra_seconds = (fdays - ndays)*86400.0;
/* Get new Julian day */
jdnew = jday0 + ndays;
/* Handle the sub-day part */
tot_extra_seconds = extra_seconds0 + extra_seconds;
ndinc = tot_extra_seconds / 86400.0;
jdnew += ndinc;
tot_extra_seconds -= ndinc*86400.0;
if( tot_extra_seconds < 0.0 ) {
tot_extra_seconds += 86400.0;
jdnew--;
}
/* Convert to a date */
if( (ierr = ccs_jday2date( cal2use, jdnew, year, month, day )) != 0 ) {
fprintf( stderr, "Error in utCalendar2: %s\n", ccs_err_str(ierr) );
return( UT_EINVALID );
}
*hour = tot_extra_seconds / 3600.0;
tot_extra_seconds -= *hour * 3600.0;
*minute = tot_extra_seconds / 60.0;
tot_extra_seconds -= *minute * 60.0;
*second = tot_extra_seconds;
/* Handle the rouding issues */
iorig = *second; /* Integer conversion */
iround = *second + sec_rounding_value;
if( iround > iorig ) {
/* printf( "rounding alg invoked, orig date: %04d-%02d-%02d %02d:%02d:%.20lf\n", *year, *month, *day, *hour, *minute, *second ); */
*second = (double)iround;
if( *second >= 60.0 ) {
*second -= 60.0;
*minute += 1.0;
if( *minute >= 60.0 ) {
*minute -= 60.0;
*hour += 1.0;
if( *hour >= 24.0 ) {
*hour -= 24.0;
if( (ierr = ccs_jday2date( cal2use, jdnew+1, year, month, day )) != 0 ) {
fprintf( stderr, "Error in utCalendar2: %s\n", ccs_err_str(ierr) );
return( UT_EINVALID );
}
}
}
}
/* printf( "after rounding alg here is new date: %04d-%02d-%02d %02d:%02d:%02.20lf\n", *year, *month, *day, *hour, *minute, *second ); */
}
return(0);
}
/*========================================================================================
* Turn the passed Y/M/D date into a value in the user's units
*/
int utInvCalendar2_cal( int year, int month, int day, int hour, int minute, double second,
ut_unit *user_unit, double *value, const char *calendar_name )
{
int jday, ierr, diff_in_days;
double fdiff_in_days, val_days, val_partdays, fdiff_in_partdays, fpartday;
calcalcs_cal *cal2use;
/* Following vars are static and retained between invocations for efficiency */
static ut_unit *prev_units=NULL;
static int y0, mon0, d0, h0, min0, jday0;
static double s0, fpartday0;
static cv_converter *conv_days_to_user_units=NULL;
static char *prev_calendar=NULL;
if( have_initted == 0 )
initialize( ut_get_system(user_unit) );
/* Get the calendar we will be using, based on the passed name
*/
cal2use = getcal( calendar_name );
if( cal2use == NULL ) {
unknown_cal_emit_warning( calendar_name );
cal2use = getcal( "Standard" );
}
if( (prev_units != NULL) && (prev_calendar != NULL)
&& (strcmp(prev_calendar,cal2use->name)==0)
&& (ut_compare( prev_units, user_unit ) == 0)) {
/* Units are same as used last call */
}
else
{
if( prev_units != NULL )
ut_free( prev_units );
if( prev_calendar != NULL )
free( prev_calendar );
if( conv_days_to_user_units != NULL )
cv_free( conv_days_to_user_units );
/* Get origin day of the data units */
get_origin( user_unit, &y0, &mon0, &d0, &h0, &min0, &s0 ); /* Note: static vars */
/* Convert the origin day to Julian Day number in the specified calendar */
if( (ierr = ccs_date2jday( cal2use, y0, mon0, d0, &jday0 )) != 0 ) {
fprintf( stderr, "Error in utCalendar2: %s\n", ccs_err_str(ierr) );
return( UT_EINVALID );
}
/* Get the origin's HMS in fractional (floating point) part of a Julian day */
fpartday0 = (double)h0/24.0 + (double)min0/1440.0 + s0/86400.0;
/* Get converter for turning days into user's units */
conv_days_to_user_units = get_day_to_user_converter( user_unit, y0, mon0, d0, h0, min0, s0 );
/* Save these units so we can reuse our time-consuming
* calculations next time if they are the same units
*/
prev_units = ut_clone( user_unit );
if( ut_compare( prev_units, user_unit ) != 0 ) {
fprintf( stderr, "error, internal error in udunits2 library found in routine utInvCalendar2: a clone of the user's units does not equal the original units!\n" );
exit(-1);
}
prev_calendar = (char *)malloc( sizeof(char) * (strlen(cal2use->name)+1 ));
strcpy( prev_calendar, cal2use->name );
}
/* Turn passed date into a Julian day */
if( (ierr = ccs_date2jday( cal2use, year, month, day, &jday )) != 0 ) {
fprintf( stderr, "Error in utInvCalendar2: %s\n", ccs_err_str(ierr) );
return( UT_EINVALID );
}
/* jday and jday0 can be very large and nearly equal, so we difference
* them first to keep the precision high
*/
diff_in_days = jday - jday0;
fdiff_in_days = (double)diff_in_days;
/* Get the fractional (floating point) part of a Julian day difference
*/
fpartday = (double)hour/24.0 + (double)minute/1440.0 + second/86400.0;
fdiff_in_partdays = fpartday - fpartday0;
/* Convert days and partial days to user's units */
val_days = cv_convert_double( conv_days_to_user_units, fdiff_in_days );
val_partdays = cv_convert_double( conv_days_to_user_units, fdiff_in_partdays );
/* Hopefully this will minimize the roundoff errors */
*value = val_days + val_partdays;
return(0);
}
bool StateMachineHelperTest::armingStateTransitionTest(void)
{
// These are the critical values from vehicle_status_s and actuator_armed_s which must be primed
// to simulate machine state prior to testing an arming state transition. This structure is also
// use to represent the expected machine state after the transition has been requested.
typedef struct {
arming_state_t arming_state; // vehicle_status_s.arming_state
bool armed; // actuator_armed_s.armed
bool ready_to_arm; // actuator_armed_s.ready_to_arm
} ArmingTransitionVolatileState_t;
// This structure represents a test case for arming_state_transition. It contains the machine
// state prior to transition, the requested state to transition to and finally the expected
// machine state after transition.
typedef struct {
const char* assertMsg; // Text to show when test case fails
ArmingTransitionVolatileState_t current_state; // Machine state prior to transition
hil_state_t hil_state; // Current vehicle_status_s.hil_state
bool condition_system_sensors_initialized; // Current vehicle_status_s.condition_system_sensors_initialized
bool safety_switch_available; // Current safety_s.safety_switch_available
bool safety_off; // Current safety_s.safety_off
arming_state_t requested_state; // Requested arming state to transition to
ArmingTransitionVolatileState_t expected_state; // Expected machine state after transition
transition_result_t expected_transition_result; // Expected result from arming_state_transition
} ArmingTransitionTest_t;
// We use these defines so that our test cases are more readable
#define ATT_ARMED true
#define ATT_DISARMED false
#define ATT_READY_TO_ARM true
#define ATT_NOT_READY_TO_ARM false
#define ATT_SENSORS_INITIALIZED true
#define ATT_SENSORS_NOT_INITIALIZED false
#define ATT_SAFETY_AVAILABLE true
#define ATT_SAFETY_NOT_AVAILABLE true
#define ATT_SAFETY_OFF true
#define ATT_SAFETY_ON false
// These are test cases for arming_state_transition
static const ArmingTransitionTest_t rgArmingTransitionTests[] = {
// TRANSITION_NOT_CHANGED tests
{ "no transition: identical states",
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_INIT,
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED },
// TRANSITION_CHANGED tests
// Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s
{ "transition: init to standby",
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: init to standby error",
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: init to reboot",
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: standby to init",
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_INIT,
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: standby to standby error",
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: standby to reboot",
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: armed to standby",
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: armed to armed error",
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED_ERROR,
{ vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: armed error to standby error",
{ vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: standby error to reboot",
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: in air restore to armed",
{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: in air restore to reboot",
{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
// hil on tests, standby error to standby not normally allowed
{ "transition: standby error to standby, hil on",
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
// Safety switch arming tests
{ "transition: standby to armed, no safety switch",
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
{ "transition: standby to armed, safety switch off",
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED },
// standby error
{ "transition: armed error to standby error requested standby",
{ vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED },
// TRANSITION_DENIED tests
// Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s
{ "no transition: init to armed",
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: standby to armed error",
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED_ERROR,
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: armed to init",
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_INIT,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: armed to reboot",
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: armed error to armed",
{ vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: armed error to reboot",
{ vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_REBOOT,
{ vehicle_status_s::ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: standby error to armed",
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: standby error to standby",
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: reboot to armed",
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
{ "no transition: in air restore to standby",
{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_STANDBY,
{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
// Sensor tests
//{ "transition to standby error: init to standby - sensors not initialized",
// { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
// vehicle_status_s::ARMING_STATE_STANDBY,
// { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },
// Safety switch arming tests
{ "no transition: init to standby, safety switch on",
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
vehicle_status_s::ARMING_STATE_ARMED,
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
};
struct vehicle_status_s status = {};
struct status_flags_s status_flags = {};
struct safety_s safety = {};
struct actuator_armed_s armed = {};
struct battery_status_s battery = {};
size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
for (size_t i=0; i<cArmingTransitionTests; i++) {
const ArmingTransitionTest_t* test = &rgArmingTransitionTests[i];
const bool check_gps = false;
// Setup initial machine state
status.arming_state = test->current_state.arming_state;
status_flags.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
status.hil_state = test->hil_state;
// The power status of the test unit is not relevant for the unit test
status_flags.circuit_breaker_engaged_power_check = true;
safety.safety_switch_available = test->safety_switch_available;
safety.safety_off = test->safety_off;
armed.armed = test->current_state.armed;
armed.ready_to_arm = test->current_state.ready_to_arm;
// Attempt transition
transition_result_t result = arming_state_transition(&status, &battery, &safety, test->requested_state, &armed,
false /* no pre-arm checks */,
nullptr /* no mavlink_log_pub */,
&status_flags,
5.0f, check_gps);
// Validate result of transition
ut_compare(test->assertMsg, test->expected_transition_result, result);
ut_compare(test->assertMsg, status.arming_state, test->expected_state.arming_state);
ut_compare(test->assertMsg, armed.armed, test->expected_state.armed);
ut_compare(test->assertMsg, armed.ready_to_arm, test->expected_state.ready_to_arm);
}
return true;
}
bool ParameterTest::exportImport()
{
static constexpr float MAGIC_FLOAT_VAL = 0.314159f;
bool ret = true;
param_t test_params[] = {p0, p1, p2, p3, p4};
// set all params to corresponding param_t value
for (auto p : test_params) {
if (param_type(p) == PARAM_TYPE_INT32) {
const int32_t set_val = p;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
int32_t get_val = 0;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, get_val);
}
if (param_type(p) == PARAM_TYPE_FLOAT) {
const float set_val = (float)p + MAGIC_FLOAT_VAL;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
float get_val = 0.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, (float)p + MAGIC_FLOAT_VAL);
}
}
// save
if (param_save_default() != PX4_OK) {
PX4_ERR("param_save_default failed");
return false;
}
// zero all params and verify, but don't save
for (auto p : test_params) {
if (param_type(p) == PARAM_TYPE_INT32) {
const int32_t set_val = 0;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
int32_t get_val = -1;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", set_val, get_val);
}
if (param_type(p) == PARAM_TYPE_FLOAT) {
const float set_val = 0.0f;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
float get_val = -1.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare_float("value for param doesn't match default value", set_val, get_val, 0.001f);
}
}
// load saved params
if (param_load_default() != PX4_OK) {
PX4_ERR("param_save_default failed");
ret = true;
}
// check every param
for (auto p : test_params) {
if (param_type(p) == PARAM_TYPE_INT32) {
int32_t get_val = 0.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, get_val);
}
if (param_type(p) == PARAM_TYPE_FLOAT) {
float get_val = 0.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare_float("value for param doesn't match default value", p, (float)p + MAGIC_FLOAT_VAL, 0.001f);
}
}
return ret;
}
bool ParameterTest::exportImportAll()
{
static constexpr float MAGIC_FLOAT_VAL = 0.217828f;
// backup current parameters
const char *param_file_name = PX4_STORAGEDIR "/param_backup";
int fd = open(param_file_name, O_WRONLY | O_CREAT, PX4_O_MODE_666);
if (fd < 0) {
PX4_ERR("open '%s' failed (%i)", param_file_name, errno);
return false;
}
int result = param_export(fd, false);
if (result != PX4_OK) {
PX4_ERR("param_export failed");
close(fd);
return false;
}
close(fd);
bool ret = true;
int N = param_count();
// set all params to corresponding param_t value
for (unsigned i = 0; i < N; i++) {
param_t p = param_for_index(i);
if (p == PARAM_INVALID) {
PX4_ERR("param invalid: %d(%d)", p, i);
break;
}
if (param_type(p) == PARAM_TYPE_INT32) {
const int32_t set_val = p;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
int32_t get_val = 0;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, get_val);
}
if (param_type(p) == PARAM_TYPE_FLOAT) {
const float set_val = (float)p + MAGIC_FLOAT_VAL;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param_set_no_notification failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
float get_val = 0.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, (float)p + MAGIC_FLOAT_VAL);
}
}
// save
if (param_save_default() != PX4_OK) {
PX4_ERR("param_save_default failed");
return false;
}
// zero all params and verify, but don't save
for (unsigned i = 0; i < N; i++) {
param_t p = param_for_index(i);
if (param_type(p) == PARAM_TYPE_INT32) {
const int32_t set_val = 0;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param set failed: %d", p);
ut_assert("param_set_no_notification failed", false);
}
int32_t get_val = -1;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", set_val, get_val);
}
if (param_type(p) == PARAM_TYPE_FLOAT) {
float set_val = 0.0f;
if (param_set_no_notification(p, &set_val) != PX4_OK) {
PX4_ERR("param set failed: %d", p);
ut_assert("param_set_no_notification failed", false);
}
float get_val = -1.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", set_val, get_val);
}
}
// load saved params
if (param_load_default() != PX4_OK) {
PX4_ERR("param_save_default failed");
ret = true;
}
// check every param
for (unsigned i = 0; i < N; i++) {
param_t p = param_for_index(i);
if (param_type(p) == PARAM_TYPE_INT32) {
int32_t get_val = 0;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, get_val);
}
if (param_type(p) == PARAM_TYPE_FLOAT) {
float get_val = 0.0f;
if (param_get(p, &get_val) != PX4_OK) {
PX4_ERR("param_get failed for: %d", p);
ut_assert("param_set_no_notification failed", false);
}
ut_compare("value for param doesn't match default value", p, (float)p + MAGIC_FLOAT_VAL);
}
}
param_reset_all();
// restore original params
fd = open(param_file_name, O_RDONLY);
if (fd < 0) {
PX4_ERR("open '%s' failed (%i)", param_file_name, errno);
return false;
}
result = param_import(fd);
close(fd);
if (result < 0) {
PX4_ERR("importing from '%s' failed (%i)", param_file_name, result);
return false;
}
// save
if (param_save_default() != PX4_OK) {
PX4_ERR("param_save_default failed");
return false;
}
return ret;
}
/// @brief Tests for correct reponse to a Read command on an open session.
bool MavlinkFtpTest::_burst_test()
{
MavlinkFTP::PayloadHeader payload;
const MavlinkFTP::PayloadHeader *reply;
BurstInfo burst_info;
for (size_t i = 0; i < sizeof(_rgDownloadTestCases) / sizeof(_rgDownloadTestCases[0]); i++) {
struct stat st;
const DownloadTestCase *test = &_rgDownloadTestCases[i];
// Read in the file so we can compare it to what we get back
ut_compare("stat failed", stat(test->file, &st), 0);
uint8_t *bytes = new uint8_t[st.st_size];
ut_assert("new failed", bytes != nullptr);
int fd = ::open(test->file, O_RDONLY);
ut_assert("open failed", fd != -1);
int bytes_read = ::read(fd, bytes, st.st_size);
ut_compare("read failed", bytes_read, st.st_size);
::close(fd);
// Test case data files are created for specific boundary conditions
ut_compare("Test case data files are out of date", test->length, st.st_size);
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
payload.offset = 0;
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(test->file) + 1, // size in bytes of data
(uint8_t *)test->file, // Data to start into FTP message payload
&reply); // Payload inside FTP message response
if (!success) {
return false;
}
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
// Setup for burst response handler
burst_info.burst_state = burst_state_first_ack;
burst_info.single_packet_file = test->singlePacketRead;
burst_info.file_size = st.st_size;
burst_info.file_bytes = bytes;
burst_info.ftp_test_class = this;
_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message_handler_burst, &burst_info);
// Send the burst command, message response will be handled by _receive_message_handler_stream
payload.opcode = MavlinkFTP::kCmdBurstReadFile;
payload.session = reply->session;
payload.offset = 0;
mavlink_message_t msg;
_setup_ftp_msg(&payload, 0, nullptr, &msg);
_ftp_server->handle_message(&msg);
// First packet is sent using stream mechanism, so we need to force it out ourselves
hrt_abstime t = 0;
_ftp_server->send(t);
ut_compare("Incorrect sequence of messages", burst_info.burst_state, burst_state_complete);
// Put back generic message handler
_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message_handler_generic, this);
// Terminate session
payload.opcode = MavlinkFTP::kCmdTerminateSession;
payload.session = reply->session;
payload.size = 0;
success = _send_receive_msg(&payload, // FTP payload header
0, // size in bytes of data
nullptr, // Data to start into FTP message payload
&reply); // Payload inside FTP message response
if (!success) {
return false;
}
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
ut_compare("Incorrect payload size", reply->size, 0);
}
return true;
}
bool MavlinkFtpTest::_receive_message_handler_burst(const mavlink_file_transfer_protocol_t *ftp_msg,
BurstInfo *burst_info)
{
hrt_abstime t = 0;
const MavlinkFTP::PayloadHeader *reply;
uint32_t full_packet_bytes = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader);
uint32_t expected_bytes;
_decode_message(ftp_msg, &reply);
switch (burst_info->burst_state) {
case burst_state_first_ack:
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
ut_compare("Offset incorrect", reply->offset, 0);
expected_bytes = burst_info->single_packet_file ? burst_info->file_size : full_packet_bytes;
ut_compare("Payload size incorrect", reply->size, expected_bytes);
ut_compare("burst_complete incorrect", reply->burst_complete, 0);
ut_compare("File contents differ", memcmp(reply->data, burst_info->file_bytes, expected_bytes), 0);
// Setup for next expected message
burst_info->burst_state = burst_info->single_packet_file ? burst_state_nak_eof : burst_state_last_ack;
ut_assert("Remaining stream packets missing", _ftp_server->get_size());
_ftp_server->send(t);
break;
case burst_state_last_ack:
ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck);
ut_compare("Offset incorrect", reply->offset, full_packet_bytes);
expected_bytes = burst_info->file_size - full_packet_bytes;
ut_compare("Payload size incorrect", reply->size, expected_bytes);
ut_compare("burst_complete incorrect", reply->burst_complete, 0);
ut_compare("File contents differ", memcmp(reply->data, &burst_info->file_bytes[full_packet_bytes], expected_bytes), 0);
// Setup for next expected message
burst_info->burst_state = burst_state_nak_eof;
ut_assert("Remaining stream packets missing", _ftp_server->get_size());
_ftp_server->send(t);
break;
case burst_state_nak_eof:
// Signal complete
burst_info->burst_state = burst_state_complete;
ut_compare("All packets should have been seent", _ftp_server->get_size(), 0);
break;
}
return true;
}
/// @brief 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; i<sizeof(_rgReadTestCases)/sizeof(_rgReadTestCases[0]); i++) {
struct stat st;
const ReadTestCase *test = &_rgReadTestCases[i];
// Read in the file so we can compare it to what we get back
ut_compare("stat failed", stat(test->file, &st), 0);
uint8_t *bytes = new uint8_t[st.st_size];
ut_assert("new failed", bytes != nullptr);
int fd = ::open(test->file, O_RDONLY);
ut_assert("open failed", fd != -1);
int bytes_read = ::read(fd, bytes, st.st_size);
ut_compare("read failed", bytes_read, st.st_size);
::close(fd);
// Test case data files are created for specific boundary conditions
ut_compare("Test case data files are out of date", test->length, st.st_size);
payload.opcode = MavlinkFTP::kCmdOpenFileRO;
payload.offset = 0;
bool success = _send_receive_msg(&payload, // FTP payload header
strlen(test->file)+1, // size in bytes of data
(uint8_t*)test->file, // Data to start into FTP message payload
&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;
}
bool MavlinkFtpTest::_list_test(void)
{
MavlinkFTP::PayloadHeader payload;
mavlink_file_transfer_protocol_t ftp_msg;
MavlinkFTP::PayloadHeader *reply;
char response1[] = "Dempty_dir|Ftest_238.data\t238|Ftest_239.data\t239|Ftest_240.data\t240";
char response2[] = "Ddev|Detc|Dfs|Dobj";
struct _testCase {
const char *dir; ///< Directory to run List command on
char *response; ///< Expected response entries from List command
int response_count; ///< Number of directories that should be returned
bool success; ///< true: List command should succeed, false: List command should fail
};
struct _testCase rgTestCases[] = {
{ "/bogus", nullptr, 0, false },
{ "/etc/unit_test_data/mavlink_tests", response1, 4, true },
{ "/", response2, 4, true },
};
for (size_t i=0; i<sizeof(rgTestCases)/sizeof(rgTestCases[0]); i++) {
const struct _testCase *test = &rgTestCases[i];
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, strlen(test->response) + 1);
// The return order of directories from the List command is not repeatable. So we can't do a direct comparison
// to a hardcoded return result string.
// Convert null terminators to seperator char so we can use strok to parse returned data
for (uint8_t j=0; j<reply->size-1; j++) {
if (reply->data[j] == 0) {
reply->data[j] = '|';
}
}
// Loop over returned directory entries trying to find then in the response list
char *dir;
int response_count = 0;
dir = strtok((char *)&reply->data[0], "|");
while (dir != nullptr) {
ut_assert("Returned directory not found in expected response", strstr(test->response, dir));
response_count++;
dir = strtok(nullptr, "|");
}
ut_compare("Incorrect number of directory entires returned", test->response_count, response_count);
} 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;
}
