-
Notifications
You must be signed in to change notification settings - Fork 1
/
EngineCtrl.m
117 lines (107 loc) · 3.5 KB
/
EngineCtrl.m
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
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
classdef EngineCtrl < EngineAeroDelta
properties (Access = protected)
% New state scalars
bank = false;
hold = false;
back = false;
free = false;
dir = +1;
count = 1;
t0 = 0;
Q0 = zeros(4, 1);
Qir = zeros(4, 1);
Qir0 = zeros(4, 1);
alpha0 = 0;
MRP = zeros(3, 1);
end
properties (Access = protected, Constant)
tolbeta = 0.02;
tolMRP = 0.001;
end
methods (Access = protected)
function Mc = controls(self, t, Ma, sc)
% Common
Wdir = self.Urel / norm(self.Urel);
% Rate
Wref = zeros(3, 1) + self.bank * self.dir * sc.maxW * Wdir;
dWref = zeros(3, 1);
deltaW = self.W - Wref;
% Attitude - reference w.r.t. inertial (by adding body @ t0 w.r.t. inertial @ t0 + reference @ t w.r.t. body @ t0)
PRA = self.dir * sc.maxW * (t - self.t0);
PRV = Wdir;
q0 = cos(PRA/2); q1 = PRV(1) * sin(PRA/2); q2 = PRV(2) * sin(PRA/2); q3 = PRV(3) * sin(PRA/2);
Qref = [q0, -q1, -q2, -q3;
q1, q0, q3, -q2;
q2, -q3, q0, q1;
q3, q2, -q1, q0];
Qri = Qref * self.Q0; % quaternion addition
self.Qir = zeros(4, 1) + self.bank * [Qri(1); -Qri(2:4)] + self.hold * self.Qir; % inertial w.r.t. reference
% Attitude - body w.r.t. reference
q0 = self.Q(1); q1 = self.Q(2); q2 = self.Q(3); q3 = self.Q(4);
Qref = [q0, -q1, -q2, -q3 ;
q1, q0, q3, -q2 ;
q2, -q3, q0, q1 ;
q3, q2, -q1, q0];
Qbr = Qref * self.Qir;
self.MRP = Qbr(2:4) / (1 + Qbr(1)); % Quaternions -> Modified Rodrigues Parameters
% Lyapunov-derived control law
ID = self.free * diag([true; false; true]) + ~self.free * eye(3);
Mext = ID * Ma;
Mc = sc.I * (dWref - cross(self.W, Wref)) + cross(self.W, sc.I * self.W) - ID * diag(sc.hold) * self.MRP - diag(sc.damp) * deltaW - Mext;
% Limit RCS torque to max
% if any(abs(Mc) > sc.maxMc) && self.bank
% Mc = Mc * min(sc.maxMc ./ abs(Mc));
% else
% Mc(abs(Mc) > sc.maxMc) = sign(Mc(abs(Mc) > sc.maxMc)) .* sc.maxMc(abs(Mc) > sc.maxMc);
% end
end
function [val, ter, sgn] = event(self, t, sc, pl)
% Call superclass method
[val, ter, sgn] = event@Engine(self, t, sc, pl);
% Start/stop bank angle rotations
% States:
% bank = while rotating
% hold = while holding attitude
% back = while rotating back
% free = while free-pitching
if ~self.bank && t >= sc.tref(self.count)
% Start new bank angle rotation, remembering initial attitude
self.bank = true;
self.hold = false;
self.free = false;
self.t0 = t;
self.Q0 = self.Q;
self.alpha0 = self.alpha;
self.dir = mod(self.count, 2) * 2 - 1;
if self.dir > 0
self.Qir0 = [self.Q0(1); -self.Q0(2:4)];
self.back = false;
else
self.back = true;
end
self.count = self.count + 1;
elseif self.bank && sign(self.alpha) ~= sign(self.alpha0) && abs(self.beta) < self.tolbeta
% Reached final attitude, now hold to keep beta ~ 0
self.bank = false;
self.hold = true;
if self.back
self.Qir = self.Qir0;
end
elseif self.hold && ~self.back && abs(self.beta) > self.tolbeta
% While holding, attitude has been perturbed - fix it
self.bank = true;
self.hold = false;
self.t0 = t;
self.Q0 = self.Q;
self.dir = -sign(self.alpha) * sign(self.beta);
elseif ~self.free && self.hold && self.back && max(self.MRP) < self.tolMRP
% Reached stable attitude, stop holding pitch
self.free = true;
end
% Append new event outputs
val = [val; self.bank];
ter = [ter; false];
sgn = [sgn; 0];
end
end
end