Skip to content

Commit ccfb3a3

Browse files
wsribunmawsribunma
authored
Fix bug on fixed wing sim (#24)
* add fixedwing sim and reformat Signed-off-by: wsribunm <worawissribunma@gmail.com> * update fixedwing model Signed-off-by: wsribunm <worawissribunma@gmail.com> * edit param for nvp sim Signed-off-by: wsribunm <worawissribunma@gmail.com> * fix fixedwing sim Signed-off-by: wsribunm <worawissribunma@gmail.com> * reformat Signed-off-by: wsribunm <worawissribunma@gmail.com> * remove orig file Signed-off-by: wsribunm <worawissribunma@gmail.com> * fix 3-channel dynamics Signed-off-by: wsribunm <worawissribunma@gmail.com> * reformat Signed-off-by: wsribunm <worawissribunma@gmail.com> * debug deprecated codes from merge Signed-off-by: wsribunm <worawissribunma@gmail.com> --------- Signed-off-by: wsribunm <worawissribunma@gmail.com>
1 parent 06fee1d commit ccfb3a3

File tree

4 files changed

+22
-115
lines changed

4 files changed

+22
-115
lines changed

‎cyecca/models/fixedwing.py‎

Lines changed: 1 addition & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -32,9 +32,6 @@ def derive_model(coeff_data):
3232
Jy = ca.SX.sym("Jy") # Moment of Inertia in y direction
3333
Jz = ca.SX.sym("Jz") # Moment of Inertia in z direction
3434
J = ca.diag(ca.vertcat(Jx, Jy, Jz)) # Moment of Inertia Array
35-
Cm_p = ca.SX.sym("Cm_p") # moment coefficient for roll
36-
Cm_q = ca.SX.sym("Cm_q") # moment coefficient for pitch
37-
Cm_r = ca.SX.sym("Cm_r") # moment coefficent for yaw
3835
cbar = ca.SX.sym("cbar") # mean chord (m)
3936
span = ca.SX.sym("span") # wing span (m)
4037

@@ -78,9 +75,6 @@ def derive_model(coeff_data):
7875
Jx,
7976
Jy,
8077
Jz,
81-
Cm_p,
82-
Cm_q,
83-
Cm_r,
8478
cbar,
8579
span,
8680
Cm0,
@@ -163,7 +157,7 @@ def derive_model(coeff_data):
163157
"velocity_b_1": 0.0,
164158
"velocity_b_2": 0.0,
165159
"quat_wb_0": 0.0,
166-
"quat_wb_1": 0.0,
160+
"quat_wb_1": 0.09,
167161
"quat_wb_2": 0.0,
168162
"quat_wb_3": 1.0,
169163
"omega_wb_b_0": 0.0,
@@ -173,7 +167,6 @@ def derive_model(coeff_data):
173167

174168
# input
175169
throttle_cmd = ca.SX.sym("throttle_cmd")
176-
ail_cmd = ca.SX.sym("ail_cmd")
177170
elev_cmd = ca.SX.sym("elev_cmd")
178171
rud_cmd = ca.SX.sym("rud_cmd")
179172

@@ -230,7 +223,6 @@ def derive_model(coeff_data):
230223
##############################################################################################
231224

232225
# Force and Moment Model
233-
cl = cl0 + cla * (-1 * alpha) # Lift Coefficient
234226

235227
# ##############################################################################################
236228
# ## From Look Up table

‎cyecca/models/fixedwing.py.orig‎

Lines changed: 10 additions & 65 deletions
Original file line numberDiff line numberDiff line change
@@ -32,26 +32,20 @@ def derive_model(coeff_data):
3232
Jy = ca.SX.sym("Jy") # Moment of Inertia in y direction
3333
Jz = ca.SX.sym("Jz") # Moment of Inertia in z direction
3434
J = ca.diag(ca.vertcat(Jx, Jy, Jz)) # Moment of Inertia Array
35+
Cm_p = ca.SX.sym("Cm_p") # moment coefficient for roll
36+
Cm_q = ca.SX.sym("Cm_q") # moment coefficient for pitch
37+
Cm_r = ca.SX.sym("Cm_r") # moment coefficent for yaw
3538
cbar = ca.SX.sym("cbar") # mean chord (m)
3639
span = ca.SX.sym("span") # wing span (m)
3740

3841
# Control Moment
3942
Cm0 = ca.SX.sym("Cm0") # Coefficient of Moment
40-
<<<<<<< HEAD
4143
# Clda = ca.SX.sym("Clda") # roll moment coefficient based on aileron
4244
Cldr = ca.SX.sym("Cldr") # roll moment coefficient based on rudder
4345
Cmde = ca.SX.sym("Cmde") # pitch moment coefficient based on elevator
4446
Cndr = ca.SX.sym("Cndr") # yaw moment coefficient based on rudder
4547
# Cnda = ca.SX.sym("Cnda") # yaw moment coefficient based on aileron
4648
# CYda = ca.SX.sym("CYda") # Sideforce due to aileron
47-
=======
48-
Clda = ca.SX.sym("Clda") # roll moment coefficient based on aileron
49-
Cldr = ca.SX.sym("Cldr") # roll moment coefficient based on rudder
50-
Cmde = ca.SX.sym("Cmde") # pitch moment coefficient based on elevator
51-
Cndr = ca.SX.sym("Cndr") # yaw moment coefficient based on rudder
52-
Cnda = ca.SX.sym("Cnda") # yaw moment coefficient based on aileron
53-
CYda = ca.SX.sym("CYda") # Sideforce due to aileron
54-
>>>>>>> cogni/main
5549
CYdr = ca.SX.sym("CYdr") # Sideforce due to rudder
5650

5751
# Longitudinal Stability Coefficients
@@ -84,21 +78,15 @@ def derive_model(coeff_data):
8478
Jx,
8579
Jy,
8680
Jz,
81+
Cm_p,
82+
Cm_q,
83+
Cm_r,
8784
cbar,
8885
span,
8986
Cm0,
90-
<<<<<<< HEAD
91-
Cldr,
92-
Cmde,
93-
Cndr,
94-
=======
95-
Clda,
9687
Cldr,
9788
Cmde,
9889
Cndr,
99-
Cnda,
100-
CYda,
101-
>>>>>>> cogni/main
10290
CYdr,
10391
CL0,
10492
CLa,
@@ -131,18 +119,9 @@ def derive_model(coeff_data):
131119
"span": 0.34, # Wingspan (m)
132120
# Control Effectiveness (Converted to radians)
133121
"Cm0": 0.01, # Zero-lift pitching moment coefficient
134-
<<<<<<< HEAD
135122
"Cldr": 0.15, # Rudder Control effectiveness in roll
136123
"Cmde": 0.25, # Elevator control effectiveness in pitch(per rad)
137124
"Cndr": 0.10, # Rudder control effectiveness in yaw (per rad)
138-
=======
139-
"Clda": 0.00, # 0.026, # Aileron control effectiveness in roll(per rad)
140-
"Cldr": 0.05, # Rudder Control effectiveness in roll
141-
"Cmde": 0.16, # Elevator control effectiveness in pitch(per rad)
142-
"Cndr": 0.20, # Rudder control effectiveness in yaw (per rad)
143-
"Cnda": 0.00, # Aileron control effectiveness in yaw (per rad)
144-
"CYda": 0.02, # Sideforce due to aileron defelction (per rad)
145-
>>>>>>> cogni/main
146125
"CYdr": -0.08, # Side force due to rudder deflection (per rad)
147126
# Longitudinal Stability
148127
"CL0": 0.6, # Adjusted lift coefficient at zero AoA
@@ -183,12 +162,12 @@ def derive_model(coeff_data):
183162
"velocity_b_0": 0.0,
184163
"velocity_b_1": 0.0,
185164
"velocity_b_2": 0.0,
186-
<<<<<<< HEAD
187165
"quat_wb_0": 0.0,
166+
<<<<<<< HEAD
167+
"quat_wb_1": 0.09,
188168
=======
189-
"quat_wb_0": 1.0,
190-
>>>>>>> cogni/main
191169
"quat_wb_1": 0.0,
170+
>>>>>>> cogni/main
192171
"quat_wb_2": 0.0,
193172
"quat_wb_3": 1.0,
194173
"omega_wb_b_0": 0.0,
@@ -249,19 +228,12 @@ def derive_model(coeff_data):
249228
# Control Surface Defelction
250229
max_defl = 30 # maximum control surface deflection in deg
251230
max_defl_elev = 24
252-
<<<<<<< HEAD
253231
elev_rad = max_defl_elev * DEG2RAD * u[1]
254232
rud_rad = max_defl * DEG2RAD * u[2]
255-
=======
256-
ail_rad = max_defl * DEG2RAD * u[1]
257-
elev_rad = max_defl_elev * DEG2RAD * u[2] #
258-
rud_rad = (
259-
max_defl * DEG2RAD * u[3]
260-
) # (u[3] is "rudder stick") (-1 * u[1] is "aileron stick")4
261-
>>>>>>> cogni/main
262233
##############################################################################################
263234

264235
# Force and Moment Model
236+
cl = cl0 + cla * (-1 * alpha) # Lift Coefficient
265237

266238
# ##############################################################################################
267239
# ## From Look Up table
@@ -283,15 +255,8 @@ def derive_model(coeff_data):
283255
CD = CD0 + CDCLS * CL * CL # Drag Polar
284256

285257
# (Steven pg 91 eqn 2.3-17a) and (Steven Pg 79 eqn 2.3-8b)
286-
<<<<<<< HEAD
287258
CC = -CYb * beta + CYdr * rud_rad / (
288259
max_defl * DEG2RAD
289-
=======
290-
CC = (
291-
-CYb * beta
292-
+ CYda * ail_rad / (max_defl * DEG2RAD)
293-
+ CYdr * rud_rad / (max_defl * DEG2RAD)
294-
>>>>>>> cogni/main
295260
) # Crosswind Force Coefficient
296261
CC += (
297262
CYp * span / (2 * V_b) * P
@@ -301,19 +266,11 @@ def derive_model(coeff_data):
301266
) # Sideforce due to yaw rate #(Steven pg 91 eqn 2.3-17a)
302267

303268
### Using Equation to calculate rotational moment coefficent
304-
<<<<<<< HEAD
305269
Cl = (-1) * Cldr * rud_rad # roll moment coefficient
306270
Cm = Cm0 + Cma * alpha + Cmde * elev_rad # pitch moment coefficient
307271
Cn = Cnb * beta + Cndr * rud_rad # yaw moment coefficient
308272
### Using Lookup table to obtain rotational moment coefficent
309273
# Cl = cl
310-
=======
311-
Cl = Clda * ail_rad + (-1) * Cldr * rud_rad # roll moment coefficient
312-
Cm = Cm0 + Cma * alpha + Cmde * elev_rad # pitch moment coefficient
313-
Cn = Cnb * beta + Cndr * rud_rad + (-1) * Cnda * ail_rad # yaw moment coefficient
314-
### Using Lookup table to obtain rotational moment coefficent
315-
# Cl = cl + Cmda * ail_rad/(max_defl*DEG2RAD)
316-
>>>>>>> cogni/main
317274
# Cm = cm
318275
# Cn = cn + Cndr * rud_rad/(max_defl*DEG2RAD)
319276

@@ -339,11 +296,7 @@ def derive_model(coeff_data):
339296
force_w = ca.if_else(
340297
pos_wheel_w[2] < 0.0,
341298
saturate(-(pos_wheel_w[2]) * 10, -100, 100) * zAxis
342-
<<<<<<< HEAD
343299
- vel_wheel_w[2] * 01.10 * zAxis # Vertical Component Damping
344-
=======
345-
- vel_wheel_w[2] * 0.10 * zAxis # Vertical Component Damping
346-
>>>>>>> cogni/main
347300
- vel_wheel_w[0] * 0.001 * xAxis # ground damping
348301
- vel_wheel_w[1] * 0.001 * yAxis, # ground damping
349302
ca.vertcat(0, 0, 0), # Airborne (no ground force effect)
@@ -442,10 +395,6 @@ def derive_model(coeff_data):
442395
qbar,
443396
beta,
444397
elev_rad,
445-
<<<<<<< HEAD
446-
=======
447-
ail_rad,
448-
>>>>>>> cogni/main
449398
rud_rad,
450399
Cndr,
451400
],
@@ -464,10 +413,6 @@ def derive_model(coeff_data):
464413
"qbar",
465414
"beta",
466415
"elev",
467-
<<<<<<< HEAD
468-
=======
469-
"ail",
470-
>>>>>>> cogni/main
471416
"rud",
472417
"Cndr",
473418
],

‎scripts/fixedwing_sim.py‎

Lines changed: 1 addition & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -60,7 +60,7 @@ def __init__(self, x0=None, p=None):
6060
# -------------------------------------------------------
6161
# mode handling
6262
# ----------------------------------------------
63-
self.input_mode = "manual"
63+
self.input_mode = "auto"
6464

6565
# -------------------------------------------------------
6666
# Lookup Table
@@ -105,7 +105,6 @@ def __init__(self, x0=None, p=None):
105105
dtype=float,
106106
)
107107
self.u = np.zeros(4, dtype=float)
108-
109108
# start main loop on timer
110109
self.system_clock = rclpy.clock.Clock(
111110
clock_type=rclpy.clock.ClockType.SYSTEM_TIME
@@ -172,7 +171,6 @@ def update_controller(self):
172171
if self.input_mode == "manual": # logitech f310
173172
self.u = ca.vertcat( # TER mode 3-Channel
174173
float(self.input_aetr[2]),
175-
float(self.input_aetr[0]),
176174
float(self.input_aetr[1]),
177175
-1 * float(self.input_aetr[0]),
178176
)
@@ -182,7 +180,6 @@ def update_controller(self):
182180
float(self.input_auto[0]),
183181
float(self.input_auto[1]),
184182
float(self.input_auto[2]),
185-
float(self.input_auto[3]), # Rudder directly affects yaw for nvp
186183
)
187184
print("auto input", self.u)
188185
else:

