1
import numpy as np
2
import matplotlib.pyplot as plt
3
 
4
# --- CONSTANTS AND ENVIRONMENT ---
5
 
6
# Mars gravity
7
g = 3.71  # m/s^2
8
 
9
# Rotorcraft mass and rotor inertia
10
m = 1.8  # kg (Ingenuity ~1.8kg)
11
Ixx = 0.1
12
Iyy = 0.1
13
Izz = 0.15
14
I = np.diag([Ixx, Iyy, Izz])
15
I_inv = np.linalg.inv(I)
16
 
17
I_rotor = 0.02  # rotor inertia approx
18
 
19
# Atmospheric baseline on Mars
20
P0 = 610  # Pa, surface pressure Mars
21
T0 = 210  # K, average temp Mars surface
22
R_gas = 188.92  # J/kg·K for CO2 (Mars atmosphere)
23
M_air = 0.04401  # kg/mol CO2
24
cp_air = 730  # J/kg·K approx for CO2
25
 
26
# Rotor geometry
27
R = 1.2  # rotor radius (meters)
28
A = np.pi * R**2  # rotor disk area (m^2)
29
h_disk = 0.1  # thickness of rotor disk column for air mass estimate
30
 
31
# Aerodynamic coefficients
32
a0_base = 5.5  # base lift slope per rad (Mars thinner air effect)
33
Cd = 0.01  # profile drag coefficient approx
34
 
35
# Heating input (W)
36
Q_heat = 15  # Watts of heating input to rotor disk (can tune)
37
 
38
# Simulation parameters
39
dt = 0.01  # timestep in seconds
40
tf = 60  # total simulation time in seconds
41
 
42
# Initial rotor pitch angle (blade pitch), radians
43
theta_pitch = 0.1
44
 
45
# Initial state vector:
46
# [x, y, z, vx, vy, vz, p, q, r, phi, theta, psi, omega, beta, T_rotor]
47
state = np.array([0, 0, 100,   # initial pos 100m altitude
48
                  0, 0, -5,    # initial downward velocity 5 m/s
49
                  0, 0, 0,     # initial angular rates p,q,r
50
                  0, 0, 0,     # initial Euler angles roll, pitch, yaw
51
                  150,         # initial rotor angular velocity rad/s
52
                  0,           # initial flapping angle
53
                  T0])         # initial rotor local temperature
54
 
55
 
56
# --- FUNCTIONS ---
57
 
58
def rho_mars(z, T_local):
59
    """Calculate air density at altitude z(m) and local temperature T (K) on Mars."""
60
    # Scale height approx 11 km Mars
61
    H = 11100  
62
    P = P0 * np.exp(-z / H)
63
    rho = P / (R_gas * T_local)
64
    return rho
65
 
66
def rotor_forces(omega, rho, V_xy, beta, theta_pitch, a0):
67
    """
68
    Calculate rotor thrust and torque using blade element momentum theory approximation.
69
    omega: rotor angular velocity rad/s
70
    rho: air density kg/m3
71
    V_xy: velocity in rotor plane (x,y) m/s, 2D horizontal velocity relative air
72
    beta: flapping angle (rad)
73
    theta_pitch: blade pitch (rad)
74
    a0: lift slope corrected for temperature
75
    """
76
 
77
    # Effective inflow velocity components
78
    V_inflow = V_xy[1]  # vertical component in rotor plane, approximated
79
 
80
    # Angle of attack approx for blade element
81
    alpha = theta_pitch - beta - V_inflow / (omega * R + 1e-6)  # avoid div0
82
    
83
    # Lift coefficient from alpha
84
    Cl = a0 * alpha
85
    
86
    # Thrust coefficient approx
87
    Ct = Cl * 0.5  # simplified scaling
88
    
89
    # Torque coefficient approx
90
    Cq = 0.01 + 0.02 * omega / 100  # arbitrary torque coef
91
    
92
    T = Ct * rho * A * (omega * R)**2
93
    Q = Cq * rho * A * (omega * R)**2 * R
94
    
95
    # Flapping moment (pitch moment), proportional to thrust and beta
96
    M_flap = -T * beta
97
 
98
    return T, Q, M_flap
99
 
100
def rot_matrix(phi, theta, psi):
101
    """ZYX Euler rotation matrix body to inertial"""
