Spaces:
Sleeping
Sleeping
| import math | |
| import numpy as np | |
| from datetime import datetime, timezone, timedelta | |
| speedOfLight_c = 299792458.0 | |
| uplinkFreq = 1646652500 | |
| downlinkFreq = 3615152500 | |
| btoBias = -495679 | |
| BFO_bias = 150 | |
| elip_a = 6378137.0 | |
| elip_f = 1.0 / 298.257223563 | |
| elip_b = elip_a * (1.0 - elip_f) | |
| elip_e2 = (2 * elip_f) - (elip_f * elip_f) | |
| def degToRad(d): | |
| return d * math.pi / 180.0 | |
| def radToDeg(r): | |
| return r * 180.0 / math.pi | |
| def feetToM(ft): | |
| return ft * 0.3048 | |
| satLoc = ( | |
| (("18:25:00"), (18136.7, 38071.8, 1148.5), (0.00188, -0.00117, 0.02690), 11, 142, 12520), | |
| (("18:40:00"), (18138.21, 38070.79, 1170.29), (0.001892134, -0.001142665, 0.02137832), 8, 88, None), | |
| (("19:40:00"), (18145.1, 38067.0, 1206.3), (0.00189, -0.00092, -0.00148), -1, 111, 11500), | |
| (("20:40:00"), (18152.1, 38064.0, 1159.7), (0.00200, -0.00077, -0.02422), -1, 141, 11740), | |
| (("21:40:00"), (18159.5, 38061.3, 1033.8), (0.00212, -0.00076, -0.04531), -18, 168, 12780), | |
| (("22:40:00"), (18167.2, 38058.3, 837.2), (0.00211, -0.00096, -0.06331), -29, 204, 14540), | |
| (("24:10:00"), (18177.5, 38051.7, 440.0), (0.00160, -0.00151, -0.08188), -38, 252, 18040), | |
| (("24:20:00"), (18178.4, 38050.8, 390.5), (0.00150, -0.00158, -0.08321), -38, 182, 18400) | |
| ) | |
| gsPerth = (-2368.8, 4881.1, -3342.0) | |
| nominalSatLoc = (0, 64.5, 36000000) | |
| def N_phi(lat_rad): | |
| sinp = math.sin(lat_rad) | |
| return elip_a / math.sqrt(1.0 - elip_e2 * sinp * sinp) | |
| def latLonToECEF(lat_deg, lon_deg, h_m): | |
| lat = degToRad(lat_deg) | |
| lon = degToRad(lon_deg) | |
| Np = N_phi(lat) | |
| x = (Np + h_m) * math.cos(lat) * math.cos(lon) | |
| y = (Np + h_m) * math.cos(lat) * math.sin(lon) | |
| z = (Np * (1 - elip_e2) + h_m) * math.sin(lat) | |
| return np.array([x, y, z], dtype=float) | |
| def distBetECEF(p1, p2): | |
| d = p1 - p2 | |
| return float(np.sqrt(np.dot(d, d))) | |
| def ENU2ECEFvelo(vE, vN, vU, lat_rad, lon_rad): | |
| sL, cL = (math.sin(lat_rad), math.cos(lat_rad)) | |
| sO, cO = (math.sin(lon_rad), math.cos(lon_rad)) | |
| R = np.array([[-sO, -sL * cO, cL * cO], [cO, -sL * sO, cL * sO], [0.0, cL, sL]], dtype=float) | |
| return R @ np.array([vE, vN, vU], dtype=float) | |
| def toECEFunitVector(p_from, p_to): | |
| v = p_to - p_from | |
| n = np.linalg.norm(v) | |
| return v / n if n != 0 else v | |
| def rhumb_direct(start_latlon, bearing_deg, distance_m): | |
| lat1 = degToRad(start_latlon[0]) | |
| lon1 = degToRad(start_latlon[1]) | |
| brg = degToRad(bearing_deg) | |
| R = elip_a | |
| d = distance_m / R | |
| dphi = d * math.cos(brg) | |
| lat2 = lat1 + dphi | |
| if abs(lat2) > math.pi / 2 - 1e-12: | |
| lat2 = math.copysign(math.pi / 2 - 1e-12, lat2) | |
| dpsi = math.log(math.tan(lat2 / 2 + math.pi / 4) / math.tan(lat1 / 2 + math.pi / 4)) | |
| q = dphi / dpsi if abs(dpsi) > 1e-12 else math.cos(lat1) | |
| dlon = d * math.sin(brg) / q | |
| lon2 = lon1 + dlon | |
| lon2 = (lon2 + math.pi) % (2 * math.pi) - math.pi | |
| return (radToDeg(lat2), radToDeg(lon2)) | |
| def vincenty_direct(latLon, alpha1_deg, s): | |
| lat1 = degToRad(latLon[0]) | |
| lon1 = degToRad(latLon[1]) | |
| alpha1 = degToRad(alpha1_deg) | |
| U1 = math.atan((1 - elip_f) * math.tan(lat1)) | |
| sigma1 = math.atan2(math.tan(U1), math.cos(alpha1)) | |
| sin_alpha = math.cos(U1) * math.sin(alpha1) | |
| cos2_alpha = 1 - sin_alpha ** 2 | |
| elip_b = elip_a * (1.0 - elip_f) | |
| u2 = cos2_alpha * (elip_a ** 2 - elip_b ** 2) / elip_b ** 2 | |
| A = 1 + u2 / 16384 * (4096 + u2 * (-768 + u2 * (320 - 175 * u2))) | |
| B = u2 / 1024 * (256 + u2 * (-128 + u2 * (74 - 47 * u2))) | |
| sigma = s / (elip_b * A) | |
| tol = 1e-12 | |
| for _ in range(200): | |
| cos2sigma_m = math.cos(2 * sigma1 + sigma) | |
| sin_sigma = math.sin(sigma) | |
| cos_sigma = math.cos(sigma) | |
| delta_sigma = B * sin_sigma * (cos2sigma_m + B / 4 * (cos_sigma * (-1 + 2 * cos2sigma_m ** 2) - B / 6 * cos2sigma_m * (-3 + 4 * sin_sigma ** 2) * (-3 + 4 * cos2sigma_m ** 2))) | |
| sigma_new = s / (elip_b * A) + delta_sigma | |
| if abs(sigma_new - sigma) < tol: | |
| sigma = sigma_new | |
| break | |
| sigma = sigma_new | |
| lat2 = math.atan2(math.sin(U1) * cos_sigma + math.cos(U1) * sin_sigma * math.cos(alpha1), (1 - elip_f) * math.sqrt(sin_alpha ** 2 + (math.sin(U1) * sin_sigma - math.cos(U1) * cos_sigma * math.cos(alpha1)) ** 2)) | |
| lam = math.atan2(sin_sigma * math.sin(alpha1), math.cos(U1) * cos_sigma - math.sin(U1) * sin_sigma * math.cos(alpha1)) | |
| C = elip_f / 16 * cos2_alpha * (4 + elip_f * (4 - 3 * cos2_alpha)) | |
| L = lam - (1 - C) * elip_f * sin_alpha * (sigma + C * sin_sigma * (cos2sigma_m + C * cos_sigma * (-1 + 2 * cos2sigma_m ** 2))) | |
| lon2 = lon1 + L | |
| alpha2 = math.atan2(sin_alpha, -math.sin(U1) * sin_sigma + math.cos(U1) * cos_sigma * math.cos(alpha1)) | |
| return ([radToDeg(lat2), radToDeg(lon2)], (radToDeg(alpha2) + 360) % 360) | |
| SIDEREAL_DAY_S = 86164.0 | |
| OMEGA = 2.0 * math.pi / SIDEREAL_DAY_S | |
| SAT_EPOCH_UTC = datetime(2014, 3, 7, 16, 30, 0, tzinfo=timezone.utc) | |
| from datetime import datetime, timedelta, timezone | |
| def _safe_float(x, default=0.0): | |
| try: | |
| return float(x) | |
| except (TypeError, ValueError): | |
| return default | |
| def _extract_time_string(label_field): | |
| if isinstance(label_field, (tuple, list)): | |
| label = label_field[0] | |
| if isinstance(label, (tuple, list)): | |
| return str(label[0]) | |
| return str(label) | |
| return str(label_field) | |
| def _time_label_to_dt(label: str, epoch: datetime) -> datetime: | |
| hh, mm, ss = map(int, label.split(":")) | |
| day_offset = hh // 24 | |
| hh = hh % 24 | |
| dt = datetime(epoch.year, epoch.month, epoch.day, hh, mm, ss, tzinfo=epoch.tzinfo) | |
| if day_offset: | |
| dt += timedelta(days=day_offset) | |
| return dt | |
| def _extract_time_string(tfield): | |
| if isinstance(tfield, (tuple, list)): | |
| return str(tfield[0]) | |
| return str(tfield) | |
| def _build_sat_time_seconds(): | |
| times_sec = [] | |
| for entry in satLoc: | |
| tstr = _extract_time_string(entry[0]) | |
| try: | |
| h, m, s = map(int, tstr.split(':')) | |
| except Exception: | |
| continue | |
| add_day = 0 | |
| if h == 24: | |
| h = 0 | |
| add_day = 1 | |
| try: | |
| dt = datetime(SAT_EPOCH_UTC.year, SAT_EPOCH_UTC.month, SAT_EPOCH_UTC.day, h, m, s, tzinfo=timezone.utc) | |
| except ValueError: | |
| continue | |
| sec = (dt - SAT_EPOCH_UTC).total_seconds() + add_day * 86400.0 | |
| times_sec.append(sec) | |
| return np.array(times_sec, dtype=float) | |
| _sat_time_seconds = _build_sat_time_seconds() | |
| def seconds_since_sat_epoch(t_utc): | |
| return (t_utc - SAT_EPOCH_UTC).total_seconds() | |
| def calculateBTO_at_time(acLat, acLon, acAlt, time_utc): | |
| """ | |
| Compute BTO (microseconds) using the *same* satellite geometry as BFO: | |
| nearest satLoc entry to time_utc, with positions in km converted to meters. | |
| """ | |
| # Aircraft ECEF (meters) | |
| acECEF = latLonToECEF(acLat, acLon, acAlt) | |
| # Nearest satLoc satellite ECEF (meters) | |
| idx_nearest = nearest_measured_for_time(time_utc)[0] | |
| satECEF = 1000.0 * np.array(satLoc[idx_nearest][1], float) | |
| # Perth ground station ECEF (meters) | |
| gsECEF = 1000.0 * np.array(gsPerth, float) | |
| # One-way distances (meters) | |
| d1 = distBetECEF(acECEF, satECEF) # AC -> SAT | |
| d2 = distBetECEF(satECEF, gsECEF) # SAT -> GS | |
| # Round-trip signal time (microseconds), plus constant system bias | |
| bto_us = (2.0 * (d1 + d2) / speedOfLight_c) * 1e6 + btoBias | |
| return bto_us | |
| def calculateBFO_at_time(flightV_mps, track_deg, acLat, acLon, acAlt, acVS_mps, time_utc): | |
| """ | |
| Predict BFO using the satellite state from satLoc that is NEAREST to time_utc. | |
| Includes δF_sat + δF_AFC from satLoc[idx][3] and BFO_bias. | |
| """ | |
| # --- aircraft velocity in ENU and ECEF --- | |
| vE = flightV_mps * math.sin(degToRad(track_deg)) | |
| vN = flightV_mps * math.cos(degToRad(track_deg)) | |
| vU = acVS_mps | |
| acECEF = latLonToECEF(acLat, acLon, acAlt) # meters | |
| vECEF = ENU2ECEFvelo(vE, vN, vU, degToRad(acLat), degToRad(acLon)) # m/s | |
| # --- find nearest satLoc entry for this UTC --- | |
| idx_nearest = nearest_measured_for_time(time_utc)[0] | |
| # --- satellite state from satLoc (convert km → m, km/s → m/s) --- | |
| sat_pos_km = np.array(satLoc[idx_nearest][1], float) | |
| sat_vel_km_s = np.array(satLoc[idx_nearest][2], float) | |
| satECEF = 1000.0 * sat_pos_km # m | |
| satV_ECEF = 1000.0 * sat_vel_km_s # m/s | |
| # --- ground station (Perth) ECEF in meters --- | |
| gsECEF = 1000.0 * np.array(gsPerth, float) | |
| # --- line-of-sight unit vectors --- | |
| u_ac_sat = toECEFunitVector(acECEF, satECEF) | |
| u_sat_gs = toECEFunitVector(satECEF, gsECEF) | |
| # --- relative LOS velocities --- | |
| v_rel_ac_sat = float(np.dot(vECEF - satV_ECEF, u_ac_sat)) # m/s | |
| v_rel_sat_gs = float(np.dot(satV_ECEF, u_sat_gs)) # m/s (GS static in ECEF) | |
| # --- Doppler terms --- | |
| Fup = uplinkFreq * (v_rel_ac_sat / speedOfLight_c) | |
| Fdown = downlinkFreq * (v_rel_sat_gs / speedOfLight_c) | |
| # --- aircraft frequency compensation toward nominal sat location --- | |
| nomECEF = latLonToECEF(nominalSatLoc[0], nominalSatLoc[1], nominalSatLoc[2]) | |
| u_ac_nom = toECEFunitVector(acECEF, nomECEF) | |
| v_rel_ac_nom = float(np.dot(vECEF, u_ac_nom)) | |
| FcompAC = uplinkFreq * (v_rel_ac_nom / speedOfLight_c) | |
| # --- δF_sat + δF_AFC from satLoc (index 3), safe-cast in case of None --- | |
| fsat_afc = float(satLoc[idx_nearest][3]) if satLoc[idx_nearest][3] is not None else 0.0 | |
| # --- final BFO --- | |
| return Fup + Fdown - FcompAC + fsat_afc + BFO_bias | |
| def nearest_measured_for_time(t_utc): | |
| """ | |
| Return (idx, label_str, meas_bfo_hz, meas_bto_us) for the satLoc entry whose time label | |
| is nearest to t_utc. Guard against None in measured fields. | |
| """ | |
| times = [] | |
| for i, row in enumerate(satLoc): | |
| label_str = _extract_time_string(row[0]) | |
| try: | |
| dt = _time_label_to_dt(label_str, SAT_EPOCH_UTC) | |
| except Exception: | |
| continue | |
| times.append((i, dt, label_str)) | |
| if not times: | |
| raise RuntimeError("No valid satLoc times to compare.") | |
| idx, dt_near, label = min(times, key=lambda it: abs((t_utc - it[1]).total_seconds())) | |
| meas_bfo = _safe_float(satLoc[idx][4], float("nan")) | |
| meas_bto = _safe_float(satLoc[idx][5], float("nan")) | |
| return (idx, label, meas_bfo, meas_bto) | |
| def nearest_measured_with_bto_for_time(t_utc): | |
| """Like nearest_measured_for_time, but guarantees meas_bto is not None if possible.""" | |
| best = None | |
| best_dt = None | |
| for i, row in enumerate(satLoc): | |
| # row[0] is a time label like "19:40:00" | |
| row_dt = SAT_EPOCH_UTC + timedelta(seconds=_sat_time_seconds[i]) | |
| dt = abs((t_utc - row_dt).total_seconds()) | |
| meas_bto = row[5] | |
| if meas_bto is None: | |
| continue | |
| if best is None or dt < best_dt: | |
| best = (i, row[0], row[4], meas_bto) | |
| best_dt = dt | |
| if best is not None: | |
| return best | |
| # Fall back: return nearest even if BTO is None | |
| idx, meas_time_str, meas_bfo, meas_bto = nearest_measured_for_time(t_utc) | |
| return (idx, meas_time_str, meas_bfo, meas_bto) | |
| def auto_tune_heading_to_bto(current_lat, current_lon, alt_m, current_time, base_heading_deg, | |
| speed_knots, leg_minutes, model_sel, search_half_width_deg=5.0, | |
| heading_step_deg=0.1): | |
| """Search around base_heading_deg to minimize |measured_bto - calculated_bto| at leg end. | |
| Returns dict with best_heading, best_lat, best_lon, best_time, best_bto, best_delta_bto, meas_time_str. | |
| """ | |
| speed_mps = speed_knots * 0.514444 | |
| distance_m = speed_mps * (leg_minutes * 60.0) | |
| target_time = current_time + timedelta(minutes=leg_minutes) | |
| # get measurement reference (prefer one with non-None BTO) | |
| meas_idx, meas_time_str, meas_bfo, meas_bto = nearest_measured_with_bto_for_time(target_time) | |
| # If measurement BTO is still None (should be rare), we cannot tune. | |
| if meas_bto is None: | |
| return { | |
| "ok": False, | |
| "reason": "No measured BTO available near target time", | |
| "best_heading": base_heading_deg, | |
| "meas_time_str": meas_time_str, | |
| } | |
| def wrap(h): | |
| h = h % 360.0 | |
| return h + 360.0 if h < 0 else h | |
| best = None | |
| # number of steps on each side | |
| n = int(round(search_half_width_deg / heading_step_deg)) | |
| for k in range(-n, n + 1): | |
| hdg = wrap(base_heading_deg + k * heading_step_deg) | |
| if model_sel == '1': | |
| (lat2, lon2), _ = vincenty_direct((current_lat, current_lon), hdg, distance_m) | |
| else: | |
| lat2, lon2 = rhumb_direct((current_lat, current_lon), hdg, distance_m) | |
| bto_val = calculateBTO_at_time(lat2, lon2, alt_m, target_time) | |
| delta_bto = meas_bto - bto_val | |
| score = abs(delta_bto) | |
| if best is None or score < best["score"]: | |
| best = { | |
| "ok": True, | |
| "score": score, | |
| "best_heading": hdg, | |
| "best_lat": lat2, | |
| "best_lon": lon2, | |
| "best_time": target_time, | |
| "best_bto": bto_val, | |
| "best_delta_bto": delta_bto, | |
| "meas_time_str": meas_time_str, | |
| "meas_bto": meas_bto, | |
| } | |
| return best | |
| def auto_tune_heading_speed_to_bto(current_lat, current_lon, alt_m, current_time, | |
| base_heading_deg, base_speed_knots, leg_minutes, model_sel, | |
| heading_half_width_deg=8.0, heading_step_deg=0.2, | |
| speed_half_width_knots=80.0, speed_step_knots=2.0, | |
| refine_heading_half_width_deg=1.0, refine_heading_step_deg=0.05, | |
| refine_speed_half_width_knots=10.0, refine_speed_step_knots=0.5): | |
| """Search around (base_heading_deg, base_speed_knots) to minimize |ΔBTO| at leg end. | |
| Two-stage search: | |
| 1) coarse grid over heading±heading_half_width_deg and speed±speed_half_width_knots | |
| 2) refine around best using smaller steps | |
| Returns dict with best_heading, best_speed_knots, best_lat, best_lon, best_time, best_bto, | |
| best_delta_bto, meas_time_str, meas_bto. | |
| """ | |
| target_time = current_time + timedelta(minutes=leg_minutes) | |
| meas_idx, meas_time_str, meas_bfo, meas_bto = nearest_measured_with_bto_for_time(target_time) | |
| if meas_bto is None: | |
| return { | |
| "ok": False, | |
| "reason": "No measured BTO available near target time.", | |
| "best_heading": base_heading_deg, | |
| "best_speed_knots": base_speed_knots, | |
| "best_lat": None, | |
| "best_lon": None, | |
| "best_time": target_time, | |
| "best_bto": None, | |
| "best_delta_bto": None, | |
| "meas_time_str": meas_time_str, | |
| "meas_bto": None, | |
| } | |
| def eval_candidate(hdg_deg: float, spd_knots: float): | |
| speed_mps = spd_knots * 0.514444 | |
| distance_m = speed_mps * (leg_minutes * 60.0) | |
| if model_sel == '1': | |
| (latlon2, _az2) = vincenty_direct((current_lat, current_lon), hdg_deg, distance_m) | |
| lat2, lon2 = latlon2 | |
| else: | |
| lat2, lon2 = rhumb_direct((current_lat, current_lon), hdg_deg, distance_m) | |
| bto_val = calculateBTO_at_time(lat2, lon2, alt_m, target_time) | |
| delta_bto = meas_bto - bto_val | |
| return lat2, lon2, bto_val, delta_bto | |
| def wrap_heading(h): | |
| h = h % 360.0 | |
| return h + 360.0 if h < 0 else h | |
| best = None | |
| # ---- stage 1: coarse grid ---- | |
| h0 = base_heading_deg | |
| s0 = base_speed_knots | |
| h_min = h0 - heading_half_width_deg | |
| h_max = h0 + heading_half_width_deg | |
| s_min = max(0.0, s0 - speed_half_width_knots) | |
| s_max = s0 + speed_half_width_knots | |
| # Build grids (inclusive ends) | |
| headings = np.arange(h_min, h_max + 1e-9, heading_step_deg) | |
| speeds = np.arange(s_min, s_max + 1e-9, speed_step_knots) | |
| for spd in speeds: | |
| for hdg in headings: | |
| hdg_w = wrap_heading(float(hdg)) | |
| lat2, lon2, bto_val, delta_bto = eval_candidate(hdg_w, float(spd)) | |
| score = abs(delta_bto) | |
| if best is None or score < best["score"]: | |
| best = { | |
| "ok": True, | |
| "score": score, | |
| "best_heading": hdg_w, | |
| "best_speed_knots": float(spd), | |
| "best_lat": lat2, | |
| "best_lon": lon2, | |
| "best_time": target_time, | |
| "best_bto": bto_val, | |
| "best_delta_bto": delta_bto, | |
| "meas_time_str": meas_time_str, | |
| "meas_bto": meas_bto, | |
| } | |
| # ---- stage 2: refine around the best ---- | |
| if best is None: | |
| return { | |
| "ok": False, | |
| "reason": "Search failed.", | |
| "best_heading": base_heading_deg, | |
| "best_speed_knots": base_speed_knots, | |
| "best_lat": None, | |
| "best_lon": None, | |
| "best_time": target_time, | |
| "best_bto": None, | |
| "best_delta_bto": None, | |
| "meas_time_str": meas_time_str, | |
| "meas_bto": meas_bto, | |
| } | |
| h1 = best["best_heading"] | |
| s1 = best["best_speed_knots"] | |
| headings2 = np.arange(h1 - refine_heading_half_width_deg, h1 + refine_heading_half_width_deg + 1e-9, refine_heading_step_deg) | |
| speeds2 = np.arange(max(0.0, s1 - refine_speed_half_width_knots), s1 + refine_speed_half_width_knots + 1e-9, refine_speed_step_knots) | |
| for spd in speeds2: | |
| for hdg in headings2: | |
| hdg_w = wrap_heading(float(hdg)) | |
| lat2, lon2, bto_val, delta_bto = eval_candidate(hdg_w, float(spd)) | |
| score = abs(delta_bto) | |
| if score < best["score"]: | |
| best.update({ | |
| "score": score, | |
| "best_heading": hdg_w, | |
| "best_speed_knots": float(spd), | |
| "best_lat": lat2, | |
| "best_lon": lon2, | |
| "best_bto": bto_val, | |
| "best_delta_bto": delta_bto, | |
| }) | |
| return best | |
| def repl_fly_from_radar_fix(): | |
| print('=== MH370 REPL (radar fix) ===') | |
| print('Start: 2014-03-07 18:22:12Z @ 6.578°N, 96.341°E, FL350') | |
| current_lat = 6.578 | |
| current_lon = 96.341 | |
| current_alt_m = feetToM(35000) | |
| current_time = datetime(2014, 3, 7, 18, 22, 12, tzinfo=timezone.utc) | |
| path_points = [(current_lat, current_lon, current_time.isoformat())] | |
| while True: | |
| try: | |
| heading = float(input('HEADING (deg): ').strip()) | |
| speed_knots = float(input('SPEED (knots): ').strip()) | |
| model_sel = input('Enter 1 for VINCENTY or 2 for RHUMB: ').strip() | |
| model_sel = '1' if model_sel not in ('1', '2') else model_sel | |
| leg_str = input('Leg duration in minutes (default 10): ').strip() | |
| leg_minutes = float(leg_str) if leg_str else 10.0 | |
| except Exception as e: | |
| print('Invalid input, try again.', e) | |
| continue | |
| auto_sel = input('Auto-tune: (h) heading, (b) heading+speed, Enter = none: ').strip().lower() | |
| speed_mps = speed_knots * 0.514444 | |
| distance_m = speed_mps * (leg_minutes * 60.0) | |
| if auto_sel in ('b', 'both'): | |
| tuned = auto_tune_heading_speed_to_bto( | |
| current_lat=current_lat, | |
| current_lon=current_lon, | |
| alt_m=current_alt_m, | |
| current_time=current_time, | |
| base_heading_deg=heading, | |
| base_speed_knots=speed_knots, | |
| leg_minutes=leg_minutes, | |
| model_sel=model_sel, | |
| ) | |
| if tuned.get('ok'): | |
| heading = tuned['best_heading'] | |
| speed_knots = tuned['best_speed_knots'] | |
| # update distance for subsequent printing/metrics | |
| speed_mps = speed_knots * 0.514444 | |
| distance_m = speed_mps * (leg_minutes * 60.0) | |
| lat2 = tuned['best_lat'] | |
| lon2 = tuned['best_lon'] | |
| new_time = tuned['best_time'] | |
| print(f"[Auto-tune] Best heading: {heading:.2f}° | Best speed: {speed_knots:.2f} kt | Expected ΔBTO vs {tuned['meas_time_str']}: {tuned['best_delta_bto']:+.3f} µs") | |
| else: | |
| if model_sel == '1': | |
| (lat2, lon2), _ = vincenty_direct((current_lat, current_lon), heading, distance_m) | |
| else: | |
| lat2, lon2 = rhumb_direct((current_lat, current_lon), heading, distance_m) | |
| new_time = current_time + timedelta(minutes=leg_minutes) | |
| elif auto_sel in ('h', 'heading', 'y', 'yes'): | |
| tuned = auto_tune_heading_to_bto( | |
| current_lat=current_lat, | |
| current_lon=current_lon, | |
| alt_m=current_alt_m, | |
| current_time=current_time, | |
| base_heading_deg=heading, | |
| speed_knots=speed_knots, | |
| leg_minutes=leg_minutes, | |
| model_sel=model_sel, | |
| search_half_width_deg=5.0, | |
| heading_step_deg=0.1, | |
| ) | |
| if tuned.get('ok'): | |
| heading = tuned['best_heading'] | |
| lat2 = tuned['best_lat'] | |
| lon2 = tuned['best_lon'] | |
| new_time = tuned['best_time'] | |
| print(f"[Auto-tune] Best heading: {heading:.2f}° | Expected ΔBTO vs {tuned['meas_time_str']}: {tuned['best_delta_bto']:+.3f} µs") | |
| else: | |
| if model_sel == '1': | |
| (lat2, lon2), _ = vincenty_direct((current_lat, current_lon), heading, distance_m) | |
| else: | |
| lat2, lon2 = rhumb_direct((current_lat, current_lon), heading, distance_m) | |
| new_time = current_time + timedelta(minutes=leg_minutes) | |
| else: | |
| if model_sel == '1': | |
| (lat2, lon2), _ = vincenty_direct((current_lat, current_lon), heading, distance_m) | |
| else: | |
| lat2, lon2 = rhumb_direct((current_lat, current_lon), heading, distance_m) | |
| new_time = current_time + timedelta(minutes=leg_minutes) | |
| bto_val = calculateBTO_at_time(lat2, lon2, current_alt_m, new_time) | |
| tsec = seconds_since_sat_epoch(new_time) | |
| bfo_val = calculateBFO_at_time(speed_mps, heading, lat2, lon2, current_alt_m, 0.0, new_time) | |
| idx, meas_time_str, meas_bfo, meas_bto = nearest_measured_for_time(new_time) | |
| delta_bto = meas_bto - bto_val | |
| delta_bfo = meas_bfo - bfo_val | |
| print() | |
| print(f'New coordinates: {lat2:.6f}, {lon2:.6f} | Calculated BTO: {bto_val:.3f} (Δ vs nearest@{meas_time_str}: {delta_bto:.3f}) | Calculated BFO: {bfo_val:.1f} (Δ vs nearest@{meas_time_str}: {delta_bfo:.1f}) | Distance travelled: {distance_m / 1000.0:.2f} km | Timing (UTC): {new_time.isoformat()}') | |
| print() | |
| path_points.append((lat2, lon2, new_time.isoformat())) | |
| print('Would you like to:') | |
| print('A) Output the flight path generated so far') | |
| print('B) Input a new heading and ground speed for calculating the flight path to handshake no. 3') | |
| print('C) Restart calculations from an earlier handshake (Handshake 1 to 2)') | |
| print('D) Proceed to calculate a flight path to handshake no. 4') | |
| print('E) Manually input coordinates and speed for BTO & BFO calculations at handshake 3') | |
| print('F) Exit the program') | |
| selection = input('Selection: ').strip().upper() | |
| # --- Menu actions (dispatch table) --- | |
| advance_after_menu = True # if False, do not overwrite current_* with the just-computed leg | |
| def action_A(): | |
| print('\nFlight path so far (index: lat, lon, utc):') | |
| for i, (la, lo, ts) in enumerate(path_points): | |
| print(f'{i:02d}: {la:.6f}, {lo:.6f}, {ts}') | |
| def action_B(): | |
| # Placeholder: keep behavior as a no-op (you can wire this to a heading/speed editor later). | |
| pass | |
| def action_C(): | |
| idx_str = input(f'Restart from index (0..{len(path_points) - 1}): ').strip() | |
| try: | |
| i_idx = int(idx_str) | |
| if 0 <= i_idx < len(path_points): | |
| la, lo, ts = path_points[i_idx] | |
| return ('restart', i_idx, la, lo, ts) | |
| else: | |
| print('Index out of range.') | |
| except Exception as e: | |
| print('Invalid index.', e) | |
| return None | |
| def action_D(): | |
| return 'continue' | |
| def action_E(): | |
| try: | |
| la = float(input('Latitude (deg): ')) | |
| lo = float(input('Longitude (deg): ')) | |
| spd_kn = float(input('Speed (knots): ')) | |
| hdg2 = float(input('Heading (deg): ')) | |
| t_str = input('UTC time (YYYY-MM-DDTHH:MM:SSZ, blank = nearest handshake to current time): ').strip() | |
| if t_str: | |
| if t_str.endswith('Z'): | |
| t_str = t_str[:-1] + '+00:00' | |
| t_utc = datetime.fromisoformat(t_str) | |
| else: | |
| nearest_idx, meas_ts, *_ = nearest_measured_for_time(current_time) | |
| t_utc = SAT_EPOCH_UTC + timedelta(seconds=_sat_time_seconds[nearest_idx]) | |
| spd_mps = spd_kn * 0.514444 | |
| bto_v = calculateBTO_at_time(la, lo, current_alt_m, t_utc) | |
| bfo_v = calculateBFO_at_time(spd_mps, hdg2, la, lo, current_alt_m, 0.0, t_utc) | |
| idxn, m_ts, m_bfo, m_bto = nearest_measured_for_time(t_utc) | |
| print(f'Manual point BTO: {bto_v:.3f} (meas@{m_ts}: {m_bto}, Δ: {m_bto - bto_v:.3f})') | |
| print(f'Manual point BFO: {bfo_v:.1f} (meas@{m_ts}: {m_bfo}, Δ: {m_bfo - bfo_v:.1f})') | |
| except Exception as e: | |
| print('Bad manual input.', e) | |
| def action_F(): | |
| return 'exit' | |
| actions = { | |
| 'A': action_A, | |
| 'B': action_B, | |
| 'C': action_C, | |
| 'D': action_D, | |
| 'E': action_E, | |
| 'F': action_F, | |
| } | |
| action = actions.get(selection) | |
| if action is None: | |
| # Unknown / blank input: do nothing. | |
| pass | |
| else: | |
| result = action() | |
| if result == 'exit': | |
| print('Exiting.') | |
| break | |
| if result == 'continue': | |
| current_lat, current_lon, current_time = (lat2, lon2, new_time) | |
| continue | |
| if isinstance(result, tuple) and result and result[0] == 'restart': | |
| _, i_idx, la, lo, ts = result | |
| current_lat, current_lon = (la, lo) | |
| current_time = datetime.fromisoformat(ts.replace('Z', '+00:00')) | |
| path_points = path_points[:i_idx + 1] | |
| print(f'Restarted from index {i_idx}.') | |
| advance_after_menu = False | |
| if advance_after_menu: | |
| current_lat, current_lon, current_time = (lat2, lon2, new_time) | |
| if __name__ == '__main__': | |
| repl_fly_from_radar_fix() | |
| def satloc_labels(): | |
| return [_extract_time_string(r[0]) for r in satLoc] | |