‎scripts/fixedwing_sim.py.orig‎

Lines changed: 10 additions & 37 deletions
Original file line numberDiff line numberDiff line change
@@ -60,7 +60,7 @@ class Simulator(Node):
6060
# -------------------------------------------------------
6161
# mode handling
6262
# ----------------------------------------------
63-
self.input_mode = "manual"
63+
self.input_mode = "auto"
6464

6565
# -------------------------------------------------------
6666
# Lookup Table
@@ -70,10 +70,6 @@ class Simulator(Node):
7070
self.coeff_data = {
7171
"CL": 0.0,
7272
"CD": 0.0,
73-
<<<<<<< HEAD
74-
=======
75-
"Cy": 0.0,
76-
>>>>>>> cogni/main
7773
"Cl": 0.0,
7874
"Cm": 0.0,
7975
"Cn": 0.0,
@@ -109,6 +105,7 @@ class Simulator(Node):
109105
dtype=float,
110106
)
111107
self.u = np.zeros(4, dtype=float)
108+
112109
# start main loop on timer
113110
self.system_clock = rclpy.clock.Clock(
114111
clock_type=rclpy.clock.ClockType.SYSTEM_TIME
@@ -146,17 +143,10 @@ class Simulator(Node):
146143
def auto_joy_callback(self, msg: Joy):
147144
self.input_auto = ca.vertcat(
148145
msg.axes[0], # thrust
149-
<<<<<<< HEAD
150146
# msg.axes[1], # aileron
151147
-msg.axes[2], # elevator -- positive = elevator down (pitch up)
152148
msg.axes[1], # rudder
153149
) # TER
154-
=======
155-
msg.axes[1], # aileron
156-
-msg.axes[2], # elevator -- positive = elevator down (pitch up)
157-
msg.axes[3], # rudder
158-
) # TAER
159-
>>>>>>> cogni/main
160150

