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

	// Bits for condition_bits
	#define MTT_ALL_NOT_VALID		0
	#define MTT_ROTARY_WING			1 << 0
	#define MTT_LOC_ALT_VALID		1 << 1
	#define MTT_LOC_POS_VALID		1 << 2
	#define MTT_HOME_POS_VALID		1 << 3
	#define MTT_GLOBAL_POS_VALID		1 << 4

	static const MainTransitionTest_t rgMainTransitionTests[] = {

		// TRANSITION_NOT_CHANGED tests

		{ "no transition: identical states",
			MTT_ALL_NOT_VALID,
			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED },

		// TRANSITION_CHANGED tests

		{ "transition: MANUAL to ACRO - rotary",
			MTT_ROTARY_WING,
			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ACRO, TRANSITION_CHANGED },

		{ "transition: ACRO to MANUAL",
			MTT_ALL_NOT_VALID,
			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,
			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,
			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,
			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,
			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,
			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,
			commander_state_s::MAIN_STATE_AUTO_RTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },

		{ "transition: MANUAL to ALTCTL - not rotary",
			MTT_LOC_ALT_VALID,
			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,
			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,
			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,
			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,
			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,
			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,
			commander_state_s::MAIN_STATE_POSCTL, commander_state_s::MAIN_STATE_MANUAL, TRANSITION_CHANGED },

		// TRANSITION_DENIED tests

		{ "transition: MANUAL to ACRO - not rotary",
			MTT_ALL_NOT_VALID,
			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ACRO, TRANSITION_DENIED },

		{ "no transition: MANUAL to AUTO_MISSION - global position not valid",
			MTT_ALL_NOT_VALID,
			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,
			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,
			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,
			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,
			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_AUTO_RTL, TRANSITION_DENIED },

		{ "transition: MANUAL to ALTCTL - not rotary",
			MTT_ALL_NOT_VALID,
			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_ALTCTL, TRANSITION_DENIED },

		{ "no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid",
			MTT_ROTARY_WING,
			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,
			commander_state_s::MAIN_STATE_MANUAL, commander_state_s::MAIN_STATE_POSCTL, TRANSITION_DENIED },
	};

	size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]);
	for (size_t i=0; i<cMainTransitionTests; i++) {
		const MainTransitionTest_t* test = &rgMainTransitionTests[i];

		// Setup initial machine state
		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_vehicle_status, test->to_state, main_state_prev,
									&current_status_flags, &current_commander_state);

		// Validate result of transition
		ut_compare(test->assertMsg, test->expected_transition_result, result);
		if (test->expected_transition_result == result) {
			if (test->expected_transition_result == TRANSITION_CHANGED) {
				ut_compare(test->assertMsg, test->to_state, current_commander_state.main_state);
			} else {
				ut_compare(test->assertMsg, test->from_state, current_commander_state.main_state);
			}
		}
	}

	return true;
}
示例#2
0
void set_link_loss_nav_state(vehicle_status_s *status, actuator_armed_s *armed,
			     const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const link_loss_actions_t link_loss_act,
			     uint8_t auto_recovery_nav_state)
{
	// do the best you can according to the action set

	switch (link_loss_act) {
	case link_loss_actions_t::DISABLED:
		// If datalink loss failsafe is disabled then no action must be taken.
		break;

	case link_loss_actions_t::AUTO_RECOVER:
		if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
			status->nav_state = auto_recovery_nav_state;
			return;
		}

	// FALLTHROUGH
	case link_loss_actions_t::AUTO_RTL:
		if (status_flags.condition_global_position_valid && status_flags.condition_home_position_valid) {
			main_state_transition(*status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, internal_state);
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
			return;
		}

	// FALLTHROUGH
	case link_loss_actions_t::AUTO_LOITER:
		if (status_flags.condition_global_position_valid) {
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
			return;
		}

	// FALLTHROUGH
	case link_loss_actions_t::AUTO_LAND:
		if (status_flags.condition_global_position_valid) {
			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
			return;

		} else {
			if (status->is_rotary_wing) {
				if (status_flags.condition_local_position_valid) {
					status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
					return;

				} else if (status_flags.condition_local_altitude_valid) {
					status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
					return;
				}

			} else {
				// TODO: FW position controller doesn't run without condition_global_position_valid
				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
				return;
			}
		}

	// FALLTHROUGH
	case link_loss_actions_t::TERMINATE:
		status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
		armed->force_failsafe = true;
		break;

	case link_loss_actions_t::LOCKDOWN:
		armed->lockdown = true;
		break;
	}
}
const char*
StateMachineHelperTest::main_state_transition_test()
{
	struct vehicle_status_s current_state;
	main_state_t new_main_state;
	
	// Identical states.
	current_state.main_state = MAIN_STATE_MANUAL;
	new_main_state = MAIN_STATE_MANUAL;
	mu_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);	

	// AUTO to MANUAL.
	current_state.main_state = MAIN_STATE_AUTO;
	new_main_state = MAIN_STATE_MANUAL;
	mu_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);

	// 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", 
		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
	mu_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",
		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
	mu_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",
		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
	mu_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",
		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
	mu_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",
		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
	mu_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",
		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
	mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);

	return 0;
}
示例#4
0
void battery_failsafe(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status,
		      const vehicle_status_flags_s &status_flags, commander_state_s *internal_state, const uint8_t battery_warning,
		      const low_battery_action_t low_battery_action)
{
	switch (battery_warning) {
	case battery_status_s::BATTERY_WARNING_NONE:
		break;

	case battery_status_s::BATTERY_WARNING_LOW:
		mavlink_log_critical(mavlink_log_pub, "LOW BATTERY, RETURN ADVISED");
		break;

	case battery_status_s::BATTERY_WARNING_CRITICAL:

		static constexpr char battery_critical[] = "CRITICAL BATTERY";

		switch (low_battery_action) {
		case LOW_BAT_ACTION::WARNING:
			mavlink_log_critical(mavlink_log_pub, "%s, RETURN ADVISED!", battery_critical);
			break;

		case LOW_BAT_ACTION::RETURN:

		// FALLTHROUGH
		case LOW_BAT_ACTION::RETURN_OR_LAND:

			// let us send the critical message even if already in RTL
			if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags,
					internal_state)) {
				mavlink_log_critical(mavlink_log_pub, "%s, RETURNING", battery_critical);

			} else {
				mavlink_log_emergency(mavlink_log_pub, "%s, RETURN FAILED", battery_critical);
			}

			break;

		case LOW_BAT_ACTION::LAND:
			if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags,
					internal_state)) {
				mavlink_log_critical(mavlink_log_pub, "%s, LANDING AT CURRENT POSITION", battery_critical);

			} else {
				mavlink_log_emergency(mavlink_log_pub, "%s, LANDING FAILED", battery_critical);
			}

			break;
		}

		break;

	case battery_status_s::BATTERY_WARNING_EMERGENCY:

		static constexpr char battery_dangerous[] = "DANGEROUS BATTERY LEVEL";

		switch (low_battery_action) {
		case LOW_BAT_ACTION::WARNING:
			mavlink_log_emergency(mavlink_log_pub, "%s, LANDING ADVISED!", battery_dangerous);
			break;

		case LOW_BAT_ACTION::RETURN:
			if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags,
					internal_state)) {
				mavlink_log_emergency(mavlink_log_pub, "%s, RETURNING", battery_dangerous);

			} else {
				mavlink_log_emergency(mavlink_log_pub, "%s, RETURN FAILED", battery_dangerous);
			}

			break;

		case LOW_BAT_ACTION::RETURN_OR_LAND:

		// FALLTHROUGH
		case LOW_BAT_ACTION::LAND:
			if (TRANSITION_DENIED != main_state_transition(status, commander_state_s::MAIN_STATE_AUTO_LAND, status_flags,
					internal_state)) {
				mavlink_log_emergency(mavlink_log_pub, "%s, LANDING IMMEDIATELY", battery_dangerous);

			} else {
				mavlink_log_emergency(mavlink_log_pub, "%s, LANDING FAILED", battery_dangerous);
			}

			break;
		}

		break;

	case battery_status_s::BATTERY_WARNING_FAILED:
		mavlink_log_emergency(mavlink_log_pub, "BATTERY FAILED");
		break;
	}
}
bool StateMachineHelperTest::mainStateTransitionTest(void)
{
	struct vehicle_status_s current_state;
	main_state_t new_main_state;
	
	// Identical states.
	current_state.main_state = MAIN_STATE_MANUAL;
	new_main_state = MAIN_STATE_MANUAL;
	ut_assert("no transition: identical states",
		  TRANSITION_NOT_CHANGED == main_state_transition(&current_state, new_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;
	ut_assert("transition changed: auto to manual",
		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
	ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state);

	// MANUAL to ALTCTRL.
	current_state.main_state = MAIN_STATE_MANUAL;
	current_state.condition_local_altitude_valid = true;
	new_main_state = MAIN_STATE_ALTCTRL;
	ut_assert("tranisition: manual to altctrl",
		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
	ut_assert("new state: altctrl", MAIN_STATE_ALTCTRL == current_state.main_state);

	// MANUAL to ALTCTRL, invalid local altitude.
	current_state.main_state = MAIN_STATE_MANUAL;
	current_state.condition_local_altitude_valid = false;
	new_main_state = MAIN_STATE_ALTCTRL;
	ut_assert("no transition: invalid local altitude",
		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);

	// MANUAL to POSCTRL.
	current_state.main_state = MAIN_STATE_MANUAL;
	current_state.condition_local_position_valid = true;
	new_main_state = MAIN_STATE_POSCTRL;
	ut_assert("transition: manual to posctrl",
		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
	ut_assert("current state: posctrl", MAIN_STATE_POSCTRL == current_state.main_state);

	// MANUAL to POSCTRL, invalid local position.
	current_state.main_state = MAIN_STATE_MANUAL;
	current_state.condition_local_position_valid = false;
	new_main_state = MAIN_STATE_POSCTRL;
	ut_assert("no transition: invalid position",
		  TRANSITION_DENIED == main_state_transition(&current_state, new_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;
	ut_assert("transition: manual to auto",
		  TRANSITION_CHANGED == main_state_transition(&current_state, new_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;
	ut_assert("no transition: invalid global position",
		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);

	return true;
}