From dec13e7f215caa3ee002fbc3b6400ae82e249b2e Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 16 Mar 2014 21:38:23 -0700 Subject: [PATCH 01/14] Use single set of arm/disarm code Set mode and arm/disarm commands now call a single method to arm disarm, thus removing code duplication. Also calls updated arming_state_transition method such that the arming state logic does not need to be duplicated outside of arming_state_transition. --- src/modules/commander/commander.cpp | 115 ++++++++-------------------- 1 file changed, 34 insertions(+), 81 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d114a2e5ce..e010c233fa 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -137,7 +137,7 @@ enum MAV_MODE_FLAG { }; /* Mavlink file descriptors */ -static int mavlink_fd; +static int mavlink_fd = 0; /* flags */ static bool commander_initialized = false; @@ -217,11 +217,10 @@ void print_reject_arm(const char *msg); void print_status(); -int arm(); -int disarm(); - transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); +transition_result_t armDisarm(bool arm, const int mavlink_fd, const char* armedBy); + /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. */ @@ -288,12 +287,12 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "arm")) { - arm(); + armDisarm(true, mavlink_fd, "command line"); exit(0); } - if (!strcmp(argv[1], "disarm")) { - disarm(); + if (!strcmp(argv[1], "2")) { + armDisarm(false, mavlink_fd, "command line"); exit(0); } @@ -363,30 +362,22 @@ void print_status() static orb_advert_t status_pub; -int arm() +transition_result_t armDisarm(bool arm, const int mavlink_fd, const char* armedBy) { - int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); - - if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); - return 0; - - } else { - return 1; - } -} - -int disarm() -{ - int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); - - if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); - return 0; - - } else { - return 1; - } + transition_result_t arming_res = TRANSITION_NOT_CHANGED; + + // Transition the armed state. By passing mavlink_fd to arming_state_transition it will + // output appropriate error messages if the state cannot transition. + arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd); + if (arming_res == TRANSITION_CHANGED && mavlink_fd) { + char buf[256]; + snprintf(buf, sizeof(buf), "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); + mavlink_log_info(mavlink_fd, buf); + } else if (arming_res == TRANSITION_DENIED) { + tune_negative(true); + } + + return arming_res; } bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) @@ -424,37 +415,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (hil_ret == OK) ret = true; - // TODO remove debug code - //mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode); - /* set arming state */ - arming_res = TRANSITION_NOT_CHANGED; - - if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) { - print_reject_arm("NOT ARMING: Press safety switch first."); - arming_res = TRANSITION_DENIED; - - } else { - arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); - } - - if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by command"); - } - - } else { - if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { - arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_res = arming_state_transition(status, safety, new_arming_state, armed); - - if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); - } - - } else { - arming_res = TRANSITION_NOT_CHANGED; - } - } + // Transition the arming state + arming_res = armDisarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); if (arming_res == TRANSITION_CHANGED) ret = true; @@ -513,26 +475,17 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } case VEHICLE_CMD_COMPONENT_ARM_DISARM: { - transition_result_t arming_res = TRANSITION_NOT_CHANGED; - - if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) { - if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) { - print_reject_arm("NOT ARMING: Press safety switch first."); - arming_res = TRANSITION_DENIED; - - } else { - arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); - } - - if (arming_res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd"); - result = VEHICLE_CMD_RESULT_ACCEPTED; - ret = true; - - } else { - mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } + // Follow exactly what the mavlink spec says for values + if (cmd->param1 != 0.0f && cmd->param1 != 1.0f) { + return VEHICLE_CMD_RESULT_UNSUPPORTED; + } + + transition_result_t arming_res = armDisarm(cmd->param1 == 1.0f, mavlink_fd, "arm/disarm component command"); + if (arming_res == TRANSITION_DENIED) { + mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } else { + result = VEHICLE_CMD_RESULT_ACCEPTED; } } break; From 4b980b508c93f8bb840ababf19e61862dcedbda9 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 16 Mar 2014 21:39:06 -0700 Subject: [PATCH 02/14] arming_state_transition now outputs error messages If mavlink fd is passed in method will output reason for arming failure to mavlink. --- .../commander/state_machine_helper.cpp | 21 ++++++++++++------- src/modules/commander/state_machine_helper.h | 2 +- 2 files changed, 14 insertions(+), 9 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 31955d3e5d..94ffc31d61 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -70,7 +70,7 @@ static bool failsafe_state_changed = true; transition_result_t arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed) + arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd) { /* * Perform an atomic state update @@ -125,13 +125,18 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * case ARMING_STATE_ARMED: /* allow arming from STANDBY and IN-AIR-RESTORE */ - if ((status->arming_state == ARMING_STATE_STANDBY - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) - && (!safety->safety_switch_available || safety->safety_off || status->hil_state == HIL_STATE_ON)) { /* only allow arming if safety is off */ - ret = TRANSITION_CHANGED; - armed->armed = true; - armed->ready_to_arm = true; - } + if ((status->arming_state == ARMING_STATE_STANDBY || status->arming_state == ARMING_STATE_IN_AIR_RESTORE)) { + if (status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) { + // If we need to wait for safety switch then output message, but only if we have fd for mavlink connection + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first."); + } + } else { + ret = TRANSITION_CHANGED; + armed->armed = true; + armed->ready_to_arm = true; + } + } break; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index f04879ff9e..0ddd4f05a2 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -57,7 +57,7 @@ typedef enum { } transition_result_t; transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed); + arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd = 0); bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); From 783a2403969032e8c0119196cb94c092847965d1 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 17 Mar 2014 11:01:15 -0700 Subject: [PATCH 03/14] Function naming and remove unneeded sprintf --- src/modules/commander/commander.cpp | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e010c233fa..1c11e25878 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -219,7 +219,7 @@ void print_status(); transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); -transition_result_t armDisarm(bool arm, const int mavlink_fd, const char* armedBy); +transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy); /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. @@ -287,12 +287,12 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "arm")) { - armDisarm(true, mavlink_fd, "command line"); + arm_disarm(true, mavlink_fd, "command line"); exit(0); } if (!strcmp(argv[1], "2")) { - armDisarm(false, mavlink_fd, "command line"); + arm_disarm(false, mavlink_fd, "command line"); exit(0); } @@ -362,7 +362,7 @@ void print_status() static orb_advert_t status_pub; -transition_result_t armDisarm(bool arm, const int mavlink_fd, const char* armedBy) +transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy) { transition_result_t arming_res = TRANSITION_NOT_CHANGED; @@ -370,9 +370,7 @@ transition_result_t armDisarm(bool arm, const int mavlink_fd, const char* armedB // output appropriate error messages if the state cannot transition. arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd); if (arming_res == TRANSITION_CHANGED && mavlink_fd) { - char buf[256]; - snprintf(buf, sizeof(buf), "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); - mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); } else if (arming_res == TRANSITION_DENIED) { tune_negative(true); } @@ -416,7 +414,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe ret = true; // Transition the arming state - arming_res = armDisarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); + arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); if (arming_res == TRANSITION_CHANGED) ret = true; @@ -480,7 +478,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe return VEHICLE_CMD_RESULT_UNSUPPORTED; } - transition_result_t arming_res = armDisarm(cmd->param1 == 1.0f, mavlink_fd, "arm/disarm component command"); + transition_result_t arming_res = arm_disarm(cmd->param1 == 1.0f, mavlink_fd, "arm/disarm component command"); if (arming_res == TRANSITION_DENIED) { mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; From 521539897e59f495adaae481911e3a4ac847625e Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 26 Mar 2014 14:51:57 -0700 Subject: [PATCH 04/14] Simpler state transition code Also fixed ARMING_STATE_ARMED_ERROR->ARMING_STATE_STANDBY_ERROR transition. --- .../commander/state_machine_helper.cpp | 173 ++++++++---------- src/modules/uORB/topics/vehicle_status.h | 2 + 2 files changed, 75 insertions(+), 100 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 94ffc31d61..8564747faf 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -68,10 +68,44 @@ static bool arming_state_changed = true; static bool main_state_changed = true; static bool failsafe_state_changed = true; +// This array defines the arming state transitions. The rows are the new state, and the columns +// are the current state. Using new state and current state you can index into the array which +// will be true for a valid transition or false for a invalid transition. In some cases even +// though the transition is marked as true additional checks must be made. See arming_state_transition +// code for those checks. +static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = { + // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE + { /* ARMING_STATE_INIT */ true, true, false, false, false, false, false }, + { /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, + { /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, + { /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, + { /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, + { /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, + { /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI +}; + +// You can index into the array with an arming_state_t in order to get it's textual representation +static const char* state_names[ARMING_STATE_MAX] = { + "ARMING_STATE_INIT", + "ARMING_STATE_STANDBY", + "ARMING_STATE_ARMED", + "ARMING_STATE_ARMED_ERROR", + "ARMING_STATE_STANDBY_ERROR", + "ARMING_STATE_REBOOT", + "ARMING_STATE_IN_AIR_RESTORE", +}; + transition_result_t -arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd) +arming_state_transition(struct vehicle_status_s *status, /// current vehicle status + const struct safety_s *safety, /// current safety settings + arming_state_t new_arming_state, /// arming state requested + struct actuator_armed_s *armed, /// current armed status + const int mavlink_fd) /// mavlink fd for error reporting, 0 for none { + // Double check that our static arrays are still valid + ASSERT(ARMING_STATE_INIT == 0); + ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1); + /* * Perform an atomic state update */ @@ -82,121 +116,60 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* only check transition if the new state is actually different from the current one */ if (new_arming_state == status->arming_state) { ret = TRANSITION_NOT_CHANGED; - } else { - /* enforce lockdown in HIL */ if (status->hil_state == HIL_STATE_ON) { armed->lockdown = true; - } else { armed->lockdown = false; } - - switch (new_arming_state) { - case ARMING_STATE_INIT: - - /* allow going back from INIT for calibration */ - if (status->arming_state == ARMING_STATE_STANDBY) { - ret = TRANSITION_CHANGED; - armed->armed = false; - armed->ready_to_arm = false; - } - - break; - - case ARMING_STATE_STANDBY: - - /* allow coming from INIT and disarming from ARMED */ - if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_ARMED - || status->hil_state == HIL_STATE_ON) { - - /* sensors need to be initialized for STANDBY state */ - if (status->condition_system_sensors_initialized) { - ret = TRANSITION_CHANGED; - armed->armed = false; - armed->ready_to_arm = true; - } - } - - break; - - case ARMING_STATE_ARMED: - - /* allow arming from STANDBY and IN-AIR-RESTORE */ - if ((status->arming_state == ARMING_STATE_STANDBY || status->arming_state == ARMING_STATE_IN_AIR_RESTORE)) { + + // Check that we have a valid state transition + bool valid_transition = arming_transitions[new_arming_state][status->arming_state]; + if (valid_transition) { + // We have a good transition. Now perform any secondary validation. + if (new_arming_state == ARMING_STATE_ARMED) { + // In non-HIL, fail transition if we need safety switch press if (status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) { - // If we need to wait for safety switch then output message, but only if we have fd for mavlink connection if (mavlink_fd) { mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first."); } - } else { - ret = TRANSITION_CHANGED; - armed->armed = true; - armed->ready_to_arm = true; + valid_transition = false; } + } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { + new_arming_state = ARMING_STATE_STANDBY_ERROR; } + } + + // HIL can always go to standby + if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) { + valid_transition = true; + } + + /* Sensors need to be initialized for STANDBY state */ + if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { + valid_transition = false; + } - break; - - case ARMING_STATE_ARMED_ERROR: - - /* an armed error happens when ARMED obviously */ - if (status->arming_state == ARMING_STATE_ARMED) { - ret = TRANSITION_CHANGED; - armed->armed = true; - armed->ready_to_arm = false; - } - - break; - - case ARMING_STATE_STANDBY_ERROR: - - /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ - if (status->arming_state == ARMING_STATE_STANDBY - || status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_ARMED_ERROR) { - ret = TRANSITION_CHANGED; - armed->armed = false; - armed->ready_to_arm = false; - } - - break; - - case ARMING_STATE_REBOOT: - - /* an armed error happens when ARMED obviously */ - if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_STANDBY - || status->arming_state == ARMING_STATE_STANDBY_ERROR) { - ret = TRANSITION_CHANGED; - armed->armed = false; - armed->ready_to_arm = false; - } - - break; - - case ARMING_STATE_IN_AIR_RESTORE: - - /* XXX implement */ - break; - - default: - break; - } - - if (ret == TRANSITION_CHANGED) { - status->arming_state = new_arming_state; - arming_state_changed = true; - } - } - + // Finish up the state transition + if (valid_transition) { + armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR; + armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY; + ret = TRANSITION_CHANGED; + status->arming_state = new_arming_state; + arming_state_changed = true; + } + } + /* end of atomic state update */ irqrestore(flags); - if (ret == TRANSITION_DENIED) - warnx("arming transition rejected"); + if (ret == TRANSITION_DENIED) { + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "Invalid arming transition from %s to %s", state_names[status->arming_state], state_names[new_arming_state]); + } + warnx("arming transition rejected"); + } return ret; } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 5d40554fdf..0071d31257 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -69,6 +69,8 @@ typedef enum { MAIN_STATE_MAX } main_state_t; +// If you change the order, add or remove arming_state_t states make sure to update the arrays +// in state_machine_helper.cpp as well. typedef enum { ARMING_STATE_INIT = 0, ARMING_STATE_STANDBY, From d5a7e7c52b619dd8684867bf31671253684378ee Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 27 Mar 2014 13:05:03 -0700 Subject: [PATCH 05/14] Rewrote to provide better feedback Also allows variables as well as static text for assert text in ut_assert --- src/modules/unit_test/unit_test.cpp | 27 +++++------ src/modules/unit_test/unit_test.h | 69 +++++++++++++---------------- 2 files changed, 41 insertions(+), 55 deletions(-) diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp index 64ee544a27..02d1af4818 100644 --- a/src/modules/unit_test/unit_test.cpp +++ b/src/modules/unit_test/unit_test.cpp @@ -32,17 +32,10 @@ * ****************************************************************************/ -/** - * @file unit_test.cpp - * A unit test library. - * - */ - #include "unit_test.h" #include - UnitTest::UnitTest() { } @@ -51,15 +44,15 @@ UnitTest::~UnitTest() { } -void -UnitTest::print_results(const char* result) +void UnitTest::printResults(void) { - if (result != 0) { - warnx("Failed: %s:%d", mu_last_test(), mu_line()); - warnx("%s", result); - } else { - warnx("ALL TESTS PASSED"); - warnx(" Tests run : %d", mu_tests_run()); - warnx(" Assertion : %d", mu_assertion()); - } + 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()); +} + +void UnitTest::printAssert(const char* msg, const char* test, const char* file, int line) +{ + warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line); } diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h index 3020734f4b..32eb8c308c 100644 --- a/src/modules/unit_test/unit_test.h +++ b/src/modules/unit_test/unit_test.h @@ -32,62 +32,55 @@ * ****************************************************************************/ -/** - * @file unit_test.h - * A unit test library based on MinUnit (http://www.jera.com/techinfo/jtns/jtn002.html). - * - */ - #ifndef UNIT_TEST_H_ -#define UNIT_TEST_ +#define UNIT_TEST_H_ #include - class __EXPORT UnitTest { public: -#define xstr(s) str(s) -#define str(s) #s #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) -#define mu_assert(message, test) \ - do \ - { \ - if (!(test)) \ - return __FILE__ ":" xstr(__LINE__) " " message " (" #test ")"; \ - else \ - mu_assertion()++; \ - } while (0) - - -#define mu_run_test(test) \ -do \ -{ \ - const char *message; \ - mu_last_test() = #test; \ - mu_line() = __LINE__; \ - message = test(); \ - mu_tests_run()++; \ - if (message) \ - return message; \ -} while (0) - - -public: UnitTest(); virtual ~UnitTest(); - virtual const char* run_tests() = 0; - virtual void print_results(const char* result); + virtual void runTests(void) = 0; + void printResults(void); + + void printAssert(const char* msg, const char* test, 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_run_test(test) \ + do { \ + warnx("RUNNING TEST: %s", #test); \ + mu_tests_run()++; \ + if (!test()) { \ + warnx("TEST FAILED: %s", #test); \ + mu_tests_failed()++; \ + } else { \ + warnx("TEST PASSED: %s", #test); \ + mu_tests_passed()++; \ + } \ + } while (0) + }; - - #endif /* UNIT_TEST_H_ */ From b9a56fbeb9effad0b30e2085dba4ea92041456be Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 27 Mar 2014 13:05:51 -0700 Subject: [PATCH 06/14] Fixed bug with transition from in air restore to armed Also added better debug output for invalid transitions --- src/modules/commander/state_machine_helper.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 8564747faf..359cc62cd9 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -129,8 +129,10 @@ arming_state_transition(struct vehicle_status_s *status, /// current if (valid_transition) { // We have a good transition. Now perform any secondary validation. if (new_arming_state == ARMING_STATE_ARMED) { - // In non-HIL, fail transition if we need safety switch press - if (status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) { + // Fail transition if we need safety switch press + // Allow if coming from in air restore + // Allow if HIL_STATE_ON + if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) { if (mavlink_fd) { mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first."); } @@ -165,10 +167,11 @@ arming_state_transition(struct vehicle_status_s *status, /// current irqrestore(flags); if (ret == TRANSITION_DENIED) { + static const char* errMsg = "Invalid arming transition from %s to %s"; if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "Invalid arming transition from %s to %s", state_names[status->arming_state], state_names[new_arming_state]); + mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]); } - warnx("arming transition rejected"); + warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]); } return ret; From dcc11b8cabda82ff8616ee2d14db9a53d6d23ac0 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 27 Mar 2014 13:06:09 -0700 Subject: [PATCH 07/14] Update to convert to C++ style --- src/modules/commander/commander_tests/commander_tests.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/commander/commander_tests/commander_tests.cpp b/src/modules/commander/commander_tests/commander_tests.cpp index 6e72cf0d9c..0abb84a82c 100644 --- a/src/modules/commander/commander_tests/commander_tests.cpp +++ b/src/modules/commander/commander_tests/commander_tests.cpp @@ -48,8 +48,7 @@ extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]); int commander_tests_main(int argc, char *argv[]) { - state_machine_helper_test(); - //state_machine_test(); + stateMachineHelperTest(); return 0; } From f97263f5a094e9112ecdf4a5af99ad39d4a5dda4 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 27 Mar 2014 13:07:27 -0700 Subject: [PATCH 08/14] Added comprehensive arming_state_transition unit test Also converted full to C++ style. Also converted to new unit test code. --- .../state_machine_helper_test.cpp | 355 +++++++++++++----- .../state_machine_helper_test.h | 2 +- 2 files changed, 266 insertions(+), 91 deletions(-) diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 40bedd9f31..8a946543d1 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -49,13 +49,12 @@ public: StateMachineHelperTest(); virtual ~StateMachineHelperTest(); - virtual const char* run_tests(); + virtual void runTests(void); private: - const char* arming_state_transition_test(); - const char* arming_state_transition_arm_disarm_test(); - const char* main_state_transition_test(); - const char* is_safe_test(); + bool armingStateTransitionTest(); + bool mainStateTransitionTest(); + bool isSafeTest(); }; StateMachineHelperTest::StateMachineHelperTest() { @@ -64,61 +63,242 @@ StateMachineHelperTest::StateMachineHelperTest() { StateMachineHelperTest::~StateMachineHelperTest() { } -const char* -StateMachineHelperTest::arming_state_transition_test() +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 transtion, 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 transtion + 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", + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_INIT, + { 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", + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY, + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: init to standby error", + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY_ERROR, + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: init to reboot", + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_REBOOT, + { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: standby to init", + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_INIT, + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: standby to standby error", + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY_ERROR, + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: standby to reboot", + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_REBOOT, + { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: armed to standby", + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY, + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: armed to armed error", + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_ARMED_ERROR, + { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: armed error to standby error", + { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY_ERROR, + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: standby error to reboot", + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_REBOOT, + { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: in air restore to armed", + { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_ARMED, + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: in air restore to reboot", + { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_REBOOT, + { 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", + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY, + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + + // Safety switch arming tests + + { "transition: init to standby, no safety switch", + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF, + ARMING_STATE_ARMED, + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: init to standby, safety switch off", + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF, + ARMING_STATE_ARMED, + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + + // standby error + { "transition: armed error to standby error requested standby", + { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY, + { 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", + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_ARMED, + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: standby to armed error", + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_ARMED_ERROR, + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: armed to init", + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_INIT, + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: armed to reboot", + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_REBOOT, + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: armed error to armed", + { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_ARMED, + { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: armed error to reboot", + { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_REBOOT, + { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: standby error to armed", + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_ARMED, + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: standby error to standby", + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY, + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: reboot to armed", + { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_ARMED, + { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: in air restore to standby", + { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY, + { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + + // Sensor tests + + { "no transition: init to standby - sensors not initialized", + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY, + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + + // Safety switch arming tests + + { "no transition: init to standby, safety switch on", + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_ARMED, + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + }; + struct vehicle_status_s status; - struct safety_s safety; - arming_state_t new_arming_state; + struct safety_s safety; struct actuator_armed_s armed; + + size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]); + for (size_t i=0; icurrent_state.arming_state; + status.condition_system_sensors_initialized = test->condition_system_sensors_initialized; + status.hil_state = test->hil_state; + 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, &safety, test->requested_state, &armed); + + // Validate result of transition + ut_assert(test->assertMsg, test->expected_transition_result == result); + ut_assert(test->assertMsg, status.arming_state == test->expected_state.arming_state); + ut_assert(test->assertMsg, armed.armed == test->expected_state.armed); + ut_assert(test->assertMsg, armed.ready_to_arm == test->expected_state.ready_to_arm); + } - // Identical states. - status.arming_state = ARMING_STATE_INIT; - new_arming_state = ARMING_STATE_INIT; - mu_assert("no transition: identical states", - TRANSITION_NOT_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)); - - // INIT to STANDBY. - armed.armed = false; - armed.ready_to_arm = false; - status.arming_state = ARMING_STATE_INIT; - status.condition_system_sensors_initialized = true; - new_arming_state = ARMING_STATE_STANDBY; - mu_assert("transition: init to standby", - TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)); - mu_assert("current state: standby", ARMING_STATE_STANDBY == status.arming_state); - mu_assert("not armed", !armed.armed); - mu_assert("ready to arm", armed.ready_to_arm); - - // INIT to STANDBY, sensors not initialized. - armed.armed = false; - armed.ready_to_arm = false; - status.arming_state = ARMING_STATE_INIT; - status.condition_system_sensors_initialized = false; - new_arming_state = ARMING_STATE_STANDBY; - mu_assert("no transition: sensors not initialized", - TRANSITION_DENIED == arming_state_transition(&status, &safety, new_arming_state, &armed)); - mu_assert("current state: init", ARMING_STATE_INIT == status.arming_state); - mu_assert("not armed", !armed.armed); - mu_assert("not ready to arm", !armed.ready_to_arm); - - return 0; + return true; } -const char* -StateMachineHelperTest::arming_state_transition_arm_disarm_test() -{ - struct vehicle_status_s status; - struct safety_s safety; - arming_state_t new_arming_state; - struct actuator_armed_s armed; - - // TODO(sjwilks): ARM then DISARM. - return 0; -} - -const char* -StateMachineHelperTest::main_state_transition_test() +bool StateMachineHelperTest::mainStateTransitionTest(void) { struct vehicle_status_s current_state; main_state_t new_main_state; @@ -126,70 +306,69 @@ StateMachineHelperTest::main_state_transition_test() // Identical states. current_state.main_state = MAIN_STATE_MANUAL; new_main_state = MAIN_STATE_MANUAL; - mu_assert("no transition: identical states", + ut_assert("no transition: identical states", TRANSITION_NOT_CHANGED == main_state_transition(¤t_state, new_main_state)); - mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); // AUTO to MANUAL. current_state.main_state = MAIN_STATE_AUTO; new_main_state = MAIN_STATE_MANUAL; - mu_assert("transition changed: auto to manual", + ut_assert("transition changed: auto to manual", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - mu_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); + ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); // MANUAL to SEATBELT. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = true; new_main_state = MAIN_STATE_SEATBELT; - mu_assert("tranisition: manual to seatbelt", + ut_assert("tranisition: manual to seatbelt", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - mu_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state); + ut_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state); // MANUAL to SEATBELT, invalid local altitude. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = false; new_main_state = MAIN_STATE_SEATBELT; - mu_assert("no transition: invalid local altitude", + ut_assert("no transition: invalid local altitude", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); - mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); // MANUAL to EASY. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = true; new_main_state = MAIN_STATE_EASY; - mu_assert("transition: manual to easy", + ut_assert("transition: manual to easy", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - mu_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state); + ut_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state); // MANUAL to EASY, invalid local position. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = false; new_main_state = MAIN_STATE_EASY; - mu_assert("no transition: invalid position", + ut_assert("no transition: invalid position", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); - mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); // MANUAL to AUTO. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_global_position_valid = true; new_main_state = MAIN_STATE_AUTO; - mu_assert("transition: manual to auto", + ut_assert("transition: manual to auto", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - mu_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state); + ut_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state); // MANUAL to AUTO, invalid global position. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_global_position_valid = false; new_main_state = MAIN_STATE_AUTO; - mu_assert("no transition: invalid global position", + ut_assert("no transition: invalid global position", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); - mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - return 0; + return true; } -const char* -StateMachineHelperTest::is_safe_test() +bool StateMachineHelperTest::isSafeTest(void) { struct vehicle_status_s current_state; struct safety_s safety; @@ -199,49 +378,45 @@ StateMachineHelperTest::is_safe_test() armed.lockdown = false; safety.safety_switch_available = true; safety.safety_off = false; - mu_assert("is safe: not armed", is_safe(¤t_state, &safety, &armed)); + ut_assert("is safe: not armed", is_safe(¤t_state, &safety, &armed)); armed.armed = false; armed.lockdown = true; safety.safety_switch_available = true; safety.safety_off = true; - mu_assert("is safe: software lockdown", is_safe(¤t_state, &safety, &armed)); + ut_assert("is safe: software lockdown", is_safe(¤t_state, &safety, &armed)); armed.armed = true; armed.lockdown = false; safety.safety_switch_available = true; safety.safety_off = true; - mu_assert("not safe: safety off", !is_safe(¤t_state, &safety, &armed)); + ut_assert("not safe: safety off", !is_safe(¤t_state, &safety, &armed)); armed.armed = true; armed.lockdown = false; safety.safety_switch_available = true; safety.safety_off = false; - mu_assert("is safe: safety off", is_safe(¤t_state, &safety, &armed)); + ut_assert("is safe: safety off", is_safe(¤t_state, &safety, &armed)); armed.armed = true; armed.lockdown = false; safety.safety_switch_available = false; safety.safety_off = false; - mu_assert("not safe: no safety switch", !is_safe(¤t_state, &safety, &armed)); + ut_assert("not safe: no safety switch", !is_safe(¤t_state, &safety, &armed)); - return 0; + return true; } -const char* -StateMachineHelperTest::run_tests() +void StateMachineHelperTest::runTests(void) { - mu_run_test(arming_state_transition_test); - mu_run_test(arming_state_transition_arm_disarm_test); - mu_run_test(main_state_transition_test); - mu_run_test(is_safe_test); - - return 0; + ut_run_test(armingStateTransitionTest); + ut_run_test(mainStateTransitionTest); + ut_run_test(isSafeTest); } -void -state_machine_helper_test() +void stateMachineHelperTest(void) { StateMachineHelperTest* test = new StateMachineHelperTest(); - test->UnitTest::print_results(test->run_tests()); + test->runTests(); + test->printResults(); } diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h index 10a68e6028..bbf66255ea 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.h +++ b/src/modules/commander/commander_tests/state_machine_helper_test.h @@ -39,6 +39,6 @@ #ifndef STATE_MACHINE_HELPER_TEST_H_ #define STATE_MACHINE_HELPER_TEST_ -void state_machine_helper_test(); +void stateMachineHelperTest(void); #endif /* STATE_MACHINE_HELPER_TEST_H_ */ From c1545bd237cdaf9636da55f5b907bdadc39ed130 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 7 Apr 2014 18:56:03 -0700 Subject: [PATCH 09/14] Fix float equality comparison Also restructured incorrect return statement --- src/modules/commander/commander.cpp | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1c11e25878..57faafcfd3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -473,18 +473,19 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } case VEHICLE_CMD_COMPONENT_ARM_DISARM: { - // Follow exactly what the mavlink spec says for values - if (cmd->param1 != 0.0f && cmd->param1 != 1.0f) { - return VEHICLE_CMD_RESULT_UNSUPPORTED; - } - - transition_result_t arming_res = arm_disarm(cmd->param1 == 1.0f, mavlink_fd, "arm/disarm component command"); - if (arming_res == TRANSITION_DENIED) { - mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. + // We use an float epsilon delta to test float equality. + if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) { + mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1); } else { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } + transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command"); + if (arming_res == TRANSITION_DENIED) { + mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } else { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } + } } break; From 3b9bfcc7a20f2bfb0562ee8cf630e8f49cbbdede Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 19 Apr 2014 22:46:06 +0200 Subject: [PATCH 10/14] attitude_estimator_ekf: use degrees for ATT_MAG_DECL parameter to be consistent with documentation --- .../attitude_estimator_ekf/attitude_estimator_ekf_params.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 4154e3db4a..528ef45fa8 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -113,6 +113,7 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->yaw_off, &(p->yaw_off)); param_get(h->mag_decl, &(p->mag_decl)); + p->mag_decl *= M_PI / 180.0f; param_get(h->acc_comp, &(p->acc_comp)); From 7372693fb9145281ea7ad0dd78c57bc780ef6c07 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 19 Apr 2014 22:56:58 +0200 Subject: [PATCH 11/14] attitude_estimator_ekf: added missing include --- .../attitude_estimator_ekf/attitude_estimator_ekf_params.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 528ef45fa8..3ff3d99229 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -40,6 +40,7 @@ */ #include "attitude_estimator_ekf_params.h" +#include /* Extended Kalman Filter covariances */ From 27755806d5577c63867b0d78f0c731ac7f374d48 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Apr 2014 21:30:57 +0200 Subject: [PATCH 12/14] More pedantic warnings --- makefiles/toolchain_gnu-arm-eabi.mk | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index bb729e1032..4ca2443c5e 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -125,7 +125,11 @@ ARCHWARNINGS = -Wall \ -Wlogical-op \ -Wmissing-declarations \ -Wpacked \ - -Wno-unused-parameter + -Wno-unused-parameter \ + -Werror=format-security \ + -Werror=array-bounds \ + -Wfatal-errors \ + -Wformat=1 # -Wcast-qual - generates spurious noreturn attribute warnings, try again later # -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code # -Wcast-align - would help catch bad casts in some cases, but generates too many false positives From 267b78f072b105294e28578d2d8bf28e56ff9fc9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Apr 2014 21:31:30 +0200 Subject: [PATCH 13/14] Fix of errors triggered by more pedantic compile options --- src/drivers/mkblctrl/mkblctrl.cpp | 2 +- .../fw_att_pos_estimator_main.cpp | 6 ++--- src/modules/sensors/sensors.cpp | 22 +++++++++---------- src/systemcmds/config/config.c | 2 +- 4 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 705e98eea4..3cb9abc492 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -1015,7 +1015,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1): /* copy the current output value from the channel */ - *(servo_position_t *)arg = Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue; + *(servo_position_t *)arg = Motor[cmd - PWM_SERVO_GET(0)].RawPwmValue; break; diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 8ea6118022..46bd399f73 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -732,21 +732,21 @@ FixedwingEstimator::task_main() case 1: { const char* str = "NaN in states, resetting"; - warnx(str); + warnx("%s", str); mavlink_log_critical(_mavlink_fd, str); break; } case 2: { const char* str = "stale IMU data, resetting"; - warnx(str); + warnx("%s", str); mavlink_log_critical(_mavlink_fd, str); break; } case 3: { const char* str = "switching dynamic / static state"; - warnx(str); + warnx("%s", str); mavlink_log_critical(_mavlink_fd, str); break; } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 4083b8b4f5..6258a18b1e 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -635,39 +635,39 @@ Sensors::parameters_update() /* channel mapping */ if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_pitch, &(_parameters.rc_map_pitch)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_yaw, &(_parameters.rc_map_yaw)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_throttle, &(_parameters.rc_map_throttle)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } // if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { @@ -737,12 +737,12 @@ Sensors::parameters_update() /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } /* scaling of ADC ticks to battery current */ if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 476015f3e3..e6d4b763be 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -121,7 +121,7 @@ do_device(int argc, char *argv[]) errx(ret,"uORB publications could not be unblocked"); } else { - errx("no valid command: %s", argv[1]); + errx(1, "no valid command: %s", argv[1]); } } From dcf1dbb7f4eeeb1c7654d3e7871551d6c2a63e57 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 26 Apr 2014 11:48:43 +0200 Subject: [PATCH 14/14] warnings: don't spam with warning for missing field initializer --- makefiles/toolchain_gnu-arm-eabi.mk | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index bb729e1032..1826b1e752 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -1,5 +1,5 @@ # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (C) 2012-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 @@ -142,7 +142,8 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \ # C++-specific warnings # -ARCHWARNINGSXX = $(ARCHWARNINGS) +ARCHWARNINGSXX = $(ARCHWARNINGS) \ + -Wno-missing-field-initializers # pull in *just* libm from the toolchain ... this is grody LIBM := $(shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a)