Skip to content

Commit 0acc088

Browse files
jgoppertjgoppert
committed
Created velocity_control function for rdd2.
Signed-off-by: James Goppert <james.goppert@gmail.com>
1 parent ccfb3a3 commit 0acc088

File tree

2 files changed

+96
-64
lines changed

2 files changed

+96
-64
lines changed

‎cyecca/models/rdd2.py‎

Lines changed: 88 additions & 59 deletions
Original file line numberDiff line numberDiff line change
@@ -136,71 +136,26 @@ def saturate(x, x_min, x_max):
136136
return ca.if_else(x > x_max, x_max, ca.if_else(x < x_min, x_min, x))
137137

138138

139-
def derive_input_acro():
140-
"""
141-
Acro mode manual input:
142-
143-
Given input, find roll rate and thrust setpoints
144-
"""
145-
146-
# INPUT CONSTANTS
147-
# -------------------------------
148-
thrust_trim = ca.SX.sym("thrust_trim")
149-
thrust_delta = ca.SX.sym("thrust_delta")
139+
def derive_velocity_control():
150140

151-
# INPUT VARIABLES
152-
# -------------------------------
153-
input_aetr = ca.SX.sym("input_aetr", 4)
154-
155-
# CALC
156-
# -------------------------------
157-
w = ca.vertcat(
158-
rollpitch_rate_max * deg2rad * input_aetr[0],
159-
rollpitch_rate_max * deg2rad * input_aetr[1],
160-
yaw_rate_max * deg2rad * input_aetr[3],
161-
)
162-
thrust = input_aetr[2] * thrust_delta + thrust_trim
163-
164-
# FUNCTION
165-
# -------------------------------
166-
f_input_acro = ca.Function(
167-
"input_acro",
168-
[thrust_trim, thrust_delta, input_aetr],
169-
[w, thrust],
170-
[
171-
"thrust_trim",
172-
"thrust_delta",
173-
"input_aetr",
174-
],
175-
["omega", "thrust"],
176-
)
177-
178-
return {"input_acro": f_input_acro}
179-
180-
181-
def derive_input_velocity():
182141
# INPUT VARIABLES
183142
# -------------------------------
184143
dt = ca.SX.sym("dt")
185144
psi_sp = ca.SX.sym("psi_sp")
186-
psi_vel_sp = ca.SX.sym("psi_vel_sp")
187-
psi_acc_sp = ca.SX.sym("psi_acc_sp")
188-
189145
pw_sp = ca.SX.sym("pw_sp", 3)
190146
pw = ca.SX.sym("pw", 3)
191-
192-
input_aetr = ca.SX.sym("input_aetr", 4)
193-
147+
vb = ca.SX.sym("vb", 3)
148+
psi_vel_sp = ca.SX.sym("psi_vel_sp")
194149
reset_position = ca.SX.sym("reset_position")
195150

196-
psi_vel_sp = 60 * deg2rad * input_aetr[3]
151+
# CALC
152+
# -------------------------------
197153
psi_sp1 = psi_sp + psi_vel_sp * dt
198154
psi_sp1 = ca.remainder(psi_sp1, 2 * ca.pi)
199155

200156
cos_yaw = ca.cos(psi_sp1)
201157
sin_yaw = ca.sin(psi_sp1)
202158

203-
vb = ca.vertcat(2 * input_aetr[1], -2 * input_aetr[0], input_aetr[2])
204159
vw_sp = ca.vertcat(
205160
vb[0] * cos_yaw - vb[1] * sin_yaw, vb[0] * sin_yaw + vb[1] * cos_yaw, vb[2]
206161
)
@@ -217,28 +172,75 @@ def derive_input_velocity():
217172
pw_sp1 = pw + e
218173