102
    Rz = np.array([[np.cos(psi), -np.sin(psi), 0],
103
                   [np.sin(psi),  np.cos(psi), 0],
104
                   [0, 0, 1]])
105
    Ry = np.array([[ np.cos(theta), 0, np.sin(theta)],
106
                   [0, 1, 0],
107
                   [-np.sin(theta), 0, np.cos(theta)]])
108
    Rx = np.array([[1, 0, 0],
109
                   [0, np.cos(phi), -np.sin(phi)],
110
                   [0, np.sin(phi),  np.cos(phi)]])
111
    return Rz @ Ry @ Rx
112
 
113
def euler_dot(phi, theta, psi, p, q, r):
114
    """Euler angle rates from body angular velocities"""
115
    t_theta = np.tan(theta)
116
    s_phi = np.sin(phi)
117
    c_phi = np.cos(phi)
118
    c_theta = np.cos(theta)
119
    phi_dot = p + q * s_phi * t_theta + r * c_phi * t_theta
120
    theta_dot = q * c_phi - r * s_phi
121
    psi_dot = q * s_phi / c_theta + r * c_phi / c_theta
122
    return np.array([phi_dot, theta_dot, psi_dot])
123
 
124
def wind_vector(t, z):
125
    """Time-varying wind with gusts and altitude decay"""
126
    base_wind = np.array([5.0, 0.0, 0.0])  # m/s steady wind East
127
    gust_amp = 2.0 * np.exp(-z / 500)
128
    gust = gust_amp * np.array([
129
        np.sin(0.1 * t),
130
        np.sin(0.07 * t + 1),
131
        0.0])
132
    return base_wind + gust
133
 
134
def dust_storm_factor(t):
135
    """Dust storm multiplier for drag force"""
136
    if 20 <= t <= 40:
137
        return 2.5  # 2.5x drag multiplier in dust storm
138
    else:
139
        return 1.0
140
 
141
def drag_force_3d(rho, V_rel, A, Cd, dust_factor=1.0):
142
    Vmag = np.linalg.norm(V_rel)
143
    if Vmag < 1e-6:
144
        return np.zeros(3)
145
    drag_mag = 0.5 * rho * Vmag**2 * Cd * A * dust_factor
146
    drag_dir = -V_rel / Vmag
147
    return drag_mag * drag_dir
148
 
149
def rotor_thrust_vector(T_force, beta, phi_flap=0.0):
150
    """Thrust vector in body frame, tilted by flap angles"""
151
    Tx = T_force * np.sin(beta)
152
    Ty = T_force * np.sin(phi_flap)
153
    Tz = T_force * np.cos(beta) * np.cos(phi_flap)
154
    return np.array([Tx, Ty, Tz])
155
 
156
def derivatives(state, t):
157
    x, y, z, vx, vy, vz, p, q, r, phi, theta, psi, omega, beta, T_local = state
158
 
159
    # Rotation matrix body to inertial
160
    Rbi = rot_matrix(phi, theta, psi).T
161
 
162
    # Wind velocity inertial
163
    V_wind = wind_vector(t, z)
164
 
165
    # Velocity relative to air inertial
166
    V_air_inertial = np.array([vx, vy, vz]) - V_wind
167
 
168
    # Velocity relative air in body frame
169
    V_air_body = rot_matrix(phi, theta, psi) @ V_air_inertial
170
 
171
    # Air density at altitude and temperature
172
    rho = rho_mars(z, T_local)
173
 
174
    # Rotor disk volume and air mass for heating
175
    A_disk = A
176
    V_disk = A_disk * h_disk
177
    m_air = rho * V_disk
178
 
179
    # Heating rate for rotor disk air temperature
180
    dTdt = Q_heat / (m_air * cp_air) if m_air > 0 else 0
181
    T_new = T_local + dTdt * dt
182
    delta_T = max(0, T_new - T0)
183
 
184
    # Adjusted lift curve slope a0
185
    a0_mod = a0_base * (1 + 0.05 * (delta_T / 10))
186
 
187
    # Rotor forces and moments
188
    T_force, Q_torque, M_flap = rotor_forces(omega, rho, V_air_body[:2], beta, theta_pitch, a0_mod)
189
 
190
    # Rotor thrust vector body frame
191
    T_vec_body = rotor_thrust_vector(T_force, beta, phi_flap=0.0)
192
 
193
    # Drag force body frame with dust multiplier
194
    dust_factor = dust_storm_factor(t)
