Exemplo n.º 1
0
def test_skywalkerX8_force_x_dir():
    control_input = ControlInput()
    control_input.throttle = 0.0
    t = 0
    state = State()
    state.vx = 20.0

    for j in range(3):
        state.pitch = state.pitch + 0.05
        state.roll = state.roll + 0.05
        state.yaw = state.yaw + 0.05
        x_update = np.zeros(11)
        constants = SkywalkerX8Constants()
        for i in range(0, 11):
            control_input.throttle = i * 0.1
            prop = IcedSkywalkerX8Properties(control_input)
            params = {"prop": prop, "wind": no_wind()}
            update = dynamics_kinetmatics_update(t,
                                                 x=state.state,
                                                 u=control_input.control_input,
                                                 params=params)
            x_update[i] = update[6]

        S_p = constants.propeller_area
        C_p = constants.motor_efficiency_fact
        k_m = constants.motor_constant
        m = constants.mass
        K = 2 * m / (AIR_DENSITY * S_p * C_p * k_m**2)
        for i in range(0, 10):
            throttle_0 = i * 0.1
            throttle_1 = (i + 1) * 0.1
            assert np.allclose(K * (x_update[i + 1] - x_update[i]),
                               throttle_1**2 - throttle_0**2)
Exemplo n.º 2
0
def test_inertial2body_test_cases():
    state1 = State()
    state1.roll = np.pi / 4.0
    vec1 = np.array([0.0, 0.0, 1.0])
    expect1 = np.array([0.0, np.sqrt(2.0) / 2.0, np.sqrt(2.0) / 2.0])

    state2 = State()
    state2.pitch = np.pi / 4.0
    vec2 = np.array([0.0, 0.0, 1.0])
    expect2 = np.array([-np.sqrt(2.0) / 2.0, 0.0, np.sqrt(2.0) / 2.0])

    state3 = State()
    state3.yaw = np.pi / 4.0
    vec3 = np.array([1.0, 0.0, 0.0])
    expect3 = np.array([np.sqrt(2.0) / 2.0, -np.sqrt(2.0) / 2.0, 0.0])
    return [(state1, vec1, expect1), (state2, vec2, expect2),
            (state3, vec3, expect3)]
Exemplo n.º 3
0
def test_kinematics():
    control_input = ControlInput()
    prop = FrictionlessBall(control_input)
    outputs = [
        "x", "y", "z", "roll", "pitch", "yaw", "vx", "vy", "vz", "ang_rate_x",
        "ang_rate_y", "ang_rate_z"
    ]
    system = build_nonlin_sys(prop, no_wind(), outputs)
    t = np.linspace(0.0, 10, 500, endpoint=True)
    state = State()
    state.vx = 20.0
    state.vy = 1
    for i in range(3):
        state.roll = 0
        state.pitch = 0
        state.yaw = 0
        if i == 0:
            state.ang_rate_x = 0.157079633
            state.ang_rate_y = 0
            state.ang_rate_z = 0
        elif i == 1:
            state.ang_rate_x = 0
            state.ang_rate_y = 0.157079633
            state.ang_rate_z = 0
        elif i == 2:
            state.ang_rate_x = 0
            state.ang_rate_y = 0
            state.ang_rate_z = 0.157079633

        T, yout = input_output_response(system, t, U=0, X0=state.state)
        pos = np.array(yout[:3])
        eul_ang = np.array(yout[3:6])

        vel_inertial = np.array(
            [np.zeros(len(t)),
             np.zeros(len(t)),
             np.zeros(len(t))])
        for j in range(len(vel_inertial[0])):
            state.roll = yout[3, j]
            state.pitch = yout[4, j]
            state.yaw = yout[5, j]
            vel = np.array([yout[6, j], yout[7, j], yout[8, j]])
            vel_inertial_elem = body2inertial(vel, state)
            vel_inertial[0, j] = vel_inertial_elem[0]
            vel_inertial[1, j] = vel_inertial_elem[1]
            vel_inertial[2, j] = vel_inertial_elem[2]

        vx_inertial_expect = 20 * np.ones(len(t))
        vy_inertial_expect = 1 * np.ones(len(t))
        vz_inertial_expect = GRAVITY_CONST * t
        x_expect = 20 * t
        y_expect = 1 * t
        z_expect = 0.5 * GRAVITY_CONST * t**2
        roll_expect = state.ang_rate_x * t
        pitch_expect = state.ang_rate_y * t
        yaw_expect = state.ang_rate_z * t
        assert np.allclose(vx_inertial_expect, vel_inertial[0], atol=7e-3)
        assert np.allclose(vy_inertial_expect, vel_inertial[1], atol=5e-3)
        assert np.allclose(vz_inertial_expect, vel_inertial[2], atol=8e-3)
        assert np.allclose(x_expect, pos[0], atol=8e-2)
        assert np.allclose(y_expect, pos[1], atol=5e-2)
        assert np.allclose(z_expect, pos[2], atol=7e-2)
        assert np.allclose(roll_expect, eul_ang[0], atol=1e-3)
        assert np.allclose(pitch_expect, eul_ang[1], atol=1e-3)
        assert np.allclose(yaw_expect, eul_ang[2], atol=1e-3)