161151
def clock_as_msg(self):
162152
msg = Clock()
@@ -169,7 +159,6 @@ class Simulator(Node):
169159
# mode handling
170160
# ---------------------------------------------------------------------
171161

172-
<<<<<<< HEAD
173162
# # 4 Channel Airplane
174163
# if self.input_mode == "manual": #logitech f310
175164
# self.u = ca.vertcat( # TAER mode
@@ -183,23 +172,23 @@ class Simulator(Node):
183172
if self.input_mode == "manual": # logitech f310
184173
self.u = ca.vertcat( # TER mode 3-Channel
185174
float(self.input_aetr[2]),
186-
float(self.input_aetr[1]),
187-
-1 * float(self.input_aetr[0]),
175+
<<<<<<< HEAD
188176
=======
189-
elif self.input_mode == "auto":
190-
self.u = ca.vertcat( # TAER mode
191-
float(self.input_auto[0]),
192-
float(self.input_auto[1]),
193-
float(self.input_auto[2]),
194-
float(self.input_auto[3]),
177+
float(self.input_aetr[0]),
195178
>>>>>>> cogni/main
179+
float(self.input_aetr[1]),
180+
-1 * float(self.input_aetr[0]),
196181
)
197182
print("manual input", self.u)
198183
elif self.input_mode == "auto":
199184
self.u = ca.vertcat( # TER mode 3-Channel
200185
float(self.input_auto[0]),
201186
float(self.input_auto[1]),
202187
float(self.input_auto[2]),
188+
<<<<<<< HEAD
189+
=======
190+
float(self.input_auto[3]), # Rudder directly affects yaw for nvp
191+
>>>>>>> cogni/main
203192
)
204193
print("auto input", self.u)
205194
else:
@@ -218,11 +207,7 @@ class Simulator(Node):
218207
self.Info = {}
219208
self.Info["alpha"] = 0.0
220209
self.Info["beta"] = 0.0
221-
<<<<<<< HEAD
222210
# self.Info["ail"] = 0.0
223-
=======
224-
self.Info["ail"] = 0.0
225-
>>>>>>> cogni/main
226211
self.Info["elev"] = 0.0
227212
self.Info["rud"] = 0.0
228213

@@ -241,14 +226,6 @@ class Simulator(Node):
241226
self.Info["beta"] * RAD2DEG,
242227
self.Info["elev"] * RAD2DEG,
243228
)
244-
<<<<<<< HEAD
245-
=======
246-
self.coeff_data["Cy"] = self.lookup_tab["Cy"](
247-
self.Info["beta"] * RAD2DEG,
248-
self.Info["ail"] * RAD2DEG,
249-
self.Info["rud"] * RAD2DEG,
250-
)
251-
>>>>>>> cogni/main
252229
self.coeff_data["Cl"] = -1 * self.lookup_tab["Cl"](
253230
self.Info["alpha"] * RAD2DEG, self.Info["beta"] * RAD2DEG
254231
)
@@ -410,11 +387,7 @@ class Simulator(Node):
410387
self.pub_vel_b.publish(
411388
vector(self.Info["v_b"], "vel_b", [0.0, 1.0, 1.0, 1.0], 0.1)
412389
)
413-
<<<<<<< HEAD
414390
# print("alpha", self.Info["alpha"], "CL", self.Info["CL"], "CD", self.Info["CD"])
415-
=======
416-
print("alpha", self.Info["alpha"], "CL", self.Info["CL"], "CD", self.Info["CD"])
417-
>>>>>>> cogni/main
418391
# ------------------------------------
419392
# publish pose with covariance stamped
420393
# ------------------------------------

0 commit comments

Comments
 (0)