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

Merge pull request #749 from DonLakeFlyer/ArmDisarm 

Arm/Disarm duplication removal and support for Disarm in VEHICLE_CMD_COMPONENT_ARM_DISARM
This commit is contained in:
Lorenz Meier 2014-04-26 12:24:42 +02:00
parent 5bd8e6f6b3 c1545bd237
commit 4a949a9565
9 changed files with 427 additions and 332 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

112
src/modules/commander/commander.cpp
View File

@ -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 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.
*/
@ -288,12 +287,12 @@ int commander_main(int argc, char *argv[])
}
if (!strcmp(argv[1], "arm")) {
arm();
arm_disarm(true, mavlink_fd, "command line");
exit(0);
}
if (!strcmp(argv[1], "disarm")) {
disarm();
if (!strcmp(argv[1], "2")) {
arm_disarm(false, mavlink_fd, "command line");
exit(0);
}
@ -363,30 +362,20 @@ void print_status()
static orb_advert_t status_pub;
int arm()
transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy)
{
int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
return 0;
// 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) {
mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
} else if (arming_res == TRANSITION_DENIED) {
tune_negative(true);
}
} 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;
}
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)
@ -429,37 +418,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 = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
if (arming_res == TRANSITION_CHANGED)
ret = true;
@ -518,27 +478,19 @@ 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: 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 {
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;

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

@ -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;
}

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

@ -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;
// 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));
size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
for (size_t i=0; i<cArmingTransitionTests; i++) {
const ArmingTransitionTest_t* test = &rgArmingTransitionTests[i];
// 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);
// Setup initial machine state
status.arming_state = test->current_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;
// 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);
// Attempt transition
transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed);
return 0;
// 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);
}
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(&current_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(&current_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(&current_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(&current_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(&current_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(&current_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(&current_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(&current_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(&current_state, &safety, &armed));
ut_assert("is safe: not armed", is_safe(&current_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(&current_state, &safety, &armed));
ut_assert("is safe: software lockdown", is_safe(&current_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(&current_state, &safety, &armed));
ut_assert("not safe: safety off", !is_safe(&current_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(&current_state, &safety, &armed));
ut_assert("is safe: safety off", is_safe(&current_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(&current_state, &safety, &armed));
ut_assert("not safe: no safety switch", !is_safe(&current_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();
}

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

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

175
src/modules/commander/state_machine_helper.cpp
View File

@ -69,10 +69,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)
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
*/
@ -83,116 +117,63 @@ 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:
// 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) {
// 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.");
}
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;
}
}
/* 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;
}
// HIL can always go to standby
if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) {
valid_transition = true;
}
break;
/* Sensors need to be initialized for STANDBY state */
if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
valid_transition = false;
}
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)
&& (!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;
}
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) {
static const char* errMsg = "Invalid arming transition from %s to %s";
if (mavlink_fd) {
mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
}
warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
}
return ret;
}

2
src/modules/commander/state_machine_helper.h
View File

@ -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);

2
src/modules/uORB/topics/vehicle_status.h
View File

@ -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,

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

@ -32,17 +32,10 @@
*
****************************************************************************/
/**
* @file unit_test.cpp
* A unit test library.
*
*/
#include "unit_test.h"
#include <systemlib/err.h>
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);
}

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

@ -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 <systemlib/err.h>
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_ */