@@ -75,13 +75,22 @@ function RotorCraft(; closed_loop = true, addload=true)
75
75
) for i = 1:num_arms
76
76
]
77
77
78
+ @variables begin
79
+ y_alt(t)
80
+ y_roll(t)
81
+ y_pitch(t)
82
+ end
83
+
78
84
thrusters = [Thruster(name = Symbol("thruster$i")) for i = 1:num_arms]
79
85
@named body = Body(m = body_mass, state_priority = 0, I_11=0.01, I_22=0.01, I_33=0.01, air_resistance=1)
80
86
@named freemotion = FreeMotion(state=true, isroot=true, quat=false) # We use Euler angles to describe the orientation of the rotorcraft.
81
87
82
88
connections = [
83
89
connect(world.frame_b, freemotion.frame_a)
84
90
connect(freemotion.frame_b, body.frame_a)
91
+ y_alt ~ body.r_0[2]
92
+ y_roll ~ freemotion.phi[3]
93
+ y_pitch ~ freemotion.phi[1]
85
94
[connect(body.frame_a, arms[i].frame_a) for i = 1:num_arms]
86
95
[connect(arms[i].frame_b, thrusters[i].frame_b) for i = 1:num_arms]
87
96
]
@@ -122,9 +131,9 @@ function RotorCraft(; closed_loop = true, addload=true)
122
131
append!(connections, [thrusters[i].u ~ uc[i] for i = 1:num_arms])
123
132
124
133
append!(connections, [
125
- Calt.err_input.u ~ -body.r_0[2]
126
- Croll.err_input.u ~ -freemotion.phi[3]
127
- Cpitch.err_input.u ~ -freemotion.phi[1]
134
+ Calt.err_input.u ~ -y_alt
135
+ Croll.err_input.u ~ -y_roll
136
+ Cpitch.err_input.u ~ -y_pitch
128
137
])
129
138
append!(systems, [Calt; Croll; Cpitch])
130
139
0 commit comments