219174
aw_sp = ca.vertcat(0, 0, 0)
220-
f_input_velocity = ca.Function(
221-
"input_velocity",
175+
176+
# FUNCTION
177+
# -------------------------------
178+
f_velocity_control = ca.Function(
179+
"velocity_control",
222180
[
223181
dt,
224182
psi_sp,
225183
pw_sp,
226184
pw,
227-
input_aetr,
185+
vb,
186+
psi_vel_sp,
228187
reset_position,
229188
],
230-
[psi_sp1, psi_vel_sp, pw_sp1, vw_sp, aw_sp, q_sp],
189+
[psi_sp1, pw_sp1, vw_sp, aw_sp, q_sp],
231190
[
232191
"dt",
233192
"psi_sp",
234193
"pw_sp",
235194
"pw",
236-
"input_aetr",
195+
"vb",
196+
"psi_vel_sp",
237197
"reset_position",
238198
],
239-
["psi_sp1", "psi_vel_sp", "pw_sp1", "vw_sp", "aw_sp", "q_sp"],
199+
["psi_sp1", "pw_sp1", "vw_sp", "aw_sp", "q_sp"],
240200
)
241-
return {"input_velocity": f_input_velocity}
201+
return {"velocity_control": f_velocity_control}
202+
203+
204+
def derive_input_acro():
205+
"""
206+
Acro mode manual input:
207+
208+
Given input, find roll rate and thrust setpoints
209+
"""
210+
211+
# INPUT CONSTANTS
212+
# -------------------------------
213+
thrust_trim = ca.SX.sym("thrust_trim")
214+
thrust_delta = ca.SX.sym("thrust_delta")
215+
216+
# INPUT VARIABLES
217+
# -------------------------------
218+
input_aetr = ca.SX.sym("input_aetr", 4)
219+
220+
# CALC
221+
# -------------------------------
222+
w = ca.vertcat(
223+
rollpitch_rate_max * deg2rad * input_aetr[0],
224+
rollpitch_rate_max * deg2rad * input_aetr[1],
225+
yaw_rate_max * deg2rad * input_aetr[3],
226+
)
227+
thrust = input_aetr[2] * thrust_delta + thrust_trim
228+
229+
# FUNCTION
230+
# -------------------------------
231+
f_input_acro = ca.Function(
232+
"input_acro",
233+
[thrust_trim, thrust_delta, input_aetr],
234+
[w, thrust],
235+
[
236+
"thrust_trim",
237+
"thrust_delta",
238+
"input_aetr",
239+
],
240+
["omega", "thrust"],
241+
)
242+
243+
return {"input_acro": f_input_acro}
242244

243245

244246
def derive_input_auto_level():
@@ -294,6 +296,32 @@ def derive_input_auto_level():
294296
return {"input_auto_level": f_input_auto_level}
295297

296298

299+
def derive_input_velocity():
300+
# INPUT VARIABLES
301+
# -------------------------------
302+
input_aetr = ca.SX.sym("input_aetr", 4)
303+
304+
# CALC
305+
# -------------------------------
306+
psi_vel_sp = 60 * deg2rad * input_aetr[3]
307+
vb = ca.vertcat(2 * input_aetr[1], -2 * input_aetr[0], input_aetr[2])
308+
309+
# FUNCTION
310+
# -------------------------------
311+
f_input_velocity = ca.Function(
312+
"input_velocity",
313+
[
314+
input_aetr,
315+
],
316+
[vb, psi_vel_sp],
317+
[
318+
"input_aetr",
319+
],
320+
["vb", "psi_vel_sp"],
321+
)
322+
return {"input_velocity": f_input_velocity}
323+
324+
297325
def derive_attitude_control():
298326
"""
299327
Attitude control loop
@@ -493,10 +521,10 @@ def derive_common():
493521
vb0 = q.inverse() @ vw0
494522
vw1 = q @ vb1
495523
f_rotate_vector_w_to_b = ca.Function(
496-
"rotate_vector_w_to_b", [q.param, vw0], [vb0], ["q", "vw0"], ["vb0"]
524+
"rotate_vector_w_to_b", [q.param, vw0], [vb0], ["q", "v_w"], ["v_b"]
497525
)
498526
f_rotate_vector_b_to_w = ca.Function(
499-
"rotate_vector_wbto_w", [q.param, vb1], [vw1], ["q", "vb1"], ["vw1"]
527+
"rotate_vector_b_to_w", [q.param, vb1], [vw1], ["q", "v_b"], ["v_w"]
500528
)
501529
return {
502530
"rotate_vector_w_to_b": f_rotate_vector_w_to_b,
@@ -687,6 +715,7 @@ def generate_code(eqs: dict, filename, dest_dir: str, **kwargs):
687715
eqs = {}
688716
eqs.update(derive_attitude_rate_control())
689717
eqs.update(derive_attitude_control())
718+
eqs.update(derive_velocity_control())
690719
eqs.update(derive_position_control())
691720
eqs.update(derive_input_acro())
692721
eqs.update(derive_input_auto_level())

‎scripts/rdd2_sim.py‎

Lines changed: 8 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -82,8 +82,8 @@ def __init__(self, x0=None, p=None):
8282
self.eqs = {}
8383
self.eqs.update(rdd2.derive_attitude_rate_control())
8484
self.eqs.update(rdd2.derive_attitude_control())
85+
self.eqs.update(rdd2.derive_velocity_control())
8586
self.eqs.update(rdd2.derive_position_control())
86-
self.eqs.update(rdd2.derive_input_acro())
8787
self.eqs.update(rdd2.derive_input_auto_level())
8888
self.eqs.update(rdd2.derive_input_velocity())
8989
self.eqs.update(rdd2.derive_strapdown_ins_propagation())
@@ -278,20 +278,23 @@ def update_controller(self):
278278
omega_sp = self.eqs["attitude_control"](k_p_att, self.q, self.q_sp)
279279

280280
elif self.input_mode == "velocity":
281+
282+
vb_sp, self.psi_vel_sp = self.eqs["input_velocity"](self.input_aetr)
283+
281284
reset_position = False
282285
[
283286
self.psi_sp,
284-
self.psi_vel_sp,
285287
self.pw_sp,
286288
self.vw_sp,
287289
self.aw_sp,
288290
self.qc_sp,
289-
] = self.eqs["input_velocity"](
291+
] = self.eqs["velocity_control"](
290292
self.dt,
291293
self.psi_sp,
292294
self.pw_sp,
293295
self.pw,
294-
self.input_aetr,
296+
vb_sp,
297+
self.psi_vel_sp,
295298
reset_position,
296299
)
297300
if self.control_mode == "mellinger":
@@ -426,7 +429,7 @@ def update_controller(self):
426429
# control allocation
427430
# ---------------------------------------------------------------------
428431
self.u, Fp, Fm, Ft, Msat = self.eqs["f_alloc"](F_max, l, CM, CT, thrust, M)
429-
self.get_logger().info("Ct: %s" % self.u)
432+
# self.get_logger().info("Ct: %s" % self.u)
430433
# self.get_logger().info('u: %s' % self.u)
431434

432435
def joy_callback(self, msg: Joy):

0 commit comments

Comments
 (0)