kopia lustrzana https://github.com/projecthorus/radiosonde_auto_rx
Add internal calculation of ascent/descent rates, using GenericTrack class from chasemapper.
Mark Jessop
rodzic
16e97af089
commit
c6915a7196
|
|
@ -0,0 +1,258 @@
|
|||
#!/usr/bin/env python
|
||||
#
|
||||
# Project Horus - Flight Data to Geometry
|
||||
#
|
||||
# Copyright (C) 2018 Mark Jessop <vk5qi@rfhead.net>
|
||||
# Released under GNU GPL v3 or later
|
||||
#
|
||||
import math
|
||||
import traceback
|
||||
import logging
|
||||
import numpy as np
|
||||
from .utils import position_info
|
||||
|
||||
|
||||
def getDensity(altitude):
|
||||
'''
|
||||
Calculate the atmospheric density for a given altitude in metres.
|
||||
This is a direct port of the oziplotter Atmosphere class
|
||||
'''
|
||||
|
||||
#Constants
|
||||
airMolWeight = 28.9644 # Molecular weight of air
|
||||
densitySL = 1.225 # Density at sea level [kg/m3]
|
||||
pressureSL = 101325 # Pressure at sea level [Pa]
|
||||
temperatureSL = 288.15 # Temperature at sea level [deg K]
|
||||
gamma = 1.4
|
||||
gravity = 9.80665 # Acceleration of gravity [m/s2]
|
||||
tempGrad = -0.0065 # Temperature gradient [deg K/m]
|
||||
RGas = 8.31432 # Gas constant [kg/Mol/K]
|
||||
R = 287.053
|
||||
deltaTemperature = 0.0;
|
||||
|
||||
# Lookup Tables
|
||||
altitudes = [0, 11000, 20000, 32000, 47000, 51000, 71000, 84852]
|
||||
pressureRels = [1, 2.23361105092158e-1, 5.403295010784876e-2, 8.566678359291667e-3, 1.0945601337771144e-3, 6.606353132858367e-4, 3.904683373343926e-5, 3.6850095235747942e-6]
|
||||
temperatures = [288.15, 216.65, 216.65, 228.65, 270.65, 270.65, 214.65, 186.946]
|
||||
tempGrads = [-6.5, 0, 1, 2.8, 0, -2.8, -2, 0]
|
||||
gMR = gravity * airMolWeight / RGas;
|
||||
|
||||
# Pick a region to work in
|
||||
i = 0
|
||||
if(altitude > 0):
|
||||
while (altitude > altitudes[i+1]):
|
||||
i = i + 1
|
||||
|
||||
|
||||
# Lookup based on region
|
||||
baseTemp = temperatures[i]
|
||||
tempGrad = tempGrads[i] / 1000.0
|
||||
pressureRelBase = pressureRels[i]
|
||||
deltaAltitude = altitude - altitudes[i]
|
||||
temperature = baseTemp + tempGrad * deltaAltitude
|
||||
|
||||
# Calculate relative pressure
|
||||
if(math.fabs(tempGrad) < 1e-10):
|
||||
pressureRel = pressureRelBase * math.exp(-1 *gMR * deltaAltitude / 1000.0 / baseTemp)
|
||||
else:
|
||||
pressureRel = pressureRelBase * math.pow(baseTemp / temperature, gMR / tempGrad / 1000.0)
|
||||
|
||||
|
||||
# Add temperature offset
|
||||
temperature = temperature + deltaTemperature
|
||||
|
||||
# Finally, work out the density...
|
||||
speedOfSound = math.sqrt(gamma * R * temperature)
|
||||
pressure = pressureRel * pressureSL
|
||||
density = densitySL * pressureRel * temperatureSL / temperature
|
||||
|
||||
return density
|
||||
|
||||
|
||||
def seaLevelDescentRate(descent_rate, altitude):
|
||||
''' Calculate the descent rate at sea level, for a given descent rate at altitude '''
|
||||
|
||||
rho = getDensity(altitude)
|
||||
return math.sqrt((rho / 1.22) * math.pow(descent_rate, 2))
|
||||
|
||||
|
||||
|
||||
def time_to_landing(current_altitude, current_descent_rate=-5.0, ground_asl=0.0, step_size=1):
|
||||
''' Calculate an estimated time to landing (in seconds) of a payload, based on its current altitude and descent rate '''
|
||||
|
||||
# A few checks on the input data.
|
||||
if current_descent_rate > 0.0:
|
||||
# If we are still ascending, return none.
|
||||
return None
|
||||
|
||||
if current_altitude <= ground_asl:
|
||||
# If the current altitude is *below* ground level, we have landed.
|
||||
return 0
|
||||
|
||||
# Calculate the sea level descent rate.
|
||||
_desc_rate = math.fabs(seaLevelDescentRate(current_descent_rate, current_altitude))
|
||||
_drag_coeff = _desc_rate*1.1045 # Magic multiplier from predict.php
|
||||
|
||||
|
||||
_alt = current_altitude
|
||||
_start_time = 0
|
||||
# Now step through the flight in <step_size> second steps.
|
||||
# Once the altitude is below our ground level, stop, and return the elapsed time.
|
||||
while _alt >= ground_asl:
|
||||
_alt += step_size * -1*(_drag_coeff/math.sqrt(getDensity(_alt)))
|
||||
_start_time += step_size
|
||||
|
||||
|
||||
return _start_time
|
||||
|
||||
class GenericTrack(object):
|
||||
"""
|
||||
A Generic 'track' object, which stores track positions for a payload or chase car.
|
||||
Telemetry is added using the add_telemetry method, which takes a dictionary with time/lat/lon/alt keys (at minimum).
|
||||
This object performs a running average of the ascent/descent rate, and calculates the predicted landing rate if the payload
|
||||
is in descent.
|
||||
The track history can be exported to a LineString using the to_line_string method.
|
||||
"""
|
||||
|
||||
def __init__(self,
|
||||
ascent_averaging = 6,
|
||||
landing_rate = 5.0):
|
||||
''' Create a GenericTrack Object. '''
|
||||
|
||||
# Averaging rate.
|
||||
self.ASCENT_AVERAGING = ascent_averaging
|
||||
# Payload state.
|
||||
self.landing_rate = landing_rate
|
||||
self.ascent_rate = 0.0
|
||||
self.heading = 0.0
|
||||
self.speed = 0.0
|
||||
self.is_descending = False
|
||||
|
||||
# Internal store of track history data.
|
||||
# Data is stored as a list-of-lists, with elements of [datetime, lat, lon, alt, comment]
|
||||
self.track_history = []
|
||||
|
||||
|
||||
def add_telemetry(self,data_dict):
|
||||
'''
|
||||
Accept telemetry data as a dictionary with fields
|
||||
datetime, lat, lon, alt, comment
|
||||
'''
|
||||
|
||||
try:
|
||||
_datetime = data_dict['time']
|
||||
_lat = data_dict['lat']
|
||||
_lon = data_dict['lon']
|
||||
_alt = data_dict['alt']
|
||||
if 'comment' in data_dict.keys():
|
||||
_comment = data_dict['comment']
|
||||
else:
|
||||
_comment = ""
|
||||
|
||||
self.track_history.append([_datetime, _lat, _lon, _alt, _comment])
|
||||
self.update_states()
|
||||
return self.get_latest_state()
|
||||
except:
|
||||
#logging.error("Web - Error adding new telemetry to GenericTrack %s" % traceback.format_exc())
|
||||
pass
|
||||
|
||||
|
||||
def get_latest_state(self):
|
||||
''' Get the latest position of the payload '''
|
||||
|
||||
if len(self.track_history) == 0:
|
||||
return None
|
||||
else:
|
||||
_latest_position = self.track_history[-1]
|
||||
_state = {
|
||||
'time' : _latest_position[0],
|
||||
'lat' : _latest_position[1],
|
||||
'lon' : _latest_position[2],
|
||||
'alt' : _latest_position[3],
|
||||
'ascent_rate': self.ascent_rate,
|
||||
'is_descending': self.is_descending,
|
||||
'landing_rate': self.landing_rate,
|
||||
'heading': self.heading,
|
||||
'speed': self.speed
|
||||
}
|
||||
return _state
|
||||
|
||||
|
||||
def calculate_ascent_rate(self):
|
||||
''' Calculate the ascent/descent rate of the payload based on the available data '''
|
||||
if len(self.track_history) <= 1:
|
||||
return 0.0
|
||||
elif len(self.track_history) == 2:
|
||||
# Basic ascent rate case - only 2 samples.
|
||||
_time_delta = (self.track_history[-1][0] - self.track_history[-2][0]).total_seconds()
|
||||
_altitude_delta = self.track_history[-1][3] - self.track_history[-2][3]
|
||||
|
||||
return _altitude_delta/_time_delta
|
||||
|
||||
|
||||
else:
|
||||
_num_samples = min(len(self.track_history), self.ASCENT_AVERAGING)
|
||||
_asc_rates = []
|
||||
|
||||
for _i in range(-1*(_num_samples-1), 0):
|
||||
_time_delta = (self.track_history[_i][0] - self.track_history[_i-1][0]).total_seconds()
|
||||
_altitude_delta = self.track_history[_i][3] - self.track_history[_i-1][3]
|
||||
_asc_rates.append(_altitude_delta/_time_delta)
|
||||
|
||||
return np.mean(_asc_rates)
|
||||
|
||||
def calculate_heading(self):
|
||||
''' Calculate the heading of the payload '''
|
||||
if len(self.track_history) <= 1:
|
||||
return 0.0
|
||||
else:
|
||||
_pos_1 = self.track_history[-2]
|
||||
_pos_2 = self.track_history[-1]
|
||||
|
||||
_pos_info = position_info((_pos_1[1],_pos_1[2],_pos_1[3]), (_pos_2[1],_pos_2[2],_pos_2[3]))
|
||||
|
||||
return _pos_info['bearing']
|
||||
|
||||
def calculate_speed(self):
|
||||
""" Calculate Payload Speed in metres per second """
|
||||
if len(self.track_history)<=1:
|
||||
return 0.0
|
||||
else:
|
||||
_time_delta = (self.track_history[-1][0] - self.track_history[-2][0]).total_seconds()
|
||||
_pos_1 = self.track_history[-2]
|
||||
_pos_2 = self.track_history[-1]
|
||||
|
||||
_pos_info = position_info((_pos_1[1],_pos_1[2],_pos_1[3]), (_pos_2[1],_pos_2[2],_pos_2[3]))
|
||||
|
||||
_speed = _pos_info['great_circle_distance']/_time_delta
|
||||
|
||||
return _speed
|
||||
|
||||
|
||||
def update_states(self):
|
||||
''' Update internal states based on the current data '''
|
||||
self.ascent_rate = self.calculate_ascent_rate()
|
||||
self.heading = self.calculate_heading()
|
||||
self.speed = self.calculate_speed()
|
||||
self.is_descending = self.ascent_rate < 0.0
|
||||
|
||||
if self.is_descending:
|
||||
_current_alt = self.track_history[-1][3]
|
||||
self.landing_rate = seaLevelDescentRate(self.ascent_rate, _current_alt)
|
||||
|
||||
|
||||
def to_polyline(self):
|
||||
''' Generate and return a Leaflet PolyLine compatible array '''
|
||||
# Copy array into a numpy representation for easier slicing.
|
||||
if len(self.track_history) == 0:
|
||||
return []
|
||||
elif len(self.track_history) == 1:
|
||||
# LineStrings need at least 2 points. If we only have a single point,
|
||||
# fudge it by duplicating the single point.
|
||||
_track_data_np = np.array([self.track_history[0], self.track_history[0]])
|
||||
else:
|
||||
_track_data_np = np.array(self.track_history)
|
||||
# Produce new array
|
||||
_track_points = np.column_stack((_track_data_np[:,1], _track_data_np[:,2], _track_data_np[:,3]))
|
||||
|
||||
return _track_points.tolist()
|
||||
|
|
@ -326,6 +326,7 @@
|
|||
return
|
||||
}
|
||||
|
||||
|
||||
// Have we seen this sonde before?
|
||||
if (sonde_positions.hasOwnProperty(msg.id) == false){
|
||||
// Nope, add a property to the sonde_positions object, and setup markers for the sonde.
|
||||
|
|
|
|||
|
|
@ -17,6 +17,8 @@ import subprocess
|
|||
import threading
|
||||
import time
|
||||
import numpy as np
|
||||
from dateutil.parser import parse
|
||||
from datetime import datetime, timedelta
|
||||
from math import radians, degrees, sin, cos, atan2, sqrt, pi
|
||||
from . import __version__ as auto_rx_version
|
||||
try:
|
||||
|
|
@ -738,7 +740,6 @@ def peak_decimation(freq, power, factor):
|
|||
return (_freq_out, _power_out)
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
import sys
|
||||
logging.basicConfig(format='%(asctime)s %(levelname)s:%(message)s', level=logging.DEBUG)
|
||||
|
|
|
|||
|
|
@ -5,6 +5,7 @@
|
|||
# Copyright (C) 2018 Mark Jessop <vk5qi@rfhead.net>
|
||||
# Released under MIT License
|
||||
#
|
||||
import copy
|
||||
import datetime
|
||||
import json
|
||||
import logging
|
||||
|
|
@ -15,6 +16,7 @@ import traceback
|
|||
import autorx
|
||||
import autorx.config
|
||||
import autorx.scan
|
||||
from autorx.geometry import GenericTrack
|
||||
from threading import Thread
|
||||
import flask
|
||||
from flask import request, abort
|
||||
|
|
@ -45,6 +47,7 @@ socketio = SocketIO(app, async_mode='threading')
|
|||
# 'latest_timestamp': timestamp (unix timestamp) of when the last packet was received.
|
||||
# 'latest_telem': telemetry dictionary.
|
||||
# 'path': list of [lat,lon,alt] pairs
|
||||
# 'track': A GenericTrack object, which is used to determine the current ascent/descent rate.
|
||||
#
|
||||
flask_telemetry_store = {}
|
||||
|
||||
|
|
@ -117,7 +120,12 @@ def flask_get_scan_data():
|
|||
@app.route("/get_telemetry_archive")
|
||||
def flask_get_telemetry_archive():
|
||||
""" Return a copy of the telemetry archive """
|
||||
return json.dumps(flask_telemetry_store)
|
||||
# Make a copy of the store, and remove the non-serialisable GenericTrack object
|
||||
_temp_store = copy.deepcopy(flask_telemetry_store)
|
||||
for _element in _temp_store:
|
||||
_temp_store[_element].pop('track')
|
||||
|
||||
return json.dumps(_temp_store)
|
||||
|
||||
|
||||
@app.route("/shutdown/<shutdown_key>")
|
||||
|
|
@ -292,21 +300,32 @@ class WebExporter(object):
|
|||
return
|
||||
|
||||
_telem = telemetry.copy()
|
||||
|
||||
# Add the telemetry information to the global telemetry store
|
||||
if _telem['id'] not in flask_telemetry_store:
|
||||
flask_telemetry_store[_telem['id']] = {'timestamp':time.time(), 'latest_telem':_telem, 'path':[], 'track': GenericTrack()}
|
||||
|
||||
flask_telemetry_store[_telem['id']]['path'].append([_telem['lat'],_telem['lon'],_telem['alt']])
|
||||
flask_telemetry_store[_telem['id']]['latest_telem'] = _telem
|
||||
flask_telemetry_store[_telem['id']]['timestamp'] = time.time()
|
||||
|
||||
# Update the sonde's track and extract the current state.
|
||||
flask_telemetry_store[_telem['id']]['track'].add_telemetry({'time': _telem['datetime_dt'], 'lat':_telem['lat'], 'lon': _telem['lon'], 'alt':_telem['alt']})
|
||||
_telem_state = flask_telemetry_store[_telem['id']]['track'].get_latest_state()
|
||||
|
||||
# Add the calculated vertical and horizontal velocity, and heading to the telemetry dict.
|
||||
_telem['vel_v'] = _telem_state['ascent_rate']
|
||||
_telem['vel_h'] = _telem_state['speed']
|
||||
_telem['heading'] = _telem_state['heading']
|
||||
|
||||
# Remove the datetime object that is part of the telemetry, if it exists.
|
||||
# (it might not be present in test data)
|
||||
if 'datetime_dt' in _telem:
|
||||
_telem.pop('datetime_dt')
|
||||
|
||||
# Pass it on to the client.
|
||||
socketio.emit('telemetry_event', _telem, namespace='/update_status')
|
||||
|
||||
# Add the telemetry information to the global telemetry store
|
||||
if _telem['id'] not in flask_telemetry_store:
|
||||
flask_telemetry_store[_telem['id']] = {'timestamp':time.time(), 'latest_telem':_telem, 'path':[]}
|
||||
|
||||
flask_telemetry_store[_telem['id']]['path'].append([_telem['lat'],_telem['lon'],_telem['alt']])
|
||||
flask_telemetry_store[_telem['id']]['latest_telem'] = _telem
|
||||
flask_telemetry_store[_telem['id']]['timestamp'] = time.time()
|
||||
|
||||
|
||||
def clean_telemetry_store(self):
|
||||
""" Remove any old data from the telemetry store """
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue