122 lines
3.3 KiB
Python
122 lines
3.3 KiB
Python
|
import json
|
||
|
|
import math
|
||
|
|
import re
|
||
|
|
|
||
|
|
|
||
|
|
def calculate_distance(point1: dict, point2: dict) -> float:
|
||
|
|
"""
|
||
|
|
Calculate the distance in meters between two geographic points with altitude.
|
||
|
|
|
||
|
|
:param point1: A dictionary with 'latitude', 'longitude', and optional 'altitude'.
|
||
|
|
:param point2: A dictionary with 'latitude', 'longitude', and optional 'altitude'.
|
||
|
|
:return: Distance in meters as a float.
|
||
|
|
"""
|
||
|
|
try:
|
||
|
|
# Extract latitude, longitude, and altitude
|
||
|
|
lat1, lon1, alt1 = (
|
||
|
|
point1["latitude"],
|
||
|
|
point1["longitude"],
|
||
|
|
point1.get("altitude", 0),
|
||
|
|
)
|
||
|
|
lat2, lon2, alt2 = (
|
||
|
|
point2["latitude"],
|
||
|
|
point2["longitude"],
|
||
|
|
point2.get("altitude", 0),
|
||
|
|
)
|
||
|
|
except KeyError:
|
||
|
|
return "Points were not properly set"
|
||
|
|
|
||
|
|
# Convert latitude and longitude from degrees to radians
|
||
|
|
lat1, lon1, lat2, lon2 = map(math.radians, [lat1, lon1, lat2, lon2])
|
||
|
|
|
||
|
|
# Radius of the Earth in meters
|
||
|
|
R = 6369500
|
||
|
|
|
||
|
|
# Compute deltas
|
||
|
|
delta_lat = lat2 - lat1
|
||
|
|
delta_lon = lon2 - lon1
|
||
|
|
|
||
|
|
# Haversine formula for horizontal distance
|
||
|
|
a = (
|
||
|
|
math.sin(delta_lat / 2) ** 2
|
||
|
|
+ math.cos(lat1) * math.cos(lat2) * math.sin(delta_lon / 2) ** 2
|
||
|
|
)
|
||
|
|
c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
|
||
|
|
horizontal_distance = R * c
|
||
|
|
|
||
|
|
# Altitude difference
|
||
|
|
delta_alt = alt2 - alt1
|
||
|
|
|
||
|
|
# Total 3D distance
|
||
|
|
distance = math.sqrt(horizontal_distance**2 + delta_alt**2)
|
||
|
|
|
||
|
|
return distance
|
||
|
|
|
||
|
|
|
||
|
|
# Split string by commas, if there are commas
|
||
|
|
def comma_split(string: str):
|
||
|
|
if "," in string:
|
||
|
|
return string.split(",")
|
||
|
|
else:
|
||
|
|
return string
|
||
|
|
|
||
|
|
|
||
|
|
# Extract stats from strings
|
||
|
|
def extract_numbers(string):
|
||
|
|
pattern = r"-?\d+\.\d+|-?\d+"
|
||
|
|
numbers = re.findall(pattern, string)
|
||
|
|
numbers = [float(num) if "." in num else int(num) for num in numbers]
|
||
|
|
return numbers
|
||
|
|
|
||
|
|
|
||
|
|
# Parse the latitude or longitude from the NMEA format
|
||
|
|
def parse_lat_lon(value: str, direction: str) -> float:
|
||
|
|
"""
|
||
|
|
Parses latitude or longitude from NMEA format.
|
||
|
|
|
||
|
|
Args:
|
||
|
|
value (str): The raw NMEA latitude or longitude value.
|
||
|
|
direction (str): Direction indicator ('N', 'S', 'E', 'W').
|
||
|
|
|
||
|
|
Returns:
|
||
|
|
float: The parsed latitude or longitude as a decimal degree.
|
||
|
|
"""
|
||
|
|
if not value or not direction:
|
||
|
|
return None
|
||
|
|
degrees = int(value[:2])
|
||
|
|
minutes = float(value[2:])
|
||
|
|
decimal = degrees + minutes / 60
|
||
|
|
if direction in ["S", "W"]:
|
||
|
|
decimal = -decimal
|
||
|
|
return decimal
|
||
|
|
|
||
|
|
|
||
|
|
# Convert RSSI to DBM
|
||
|
|
def rssi_to_dbm(rssi):
|
||
|
|
if rssi == 99:
|
||
|
|
return "Unknown"
|
||
|
|
else:
|
||
|
|
# Convert RSSI to dBm
|
||
|
|
return -113 + 2 * rssi
|
||
|
|
|
||
|
|
|
||
|
|
# Save data to JSON file
|
||
|
|
def save_data_to_json(data, filename):
|
||
|
|
try:
|
||
|
|
# Load existing data
|
||
|
|
try:
|
||
|
|
with open(filename, "r") as file:
|
||
|
|
existing_data = json.load(file)
|
||
|
|
except FileNotFoundError:
|
||
|
|
# If file doesn't exist, start with an empty list
|
||
|
|
existing_data = []
|
||
|
|
|
||
|
|
# Append new data
|
||
|
|
existing_data.append(data)
|
||
|
|
|
||
|
|
# Save updated data back to the file
|
||
|
|
with open(filename, "w") as file:
|
||
|
|
json.dump(existing_data, file, indent=4)
|
||
|
|
except Exception as e:
|
||
|
|
print(f"Error saving data to JSON: {e}")
|