@@ -75,13 +75,22 @@ function RotorCraft(; closed_loop = true, addload=true)
7575 ) for i = 1:num_arms
7676 ]
7777
78+ @variables begin
79+ y_alt(t)
80+ y_roll(t)
81+ y_pitch(t)
82+ end
83+
7884 thrusters = [Thruster(name = Symbol("thruster$i")) for i = 1:num_arms]
7985 @named body = Body(m = body_mass, state_priority = 0, I_11=0.01, I_22=0.01, I_33=0.01, air_resistance=1)
8086 @named freemotion = FreeMotion(state=true, isroot=true, quat=false) # We use Euler angles to describe the orientation of the rotorcraft.
8187
8288 connections = [
8389 connect(world.frame_b, freemotion.frame_a)
8490 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]
8594 [connect(body.frame_a, arms[i].frame_a) for i = 1:num_arms]
8695 [connect(arms[i].frame_b, thrusters[i].frame_b) for i = 1:num_arms]
8796 ]
@@ -122,9 +131,9 @@ function RotorCraft(; closed_loop = true, addload=true)
122131 append!(connections, [thrusters[i].u ~ uc[i] for i = 1:num_arms])
123132
124133 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
128137 ])
129138 append!(systems, [Calt; Croll; Cpitch])
130139
0 commit comments