-
Notifications
You must be signed in to change notification settings - Fork 6
/
Copy pathprediction.py
executable file
·181 lines (133 loc) · 7.86 KB
/
prediction.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
from enum import Enum
import math
class FlightMode(Enum):
fmIdle = 0
fmLaunched = 1
fmDescending = 2
fmLanded = 3
class Delta():
def __init__(self, latitude, longitude):
self.latitude = latitude
self.longitude = longitude
class Predictor(object):
def __init__(self, LandingAltitude, DefaultCDA):
self.SlotSize = 100
self.SlotCount = 60000 // self.SlotSize
self.Deltas = []
self.FlightMode = FlightMode.fmIdle
self.PreviousPosition = {'time': '00:00:00', 'lat': 0.0, 'lon': 0.0, 'alt': 0, 'sats': 0, 'fixtype': 0}
self.MinimumAltitude = 0
self.MaximumAltitude = 0
self.AscentRate = 0
self.LandingAltitude = LandingAltitude
self.LandingLatitude = 0.0
self.LandingLongitude = 0.0
self.PollPeriod = 5
self.Counter = 0
self.CDA = DefaultCDA
for i in range(self.SlotCount):
self.Deltas.append(Delta(0,0))
# Callback for a new prediction
# Is called everytime a new prediction is calculated
self._WhenNewPrediction = None
def GetSlot(self, Altitude):
Slot = int(Altitude // self.SlotSize)
if Slot < 0:
Slot = 0
if Slot >= self.SlotSize:
Slot = self.SlotSize-1
return Slot
def CalculateAirDensity(self, Altitude):
if Altitude < 11000.0:
# below 11Km - Troposphere
Temperature = 15.04 - (0.00649 * Altitude)
Pressure = 101.29 * pow((Temperature + 273.1) / 288.08, 5.256)
elif Altitude < 25000.0:
# between 11Km and 25Km - lower Stratosphere
Temperature = -56.46
Pressure = 22.65 * math.exp(1.73 - ( 0.000157 * Altitude))
else:
# above 25Km - upper Stratosphere
Temperature = -131.21 + (0.00299 * Altitude)
Pressure = 2.488 * math.pow((Temperature + 273.1) / 216.6, -11.388)
return Pressure / (0.2869 * (Temperature + 273.1))
def CalculateDescentRate(self, Weight, CDTimesArea, Altitude):
Density = self.CalculateAirDensity(Altitude)
return math.sqrt((Weight * 9.81)/(0.5 * Density * CDTimesArea))
def CalculateCDA(self, Weight, Altitude, DescentRate):
if DescentRate > 0.0:
Density = self.CalculateAirDensity(Altitude)
# printf("Alt %.0lf, Rate %.1lf, CDA %.1lf\n", Altitude, DescentRate, (Weight * 9.81)/(0.5 * Density * DescentRate * DescentRate));
return (Weight * 9.81)/(0.5 * Density * DescentRate * DescentRate)
else:
return self.CDA
def CalculateLandingPosition(self, Latitude, Longitude, Altitude):
TimeTillLanding = 0;
Slot = self.GetSlot(Altitude);
DistanceInSlot = Altitude + 1 - Slot * self.SlotSize
while Altitude > self.LandingAltitude:
Slot = self.GetSlot(Altitude)
if Slot == self.GetSlot(self.LandingAltitude):
DistanceInSlot = Altitude - self.LandingAltitude
DescentRate = self.CalculateDescentRate(1.0, self.CDA, Altitude)
TimeInSlot = DistanceInSlot / DescentRate
Latitude += self.Deltas[Slot].latitude * TimeInSlot
Longitude += self.Deltas[Slot].longitude * TimeInSlot
# printf("SLOT %d: alt %lu, lat=%lf, long=%lf, rate=%lf, dist=%lu, time=%lf\n", Slot, Altitude, Latitude, Longitude, DescentRate, DistanceInSlot, TimeInSlot);
TimeTillLanding = TimeTillLanding + TimeInSlot
Altitude -= DistanceInSlot
DistanceInSlot = self.SlotSize
return {'pred_lat': Latitude, 'pred_lon': Longitude ,'TTL': TimeTillLanding}
def AddGPSPosition(self, Position):
Result = None
if Position['sats'] >= 4:
self.Counter = self.Counter + 1
if self.Counter >= self.PollPeriod:
self.Counter = 0
if Position['alt'] <= 0:
self.AscentRate = 0
else:
self.AscentRate = self.AscentRate * 0.7 + (Position['alt'] - self.PreviousPosition['alt']) * 0.3;
if (Position['alt'] < self.MinimumAltitude) or (self.MinimumAltitude == 0):
self.MinimumAltitude = Position['alt']
if Position['alt'] > self.MaximumAltitude:
self.MaximumAltitude = Position['alt']
if (self.AscentRate >= 1.0) and (Position['alt'] > (self.MinimumAltitude+150)) and (self.FlightMode == FlightMode.fmIdle):
self.FlightMode = FlightMode.fmLaunched
print("*** LAUNCHED ***");
if (self.AscentRate < -10.0) and (self.MaximumAltitude >= (self.MinimumAltitude+2000)) and (self.FlightMode == FlightMode.fmLaunched):
self.FlightMode = FlightMode.fmDescending
print("*** DESCENDING ***");
if (self.AscentRate >= -0.1) and (Position['alt'] <= self.LandingAltitude+2000) and (self.FlightMode == FlightMode.fmDescending):
self.FlightMode = FlightMode.fmLanded
print("*** LANDED ***")
if self.FlightMode == FlightMode.fmLaunched:
# Going up - store deltas
Slot = self.GetSlot(Position['alt']/2 + self.PreviousPosition['alt']/2);
# Deltas are scaled to be horizontal distance per second (i.e. speed)
self.Deltas[Slot].latitude = (Position['lat'] - self.PreviousPosition['lat']) / self.PollPeriod
self.Deltas[Slot].longitude = (Position['lon'] - self.PreviousPosition['lon']) / self.PollPeriod
print("Slot " + str(Slot) + " = " + str(Position['alt']) + "," + str(self.Deltas[Slot].latitude) + "," + str(self.Deltas[Slot].longitude))
elif self.FlightMode == FlightMode.fmDescending:
# Coming down - try and calculate how well chute is doing
self.CDA = (self.CDA * 4 + self.CalculateCDA(1.0, Position['alt']/2 + self.PreviousPosition['alt']/2, (self.PreviousPosition['alt'] - Position['alt']) / self.PollPeriod)) / 5
if (self.FlightMode == FlightMode.fmLaunched) or (self.FlightMode == FlightMode.fmDescending):
Result = self.CalculateLandingPosition(Position['lat'], Position['lon'], Position['alt']);
# GPS->PredictedLandingSpeed = CalculateDescentRate(Config.payload_weight, GPS->CDA, Config.LandingAltitude);
# printf("Expected Descent Rate = %4.1lf (now) %3.1lf (landing), time till landing %d\n",
# CalculateDescentRate(Config.payload_weight, GPS->CDA, GPS->Altitude),
# GPS->PredictedLandingSpeed,
# GPS->TimeTillLanding);
# printf("Current %f, %f, alt %" PRId32 "\n", GPS->Latitude, GPS->Longitude, GPS->Altitude);
# printf("Prediction %f, %f, CDA %lf\n", GPS->PredictedLatitude, GPS->PredictedLongitude, GPS->CDA);
print('PREDICTOR: ' + str(Position['time']) + ', ' + "{:.5f}".format(Position['lat']) + ', ' + "{:.5f}".format(Position['lon']) + ', ' + str(Position['alt']) + ', ' + str(Position['sats']))
self.PreviousPosition = Position.copy()
if self._WhenNewPrediction:
self._WhenNewPrediction(Result, self.FlightMode)
return Result
@property
def WhenNewPrediction(self):
return self._WhenNewPrediction
@WhenNewPrediction.setter
def WhenNewPrediction(self, value):
self._WhenNewPrediction = value