def process_gprmc(time, row): global gprmc_time, gprmc_groundspeed_ms, gprmc_vario_ms true_groundspeed_kts = measurement(0.0, 2.2035) try: true_groundspeed_kts = measurement(float(row[11]), 10.0/kts2ms) true_course_deg = measurement(float(row[12]), 0.0) except: return true_course_rad = (true_course_deg/180.0)*math.pi true_groundspeed_ms = true_groundspeed_kts * kts2ms #Update kinetic vario dtime = time - gprmc_time gprmc_vario_ms = (true_groundspeed_ms - gprmc_groundspeed_ms)/dtime gprmc_time = time gprmc_groundspeed_ms = true_groundspeed_ms #Sin/cos swapped because of 90deg difference groundspeed_x = measurement(true_groundspeed_ms.get_mean() * math.sin(true_course_rad.get_mean()), 10.0) groundspeed_y = measurement(true_groundspeed_ms.get_mean() * math.cos(true_course_rad.get_mean()), 10.0) [wind_x, wind_y, wind_x0, wind_y0, wind_x1, wind_y1, wind_x2, wind_y2] = calc_wind(groundspeed_x, groundspeed_y) #[measurement(-2.28, 1.0), measurement(-1.46, 1.0)] kalman.update_true_groundspeed(time, groundspeed_x, groundspeed_y) kalman.update_wind(time, wind_x, wind_y) line = ",".join([str(time), csv_meas(groundspeed_x), csv_meas(groundspeed_y), csv_meas(wind_x), csv_meas(wind_y), csv_meas(kalman.get_wind_x().predict(time, 0.0)), csv_meas(kalman.get_wind_y().predict(time, 0.0)), csv_meas(wind_x0), csv_meas(wind_y0), csv_meas(wind_x1), csv_meas(wind_y1), csv_meas(wind_x2), csv_meas(wind_y2)])
def process_bmp085(time, row): global bmp085_time, bmp085_height try: static_pressure = measurement(float(row[4]), 6.362254) except: return height = measurement(pressure2height(static_pressure.get_mean()), 0.4533) vario = (height - bmp085_height)/(time - bmp085_time) kalman.update_static_pressure(time, static_pressure) kalman.update_alt_vario(time, height, vario) bmp085_time = time bmp085_height = height
def calc_safety_glide(a,b,c,height,distance,probability,uncertainty):
target_uncertainty = math.sqrt(distance) * uncertainty
target = measurement(target_height, target_uncertainty)
safety_target_height = calc_probability_height(target, probability)
safety_delta_height = height - safety_target_height
safety_glide_ratio = distance/safety_delta_height
velocity = calc_speed_from_glide_ratio(a,b,c,safety_glide_ratio)
if ((target < safety_target_height) - probability) > 0.01:
exit("SANITY CHECK FAILED")
return velocity
def process_mma8452(time, row): try: g0 = float(row[10]) g1 = float(row[11]) g2 = float(row[12]) except: return g = math.sqrt((g0*g0) + (g1*g1) + (g2*g2)) g_meas = measurement(g, 0.10567) kalman.update_g(time, g_meas)
def process_ms5803_01ba_77(time, row):
global ms5803_01ba_77_height, ms5803_01ba_77_time
# try:
pressure = float(row[12])
# except:
# return
height = pressure2height(pressure)
height_meas = measurement(height, 0.21)
vario = (height_meas - ms5803_01ba_77_height)/(time - ms5803_01ba_77_time)
kalman.update_alt_vario(time, height_meas, vario)
ms5803_01ba_77_time = time
ms5803_01ba_77_height = height_meas
def calc_wind(wind_x2, wind_y2): global wind_x_backlog, wind_y_backlog wind_x_backlog.append(wind_x2) wind_y_backlog.append(wind_y2) wind_len = len(wind_x_backlog) wind_x0 = wind_x_backlog[0] wind_y0 = wind_y_backlog[0] wind_x1 = wind_x_backlog[wind_len/2] wind_y1 = wind_y_backlog[wind_len/2] if wind_len < 3: return [measurement(0.0, finf), measurement(0.0, finf),wind_x0,wind_y0,wind_x1,wind_y1,wind_x2,wind_y2] [m12_x, m12_y] = midpoint(wind_x0, wind_y0, wind_x1, wind_y1) [m23_x, m23_y] = midpoint(wind_x1, wind_y1, wind_x2, wind_y2) [r12_x, r12_y] = perpendicular(wind_x0, wind_y0, wind_x1, wind_y1) [r23_x, r23_y] = perpendicular(wind_x1, wind_y1, wind_x2, wind_y2) teller = (-r23_x * m23_y) - (r23_y * m12_x) + (r23_y * m23_x) + (m12_y * r23_x) noemer = (r23_y * r12_x) - (r12_y * r23_x) try: v = teller/noemer except ZeroDivisionError: v = measurement(math.copysign(finf, teller.get_mean()), finf) x = m12_x + (v * r12_x) y = m12_y + (v * r12_y) if wind_len > (20*22): wind_x_backlog.pop(0) wind_y_backlog.pop(0) return [x,y,wind_x0,wind_y0,wind_x1,wind_y1,wind_x2,wind_y2]
def process_gpgga(time, row): global gpgga_time, gpgga_height try: fix_type = int(row[10]) if fix_type > 0: hdop = float(row[12]) height = measurement(float(row[13]), 10.0*hdop) vario = (height - gpgga_height)/(time - gpgga_time) except: return if fix_type > 0: kalman.update_alt_vario(time, height, vario) gpgga_time = time gpgga_height = height
end = {}
seconds = {}
pascal = {}
height = {}
dtime = {}
height_prediction = {}
height_kalman = {}
vario = {}
vario_prediction = {}
vario_kalman = {}
g_limit = 0.5
vario_process_noise = measurement(0, ((9.81*g_limit)/3)) #Acceleration is 20m/s^2 99% probability (3-sigma).
height_process_noise = measurement(0, 0)
with sys.stdin as csvfile:
metingen = csv.DictReader(csvfile, delimiter=',')
i = 0
for row in metingen:
#Read data
start_sec[i] = float(row["Start"])
start_nsec[i] = float(row["Start_nsec"])
end_sec[i] = float(row["End"])
end_nsec[i] = float(row["End_nsec"])
pascal[i] = float(row["Pascal"])
#Construct start time and end time
v = teller/noemer except ZeroDivisionError: v = measurement(math.copysign(finf, teller.get_mean()), finf) x = m12_x + (v * r12_x) y = m12_y + (v * r12_y) if wind_len > (20*22): wind_x_backlog.pop(0) wind_y_backlog.pop(0) return [x,y,wind_x0,wind_y0,wind_x1,wind_y1,wind_x2,wind_y2] bmp085_time = 0.0 bmp085_height = measurement(0.0, 0.0) def process_bmp085(time, row): global bmp085_time, bmp085_height try: static_pressure = measurement(float(row[4]), 6.362254) except: return height = measurement(pressure2height(static_pressure.get_mean()), 0.4533) vario = (height - bmp085_height)/(time - bmp085_time) kalman.update_static_pressure(time, static_pressure) kalman.update_alt_vario(time, height, vario) bmp085_time = time bmp085_height = height
velocity_ms = {}
velocity_ms_prediction = {}
velocity_ms_kalman = {}
accel_hor = {}
accel_hor_prediction = {}
accel_hor_kalman = {}
dtime = {}
g=9.81
g_limit = 1.0
knots2ms = 0.514444
velocity_process_noise = measurement(0, g*g_limit/3)
accel_hor_process_noise = measurement(0, g*g_limit/3)
with sys.stdin as csvfile:
metingen = csv.reader(csvfile, delimiter=',')
i = 0
for row in metingen:
#Read data
start_sec[i] = float(row[0])
start_nsec[i] = float(row[1])
end_sec[i] = float(row[2])
end_nsec[i] = float(row[3])
try:
velocity_knots[i] = measurement(float(row[11]), 2.2035) #Value from stdev(L1:L12000)
end = {}
dtime = {}
orientation = {}
rotation = {}
orientation_prediction = {}
with sys.stdin as csvfile:
metingen = csv.DictReader(csvfile, delimiter=',')
i = 0
for row in metingen:
start_sec[i] = float(row["Start_sec"])
start_nsec[i] = float(row["Start_nsec"])
end_sec[i] = float(row["End_sec"])
end_nsec[i] = float(row["End_nsec"])
accel_x[i] = measurement(float(row["A_x"]), 1.21)
accel_y[i] = measurement(float(row["A_y"]), 7.25)
accel_z[i] = measurement(float(row["A_z"]), 1.97)
rot_x[i] = measurement(float(row["Rot_x"]), 0.94)
rot_y[i] = measurement(float(row["Rot_y"]), 1.40)
rot_z[i] = measurement(float(row["Rot_z"]), 0.89)
mag_x[i] = measurement(float(row["Mag_x"]), 3.90)
mag_y[i] = measurement(float(row["Mag_y"]), 14.92)
mag_z[i] = measurement(float(row["Mag_z"]), 5.91)
start[i] = start_sec[i] + (start_nsec[i] * math.pow(10, -9))
end[i] = end_sec[i] + (end_nsec[i] * math.pow(10, -9))
#Time
if i > 0: dtime[i] = ((start[i] - start[i-1]) + (end[i] - end[i - 1]) / 2)
else: dtime[i] = 1
end = {}
seconds = {}
hdop = {}
height = {}
dtime = {}
height_prediction = {}
height_kalman = {}
vario = {}
vario_prediction = {}
vario_kalman = {}
g_limit = 1.7
vario_process_noise = measurement(0, ((9.81*g_limit)/3.0)) #Acceleration is 20m/s^2 99% probability (3-sigma).
height_process_noise = measurement(0, 0.1/3.0)
with sys.stdin as csvfile:
metingen = csv.reader(csvfile, delimiter=',')
i = 0
for row in metingen:
#Read data
start_sec[i] = float(row[0])
start_nsec[i] = float(row[1])
end_sec[i] = float(row[2])
end_nsec[i] = float(row[3])
hdop[i] = float(row[12])
if hdop[i] == 0.0:
hdop[i] = 1.0
height[i] = measurement(float(row[13]), hdop[i]*10.0) #bmp085 value for lack of something better