195
    F_drag_body = drag_force_3d(rho, V_air_body, A, Cd, dust_factor)
196
 
197
    # Gravity inertial
198
    Fg_inertial = np.array([0, 0, -m * g])
199
    # Gravity body frame
200
    Fg_body = rot_matrix(phi, theta, psi) @ Fg_inertial
201
 
202
    # Total force body
203
    F_total_body = Fg_body + T_vec_body + F_drag_body
204
 
205
    # Linear acceleration inertial
206
    a_body = F_total_body / m
207
    acc_inertial = rot_matrix(phi, theta, psi).T @ a_body
208
 
209
    # Euler rates
210
    eul_rates = euler_dot(phi, theta, psi, p, q, r)
211
 
212
    # Moments
213
    Mx = -M_flap
214
    My = 0.0
215
    Mz = -Q_torque
216
    Moments = np.array([Mx, My, Mz])
217
 
218
    omega_body = np.array([p, q, r])
219
    omega_dot = I_inv @ (Moments - np.cross(omega_body, I @ omega_body))
220
 
221
    # Rotor angular acceleration with mechanical losses
222
    tau_mech = -0.01 * omega
223
    alpha = (-Q_torque + tau_mech) / I_rotor
224
 
225
    # Flapping acceleration simplified
226
    beta_dot = -0.5 * beta + 0.1 * (theta_pitch - beta)
227
 
228
    # Temperature rate
229
    T_dot = dTdt
230
 
231
    return np.array([
232
        vx, vy, vz,            # Position derivatives = velocities inertial
233
        acc_inertial[0], acc_inertial[1], acc_inertial[2],  # Velocity derivatives inertial
234
        omega_dot[0], omega_dot[1], omega_dot[2],           # Angular velocity derivatives body
235
        eul_rates[0], eul_rates[1], eul_rates[2],           # Euler angle derivatives
236
        alpha, beta_dot, T_dot                                # Rotor angular acceleration, flap, temperature
237
    ])
238
 
239
def rk4_step(f, y, t, dt):
240
    k1 = f(y, t)
241
    k2 = f(y + dt/2 * k1, t + dt/2)
242
    k3 = f(y + dt/2 * k2, t + dt/2)
243
    k4 = f(y + dt * k3, t + dt)
244
    return y + dt/6 * (k1 + 2*k2 + 2*k3 + k4)
245
 
246
# --- SIMULATION ---
247
 
248
time = np.arange(0, tf, dt)
249
states = np.zeros((len(time), len(state)))
250
states[0] = state
251
 
252
for i in range(1, len(time)):
253
    states[i] = rk4_step(derivatives, states[i-1], time[i-1], dt)
254
 
255
# --- PLOTTING RESULTS ---
256
 
257
plt.figure(figsize=(12, 8))
258
 
259
plt.subplot(3, 2, 1)
260
plt.plot(time, states[:, 2])
261
plt.xlabel('Time (s)')
262
plt.ylabel('Altitude (m)')
263
plt.title('Altitude vs Time')
264
 
265
plt.subplot(3, 2, 2)
266
plt.plot(time, states[:, 13])  # flapping angle beta
267
plt.xlabel('Time (s)')
268
plt.ylabel('Flapping angle (rad)')
269
plt.title('Flapping angle vs Time')
270
 
271
plt.subplot(3, 2, 3)
272
plt.plot(time, states[:, 12])
273
plt.xlabel('Time (s)')
274
plt.ylabel('Rotor speed (rad/s)')
275
plt.title('Rotor speed vs Time')
276
 
277
plt.subplot(3, 2, 4)
278
plt.plot(time, states[:, 14])
279
plt.xlabel('Time (s)')
280
plt.ylabel('Rotor temperature (K)')
281
plt.title('Rotor Temperature vs Time')
282
 
283
plt.subplot(3, 2, 5)
284
plt.plot(time, states[:, 9] * 180/np.pi, label='Roll')
285
plt.plot(time, states[:, 10] * 180/np.pi, label='Pitch')
286
plt.plot(time, states[:, 11] * 180/np.pi, label='Yaw')
287
plt.xlabel('Time (s)')
288
plt.ylabel('Euler Angles (deg)')
289
plt.title('Euler Angles vs Time')
290
plt.legend()
291
 
292
plt.tight_layout()
293
plt.show()
294
 
295
 
296
 
297
 
298
 
299