def InputCommand(self, track, gs, climbrate):
self.command.VelocityControlMode = rtString(b"GS", (c_int*2)(1,2))
self.command.VTAS = 0
self.command.GS = gs*1.94384449 # m/s to kts
self.command.VerticalControlMode = rtString(b"ROCD", (c_int*2)(1,4))
self.command.Alt = 0
self.command.ROCD = climbrate*196.8503937 # m/s to fpm
self.command.HeadingControlMode = rtString(b"GT", (c_int*2)(1,2))
self.command.Psi = 0
self.command.GT = track
def __init__(self, idx, home_gps, dt=0.05):
"""
Initialize a Six-passenger Quadrotor UAM simulation.
Input arguments are defined in VehicleSimInterface.
The vehicle starts at the given home_gps location.
"""
super().__init__(idx, home_gps, dt)
self.massLevel = 2
# Define controller gains
self.gains = SpqGains()
self.gains.k_VTAS = 1.5 # airspeed gain (1/s)
self.gains.gLimit_dVTAS = 0.5 # max horizontal accel (g's)
self.gains.k_ROCD = 2*1.25*0.4 # climbrate gain (2*zeta*wn)
self.gains.ROCD_max = 2000 # max climbrate (fpm)
self.gains.gLimit_dROCD = 0.15 # max vertical accel (g's)
self.gains.k_Alt = 0.4**2*60 # altitude gain (wn^2)
self.gains.k_deltaPsi = 0.3 # turn rate gain (1/s)
self.gains.dPsi_desired_max = 10 # max turn rate command (deg/s)
self.gains.Phi_desired_max = 60 # max bank angle (deg)
self.gains.k_Phi = 2.0 # bank angle feedback gain
self.gains.dPhi_max = 30 # max roll rate (deg/s)
self.gains.P_MAX = 2079 # max battery draw (MJ/hr)
self.gains.usePowerLimitEnforcement = True # limit accel based on P_MAX
# Define input command
self.command = SpqCommand()
# Velocity contol mode: "GS" or "VTAS"
self.command.VelocityControlMode = rtString(b"GS", (c_int*2)(1,2))
self.command.VTAS = 0 # commanded airspeed (kts)
self.command.GS = 0 # commanded groundspeed (kts)
# Vertical contol mode: "Alt" or "ROCD"
self.command.VerticalControlMode = rtString(b"ROCD", (c_int*2)(1,4))
self.command.Alt = 0 # commanded altitude (ft)
self.command.ROCD = 0 # commanded climb rate (fpm)
# Vertical contol mode: "Heading" or "GT"
self.command.HeadingControlMode = rtString(b"GT", (c_int*2)(1,2))
self.command.Psi = 0 # commanded heading (deg CW from North)
self.command.GT = 0 # commanded ground track (deg CW from North)
# Define initial vehicle state
self.state = SpqState(
distance = 0, # horizontal distance from starting point (nm)
VTAS = 0, # airspeed (kts)
Altitude = 0, # altitude (ft)
ROCD = 0, # climb rate (fpm)
Psi = 0, # heading (deg CW from North)
Phi = 0, # bank angle (deg)
N_pos = 0, # distance North from starting point (nm)
E_pos = 0, # distance East from starting point (nm)
)
self.groundtrack = 0 # current ground track (deg CW from North)
self.wind_mps = [0, 0] # wind speed [vn, ve] (m/s)