forked from TUDelft-CNS-ATM/bluesky
-
Notifications
You must be signed in to change notification settings - Fork 2
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
A correction when drone's TAS is less than the wind magnitude
- Loading branch information
jbueno
committed
Sep 12, 2022
1 parent
dfa7840
commit db1f6d9
Showing
3 changed files
with
390 additions
and
372 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,62 +1,66 @@ | ||
| """ Pilot logic.""" | ||
| import numpy as np | ||
| import bluesky as bs | ||
| from bluesky.core import TrafficArrays | ||
| import bluesky as bs | ||
| import numpy as np | ||
|
|
||
|
|
||
| class APorASAS(TrafficArrays): | ||
| def __init__(self): | ||
| class APorASAS( TrafficArrays ): | ||
| def __init__( self ): | ||
| super().__init__() | ||
| with self.settrafarrays(): | ||
| # Desired aircraft states | ||
| self.alt = np.array([]) # desired altitude [m] | ||
| self.hdg = np.array([]) # desired heading [deg] | ||
| self.trk = np.array([]) # desired track angle [deg] | ||
| self.vs = np.array([]) # desired vertical speed [m/s] | ||
| self.tas = np.array([]) # desired speed [m/s] | ||
|
|
||
| def create(self, n=1): | ||
| super().create(n) | ||
| self.alt = np.array( [] ) # desired altitude [m] | ||
| self.hdg = np.array( [] ) # desired heading [deg] | ||
| self.trk = np.array( [] ) # desired track angle [deg] | ||
| self.vs = np.array( [] ) # desired vertical speed [m/s] | ||
| self.tas = np.array( [] ) # desired speed [m/s] | ||
|
|
||
| def create( self, n=1 ): | ||
| super().create( n ) | ||
| self.alt[-n:] = bs.traf.alt[-n:] | ||
| self.tas[-n:] = bs.traf.tas[-n:] | ||
| self.hdg[-n:] = bs.traf.hdg[-n:] | ||
| self.trk[-n:] = bs.traf.trk[-n:] | ||
|
|
||
| def update(self): | ||
| def update( self ): | ||
| #--------- Input to Autopilot settings to follow: destination or ASAS ---------- | ||
| # Convert the ASAS commanded speed from ground speed to TAS | ||
| if bs.traf.wind.winddim > 0: | ||
| vwn, vwe = bs.traf.wind.getdata(bs.traf.lat, bs.traf.lon, bs.traf.alt) | ||
| asastasnorth = bs.traf.cr.tas * np.cos(np.radians(bs.traf.cr.trk)) - vwn | ||
| asastaseast = bs.traf.cr.tas * np.sin(np.radians(bs.traf.cr.trk)) - vwe | ||
| asastas = np.sqrt(asastasnorth**2 + asastaseast**2) | ||
| vwn, vwe = bs.traf.wind.getdata( bs.traf.lat, bs.traf.lon, bs.traf.alt ) | ||
| asastasnorth = bs.traf.cr.tas * np.cos( np.radians( bs.traf.cr.trk ) ) - vwn | ||
| asastaseast = bs.traf.cr.tas * np.sin( np.radians( bs.traf.cr.trk ) ) - vwe | ||
| asastas = np.sqrt( asastasnorth ** 2 + asastaseast ** 2 ) | ||
| # no wind, then ground speed = TAS | ||
| else: | ||
| asastas = bs.traf.cr.tas # TAS [m/s] | ||
| asastas = bs.traf.cr.tas # TAS [m/s] | ||
|
|
||
| # Select asas if there is a conflict AND resolution is on | ||
| # Determine desired states per channel whether to use value from ASAS or AP. | ||
| # bs.traf.cr.active may be used as well, will set all of these channels | ||
| self.trk = np.where(bs.traf.cr.hdgactive, bs.traf.cr.trk, bs.traf.ap.trk) | ||
| self.tas = np.where(bs.traf.cr.tasactive, asastas, bs.traf.ap.tas) | ||
| self.alt = np.where(bs.traf.cr.altactive, bs.traf.cr.alt, bs.traf.ap.alt) | ||
| self.vs = np.where(bs.traf.cr.vsactive, bs.traf.cr.vs, bs.traf.ap.vs) | ||
| self.trk = np.where( bs.traf.cr.hdgactive, bs.traf.cr.trk, bs.traf.ap.trk ) | ||
| self.tas = np.where( bs.traf.cr.tasactive, asastas, bs.traf.ap.tas ) | ||
| self.alt = np.where( bs.traf.cr.altactive, bs.traf.cr.alt, bs.traf.ap.alt ) | ||
| self.vs = np.where( bs.traf.cr.vsactive, bs.traf.cr.vs, bs.traf.ap.vs ) | ||
|
|
||
| # ASAS can give positive and negative VS, but the sign of VS is determined using delalt in Traf.ComputeAirSpeed | ||
| # Therefore, ensure that pilot.vs is always positive to prevent opposite signs of delalt and VS in Traf.ComputeAirSpeed | ||
| self.vs = np.abs(self.vs) | ||
| self.vs = np.abs( self.vs ) | ||
|
|
||
| # Compute the desired heading needed to compensate for the wind | ||
| if bs.traf.wind.winddim > 0: | ||
|
|
||
| # Calculate wind correction | ||
| vwn, vwe = bs.traf.wind.getdata(bs.traf.lat, bs.traf.lon, bs.traf.alt) | ||
| Vw = np.sqrt(vwn * vwn + vwe * vwe) | ||
| winddir = np.arctan2(vwe, vwn) | ||
| drift = np.radians(self.trk) - winddir # [rad] | ||
| steer = np.arcsin(np.minimum(1.0, np.maximum(-1.0, | ||
| Vw * np.sin(drift) / np.maximum(0.001, bs.traf.tas)))) | ||
| vwn, vwe = bs.traf.wind.getdata( bs.traf.lat, bs.traf.lon, bs.traf.alt ) | ||
| Vw = np.sqrt( vwn * vwn + vwe * vwe ) | ||
| winddir = np.arctan2( vwe, vwn ) | ||
| drift = np.radians( self.trk ) - winddir # [rad] | ||
| steer = np.arcsin( np.minimum( 1.0, np.maximum( -1.0, | ||
| Vw * np.sin( drift ) / np.maximum( 0.001, bs.traf.tas ) ) ) ) | ||
| # USEPE | ||
| applywind = ( abs( bs.traf.tas ) > 1.1 * abs( Vw ) ) | ||
| # | ||
| # desired heading | ||
| self.hdg = (self.trk + np.degrees(steer)) % 360. | ||
| self.hdg = ( self.trk + np.degrees( steer ) * applywind ) % 360. | ||
| else: | ||
| self.hdg = self.trk % 360. | ||
|
|
Oops, something went wrong.