forked from Archive/PX4-Autopilot
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:
commit
4a949a9565
|
@ -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);
|
||||
|
||||
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) {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
|
||||
} 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)
|
||||
|
@ -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;
|
||||
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
|
|
@ -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; i<cArmingTransitionTests; i++) {
|
||||
const ArmingTransitionTest_t* test = &rgArmingTransitionTests[i];
|
||||
|
||||
// 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;
|
||||
|
||||
// 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();
|
||||
}
|
||||
|
|
|
@ -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_ */
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
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)
|
||||
&& (!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;
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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,
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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_ */
|
||||
|
|
Loading…
Reference in New Issue