state_machine_helper_test.cpp

Go to the documentation of this file.

/****************************************************************************
 *
 *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
 *   Author: Simon Wilks <sjwilks@gmail.com>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file state_machine_helper_test.cpp
 * System state machine unit test.
 *
 */

#include "state_machine_helper_test.h"

#include "../state_machine_helper.h"
#include <unit_test.h>
#include "../Arming/PreFlightCheck/PreFlightCheck.hpp"

class StateMachineHelperTest : public UnitTest
{
public:
    StateMachineHelperTest() = default;
    ~StateMachineHelperTest() override = default;

    bool run_tests() override;

private:
    bool armingStateTransitionTest();
    bool mainStateTransitionTest();
    bool isSafeTest();
};

bool StateMachineHelperTest::armingStateTransitionTest()
{
    // 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 transition, 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 transition
        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",
            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_INIT,
            { vehicle_status_s::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",
            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_STANDBY,
            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
        },

        {
            "transition: init to standby error",
            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
        },

        {
            "transition: init to reboot",
            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_SHUTDOWN,
            { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
        },

        {
            "transition: standby to init",
            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_INIT,
            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
        },

        {
            "transition: standby to standby error",
            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
        },

        {
            "transition: standby to reboot",
            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_SHUTDOWN,
            { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
        },

        {
            "transition: armed to standby",
            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_STANDBY,
            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
        },

        {
            "transition: standby error to reboot",
            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_SHUTDOWN,
            { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED
        },

        {
            "transition: in air restore to armed",
            { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_ARMED,
            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
        },

        {
            "transition: in air restore to reboot",
            { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_SHUTDOWN,
            { vehicle_status_s::ARMING_STATE_SHUTDOWN, 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",
            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_STANDBY,
            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
        },

        // Safety switch arming tests

        {
            "transition: standby to armed, no safety switch",
            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
            vehicle_status_s::ARMING_STATE_ARMED,
            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED
        },

        {
            "transition: standby to armed, safety switch off",
            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
            vehicle_status_s::ARMING_STATE_ARMED,
            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_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",
            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_ARMED,
            { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
        },

        {
            "no transition: armed to init",
            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_INIT,
            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED
        },

        {
            "no transition: armed to reboot",
            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_SHUTDOWN,
            { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED
        },

        {
            "no transition: standby error to armed",
            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_ARMED,
            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
        },

        {
            "no transition: standby error to standby",
            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_STANDBY,
            { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
        },

        {
            "no transition: reboot to armed",
            { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_ARMED,
            { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
        },

        {
            "no transition: in air restore to standby",
            { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_STANDBY,
            { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED
        },

        // Sensor tests

        //{ "transition to standby error: init to standby - sensors not initialized",
        //    { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
        //    vehicle_status_s::ARMING_STATE_STANDBY,
        //    { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED },

        // Safety switch arming tests

        {
            "no transition: init to armed, safety switch on",
            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
            vehicle_status_s::ARMING_STATE_ARMED,
            { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED
        },
    };

    struct vehicle_status_s status = {};
    struct vehicle_status_flags_s status_flags = {};
    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];

        const bool check_gps = false;

        // Setup initial machine state
        status.arming_state = test->current_state.arming_state;
        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_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,
                         true /* enable pre-arm checks */,
                         nullptr /* no mavlink_log_pub */,
                         &status_flags,
                         (check_gps ? PreFlightCheck::ARM_REQ_GPS_BIT : 0),
                         2e6 /* 2 seconds after boot, everything should be checked */
                                    );

        // Validate result of transition
        ut_compare(test->assertMsg, test->expected_transition_result, result);
        ut_compare(test->assertMsg, status.arming_state, test->expected_state.arming_state);
        ut_compare(test->assertMsg, armed.armed, test->expected_state.armed);
        ut_compare(test->assertMsg, armed.ready_to_arm, test->expected_state.ready_to_arm);
    }

    return true;
}

bool StateMachineHelperTest::mainStateTransitionTest()
{
    // 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: MANUAL to ACRO - not rotary",
            MTT_ALL_NOT_VALID,
            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

        {
            "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 vehicle_status_flags_s current_status_flags = {};

        current_commander_state.main_state = test->from_state;
        current_vehicle_status.vehicle_type = (test->condition_bits & MTT_ROTARY_WING) ?
                              vehicle_status_s::VEHICLE_TYPE_ROTARY_WING : vehicle_status_s::VEHICLE_TYPE_FIXED_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;
        current_status_flags.condition_auto_mission_available = true;

        // Attempt transition
        transition_result_t result = main_state_transition(current_vehicle_status, test->to_state, 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;
}

bool StateMachineHelperTest::isSafeTest()
{
    struct safety_s safety = {};
    struct actuator_armed_s armed = {};

    armed.armed = false;
    armed.lockdown = false;
    safety.safety_switch_available = true;
    safety.safety_off = false;
    ut_compare("is safe: not armed", is_safe(safety, armed), true);

    armed.armed = false;
    armed.lockdown = true;
    safety.safety_switch_available = true;
    safety.safety_off = true;
    ut_compare("is safe: software lockdown", is_safe(safety, armed), true);

    armed.armed = true;
    armed.lockdown = false;
    safety.safety_switch_available = true;
    safety.safety_off = true;
    ut_compare("not safe: safety off", is_safe(safety, armed), false);

    armed.armed = true;
    armed.lockdown = false;
    safety.safety_switch_available = true;
    safety.safety_off = false;
    ut_compare("is safe: safety off", is_safe(safety, armed), true);

    armed.armed = true;
    armed.lockdown = false;
    safety.safety_switch_available = false;
    safety.safety_off = false;
    ut_compare("not safe: no safety switch", is_safe(safety, armed), false);

    return true;
}

bool StateMachineHelperTest::run_tests()
{
    ut_run_test(armingStateTransitionTest);
    ut_run_test(mainStateTransitionTest);
    ut_run_test(isSafeTest);

    return (_tests_failed == 0);
}

ut_declare_test(stateMachineHelperTest, StateMachineHelperTest)