Illustrated C/C++ source sheets, symbol index, API relationship graph, compiler console and debug trace

LIVE AI / FLEXIBLE SENSING / FLIGHT CONTEXT

Sensoraia API

Live AI and sensing, connected to the flight.
See, listen, inspect and return supported results while airborne.

Loading...
Searching...
No Matches
motion_compensation.h
Go to the documentation of this file.
1/*
2 * Copyright 2026 OpenUAS
3 * SPDX-License-Identifier: Apache-2.0
4 *
5 * Licensed under the Apache License, Version 2.0 (the "License");
6 * you may not use this file except in compliance with the License.
7 * You may obtain a copy of the License at
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 */
17
18#ifndef USHER_MOTION_COMPENSATION_H
19#define USHER_MOTION_COMPENSATION_H
20
21/**
22 * @file motion_compensation.h
23 * @brief Advance a position along ground velocity by a bounded elapsed interval.
24 * @details The helper uses WGS-84 local curvature radii rather than a fixed
25 * meters-per-degree approximation, which keeps the correction meaningful over USHER's
26 * valid operating latitudes while retaining a small, allocation-free calculation.
27 */
28
29#include <math.h>
30#include <stdint.h>
31
32/** @brief Correct latitude/longitude for horizontal platform motion during capture.
33 * @param latitude In/out latitude in $10^{-7}$ degrees.
34 * @param longitude In/out longitude in $10^{-7}$ degrees.
35 * @param speed Ground speed in m/s.
36 * @param course Ground-track angle in radians, clockwise from north.
37 * @param delay Capture latency in seconds.
38 * @return 1 when corrected coordinates were stored, 0 when inputs were unsafe.
39 * @details Conservative limits reject implausible telemetry and polar geometry, where
40 * longitude correction becomes ill-conditioned. Longitude is normalized after the
41 * local tangent-plane displacement to preserve the wire representation's range. */
42static inline int compensate_ground_position(int32_t *latitude, int32_t *longitude,
43 double speed, double course, double delay)
44{
45 if (!latitude || !longitude || !isfinite(speed) || !isfinite(course) || !isfinite(delay)
46 || speed < 0 || speed > 100 || delay < 0 || delay > 2
48 || *longitude < -1800000000 || *longitude > 1800000000) return 0;
49 const double radians = 0.017453292519943295;
50 const double phi = *latitude / 1e7 * radians;
51 const double eccentricity = 0.0066943799901413165;
52 const double denominator = 1 - eccentricity * sin(phi) * sin(phi);
53 const double prime_vertical = 6378137.0 / sqrt(denominator);
54 const double meridional = prime_vertical * (1 - eccentricity) / denominator;
55 const double corrected_lat = *latitude / 1e7 + speed * delay * cos(course) / meridional / radians;
56 double corrected_lon = *longitude / 1e7 + speed * delay * sin(course) / (prime_vertical * cos(phi)) / radians;
57 if (corrected_lat < -85 || corrected_lat > 85) return 0;
58 if (corrected_lon > 180) corrected_lon -= 360;
59 if (corrected_lon < -180) corrected_lon += 360;
60 *latitude = (int32_t)llround(corrected_lat * 1e7);
61 *longitude = (int32_t)llround(corrected_lon * 1e7);
62 return 1;
63}
64
65#endif
static double latitude
static double longitude
static int compensate_ground_position(int32_t *latitude, int32_t *longitude, double speed, double course, double delay)
Correct latitude/longitude for horizontal platform motion during capture.