Helio-SolarSail/test_attitude_controller.py at main · SahilKhutey/Helio-SolarSail · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
"""Tests for AttitudeController GNC module."""
import sys
from pathlib import Path
from unittest.mock import Mock
import math

sys.path.insert(0, str(Path(__file__).parent / 'control'))

from gnc.attitude_controller import AttitudeController


def test_initialize_and_validate():
    mod = AttitudeController()
    mod.initialize({'Kp':5.0,'Kd':1.0,'inertia':(5.0,5.0,5.0),'q_desired':(1.0,0.0,0.0,0.0)})
    assert mod.initialized is True
    assert mod.validate().passed is True


def test_compute_torque_and_angular_acceleration():
    mod = AttitudeController()
    # desired is identity, current is small rotation around z
    mod.initialize({'Kp':10.0,'Kd':2.0,'inertia':(2.0,2.0,2.0),'q_desired':(1.0,0.0,0.0,0.0)})
    state = Mock()
    # small rotation about z of 10 degrees
    theta = math.radians(10.0)
    qw = math.cos(theta/2)
    qz = math.sin(theta/2)
    state.attitude_quaternion = (qw, 0.0, 0.0, qz)
    state.angular_velocity = (0.0,0.0,0.0)
    state.position = (0.0,0.0,0.0)
    state.velocity = (0.0,0.0,0.0)

    deriv = mod.compute_derivatives(state, Mock(), 0.0)
    # Expect non-zero torque around z
    td = deriv.thermal_derivatives
    assert 'torque_z' in td
    assert abs(td['torque_z']) > 0.0
    # angular acceleration around z should be non-zero
    assert abs(deriv.angular_acceleration[2]) > 0.0


def test_commanded_thrust_and_sail_angle_in_outputs():
    mod = AttitudeController()
    mod.initialize({'thrust_cmd':(0.0,0.0,1.0),'sail_angle_cmd':0.5})
    state = Mock()
    state.attitude_quaternion = (1.0,0.0,0.0,0.0)
    state.angular_velocity = (0.0,0.0,0.0)
    state.position = (0.0,0.0,0.0)
    state.velocity = (0.0,0.0,0.0)

    _ = mod.compute_derivatives(state, Mock(), 0.0)
    out = mod.compute_outputs(state, Mock(), 0.0)
    assert out['commanded_thrust'] == (0.0,0.0,1.0)
    assert out['sail_angle_update'] == 0.5


def test_does_not_modify_state():
    mod = AttitudeController()
    mod.initialize({})
    state = Mock()
    state.attitude_quaternion = (1.0,0.0,0.0,0.0)
    before = dict(attitude_quaternion=state.attitude_quaternion)
    _ = mod.compute_derivatives(state, Mock(), 0.0)
    after = dict(attitude_quaternion=state.attitude_quaternion)
    assert before == after

if __name__ == '__main__':
    import pytest
    pytest.main([__file__,'-q'])