fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 01:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

restore px4fmu-v2_test

Browse Source
This commit is contained in:
Daniel Agar
2016-05-13 13:01:41 +02:00
committed by Lorenz Meier
parent 848e87ff76
commit 2bc74fd5d9
16 changed files with 818 additions and 369 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+2 -4
View File
@@ -509,9 +509,7 @@ int commander_main(int argc, char *argv[])
warnx("argument %s unsupported.", argv[2]);
}
if (TRANSITION_DENIED == main_state_transition(&status, new_main_state, main_state_prev,
&status_flags, &internal_state)) {
;
if (TRANSITION_DENIED == main_state_transition(&status, new_main_state, main_state_prev, &status_flags, &internal_state)) {
warnx("mode change failed");
}
return 0;
@@ -1260,7 +1258,7 @@ int commander_thread_main(int argc, char *argv[])
// We want to accept RC inputs as default
status_flags.rc_input_blocked = false;
status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT;
internal_state.main_state =commander_state_s::MAIN_STATE_MANUAL;
internal_state.main_state = commander_state_s::MAIN_STATE_MANUAL;
main_state_prev = commander_state_s::MAIN_STATE_MAX;
status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
src/modules/commander/commander_tests/state_machine_helper_test.cpp
+55 -43
View File
@@ -268,9 +268,11 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED },
};
struct vehicle_status_s status;
struct safety_s safety;
struct actuator_armed_s armed;
struct vehicle_status_s status = {};
struct status_flags_s status_flags = {};
struct safety_s safety = {};
struct actuator_armed_s armed = {};
struct battery_status_s battery = {};
size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
for (size_t i=0; i<cArmingTransitionTests; i++) {
@@ -278,17 +280,21 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
// Setup initial machine state
status.arming_state = test->current_state.arming_state;
status.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
status_flags.condition_system_sensors_initialized = test->condition_system_sensors_initialized;
status.hil_state = test->hil_state;
// The power status of the test unit is not relevant for the unit test
status.circuit_breaker_engaged_power_check = true;
status_flags.circuit_breaker_engaged_power_check = true;
safety.safety_switch_available = test->safety_switch_available;
safety.safety_off = test->safety_off;
armed.armed = test->current_state.armed;
armed.ready_to_arm = test->current_state.ready_to_arm;
// Attempt transition
transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, nullptr /* no mavlink_log_pub */);
transition_result_t result = arming_state_transition(&status, &battery, &safety, test->requested_state, &armed,
false /* no pre-arm checks */,
nullptr /* no mavlink_log_pub */,
&status_flags,
5.0f);
// Validate result of transition
ut_assert(test->assertMsg, test->expected_transition_result == result);
@@ -304,10 +310,10 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
{
// This structure represent a single test case for testing Main State transitions.
typedef struct {
const char* assertMsg; // Text to show when test case fails
uint8_t condition_bits; // Bits for various condition_* values
main_state_t from_state; // State prior to transition request
main_state_t to_state; // State to transition to
const char* assertMsg; // Text to show when test case fails
uint8_t condition_bits; // Bits for various condition_* values
uint8_t from_state; // State prior to transition request
main_state_t to_state; // State to transition to
transition_result_t expected_transition_result; // Expected result from main_state_transition call
} MainTransitionTest_t;
@@ -325,99 +331,99 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
{ "no transition: identical states",
MTT_ALL_NOT_VALID,
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED },
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED },
// TRANSITION_CHANGED tests
{ "transition: MANUAL to ACRO",
MTT_ALL_NOT_VALID,
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ACRO, TRANSITION_CHANGED },
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ACRO, TRANSITION_CHANGED },
{ "transition: ACRO to MANUAL",
MTT_ALL_NOT_VALID,
vehicle_status_s::MAIN_STATE_ACRO, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
commander_state_s::MAIN_STATE_ACRO, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
{ "transition: MANUAL to AUTO_MISSION - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
{ "transition: AUTO_MISSION to MANUAL - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
vehicle_status_s::MAIN_STATE_AUTO_MISSION, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
commander_state_s::MAIN_STATE_AUTO_MISSION, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
{ "transition: MANUAL to AUTO_LOITER - global position valid",
MTT_GLOBAL_POS_VALID,
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED },
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED },
{ "transition: AUTO_LOITER to MANUAL - global position valid",
MTT_GLOBAL_POS_VALID,
vehicle_status_s::MAIN_STATE_AUTO_LOITER, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
commander_state_s::MAIN_STATE_AUTO_LOITER, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
{ "transition: MANUAL to AUTO_RTL - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED },
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED },
{ "transition: AUTO_RTL to MANUAL - global position valid, home position valid",
MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
vehicle_status_s::MAIN_STATE_AUTO_RTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
commander_state_s::MAIN_STATE_AUTO_RTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
{ "transition: MANUAL to ALTCTL - not rotary",
MTT_ALL_NOT_VALID,
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
{ "transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid",
MTT_ROTARY_WING | MTT_LOC_ALT_VALID,
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
{ "transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid",
MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID,
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_CHANGED },
{ "transition: ALTCTL to MANUAL - local altitude valid",
MTT_LOC_ALT_VALID,
vehicle_status_s::MAIN_STATE_ALTCTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
commander_state_s::MAIN_STATE_ALTCTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
{ "transition: MANUAL to POSCTL - local position not valid, global position valid",
MTT_GLOBAL_POS_VALID,
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED },
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED },
{ "transition: MANUAL to POSCTL - local position valid, global position not valid",
MTT_LOC_POS_VALID,
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED },
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_CHANGED },
{ "transition: POSCTL to MANUAL - local position valid, global position valid",
MTT_LOC_POS_VALID,
vehicle_status_s::MAIN_STATE_POSCTL, vehicle_status_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
commander_state_s::MAIN_STATE_POSCTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },
// TRANSITION_DENIED tests
{ "no transition: MANUAL to AUTO_MISSION - global position not valid",
MTT_ALL_NOT_VALID,
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED },
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED },
{ "no transition: MANUAL to AUTO_LOITER - global position not valid",
MTT_ALL_NOT_VALID,
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED },
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED },
{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid",
MTT_ALL_NOT_VALID,
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
{ "no transition: MANUAL to AUTO_RTL - global position not valid, home position valid",
MTT_HOME_POS_VALID,
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
{ "no transition: MANUAL to AUTO_RTL - global position valid, home position not valid",
MTT_GLOBAL_POS_VALID,
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },
{ "no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid",
MTT_ROTARY_WING,
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED },
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED },
{ "no transition: MANUAL to POSCTL - local position not valid, global position not valid",
MTT_ALL_NOT_VALID,
vehicle_status_s::MAIN_STATE_MANUAL, vehicle_status_s::MAIN_STATE_POSCTL, TRANSITION_DENIED },
commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_DENIED },
};
size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]);
@@ -425,24 +431,30 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
const MainTransitionTest_t* test = &rgMainTransitionTests[i];
// Setup initial machine state
struct vehicle_status_s current_state;
current_state.main_state = test->from_state;
current_state.is_rotary_wing = test->condition_bits & MTT_ROTARY_WING;
current_state.condition_local_altitude_valid = test->condition_bits & MTT_LOC_ALT_VALID;
current_state.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID;
current_state.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID;
current_state.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID;
struct vehicle_status_s current_vehicle_status = {};
struct commander_state_s current_commander_state = {};
struct status_flags_s current_status_flags = {};
uint8_t main_state_prev = 0;
current_commander_state.main_state = test->from_state;
current_vehicle_status.is_rotary_wing = test->condition_bits & MTT_ROTARY_WING;
current_status_flags.condition_local_altitude_valid = test->condition_bits & MTT_LOC_ALT_VALID;
current_status_flags.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID;
current_status_flags.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID;
current_status_flags.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID;
// Attempt transition
transition_result_t result = main_state_transition(&current_state, test->to_state);
transition_result_t result = main_state_transition(&current_vehicle_status, test->to_state, main_state_prev,
&current_status_flags, &current_commander_state);
// Validate result of transition
ut_assert(test->assertMsg, test->expected_transition_result == result);
if (test->expected_transition_result == result) {
if (test->expected_transition_result == TRANSITION_CHANGED) {
ut_assert(test->assertMsg, test->to_state == current_state.main_state);
ut_assert(test->assertMsg, test->to_state == current_commander_state.main_state);
} else {
ut_assert(test->assertMsg, test->from_state == current_state.main_state);
ut_assert(test->assertMsg, test->from_state == current_commander_state.main_state);
}
}
}