267 lines
9.2 KiB
C++
267 lines
9.2 KiB
C++
#include "SensorManager.h"
|
|
|
|
#if ENV_INCLUDE_GPS
|
|
#ifndef GPS_TELEMETRY_CACHE_INTERVAL_SEC
|
|
#define GPS_TELEMETRY_CACHE_INTERVAL_SEC (2UL * 60UL * 60UL)
|
|
#endif
|
|
#ifndef GPS_TELEMETRY_MAX_ACQUIRE_SEC
|
|
#define GPS_TELEMETRY_MAX_ACQUIRE_SEC (15UL * 60UL)
|
|
#endif
|
|
#ifndef GPS_TELEMETRY_STABLE_SEC
|
|
#define GPS_TELEMETRY_STABLE_SEC 30UL
|
|
#endif
|
|
#ifndef GPS_TELEMETRY_HOLD_SEC
|
|
#define GPS_TELEMETRY_HOLD_SEC (2UL * 60UL * 60UL)
|
|
#endif
|
|
#ifndef GPS_TELEMETRY_MAX_STALE_SEC
|
|
#define GPS_TELEMETRY_MAX_STALE_SEC (12UL * 60UL * 60UL)
|
|
#endif
|
|
#ifndef GPS_TELEMETRY_STABLE_RADIUS_M
|
|
#define GPS_TELEMETRY_STABLE_RADIUS_M 100.0f
|
|
#endif
|
|
|
|
static bool millisDue(unsigned long now, unsigned long deadline) {
|
|
return (long)(now - deadline) >= 0;
|
|
}
|
|
|
|
static float gpsDistanceSquaredMeters(float lat1, float lon1, float lat2, float lon2) {
|
|
static const float METERS_PER_DEGREE = 111320.0f;
|
|
float x = (lon2 - lon1) * METERS_PER_DEGREE;
|
|
float y = (lat2 - lat1) * METERS_PER_DEGREE;
|
|
return x * x + y * y;
|
|
}
|
|
|
|
bool SensorManager::gpsTelemetryHoldActive(unsigned long now) const {
|
|
return gps_hold_until != 0 && !millisDue(now, gps_hold_until);
|
|
}
|
|
|
|
bool SensorManager::gpsTelemetryCacheFresh(unsigned long now) const {
|
|
return gps_cache_valid &&
|
|
(unsigned long)(now - gps_cache_updated_at) <= GPS_TELEMETRY_MAX_STALE_SEC * 1000UL;
|
|
}
|
|
|
|
void SensorManager::updateGpsTelemetryCache(float lat, float lon, float altitude, unsigned long now) {
|
|
gps_cache_lat = lat;
|
|
gps_cache_lon = lon;
|
|
gps_cache_altitude = altitude;
|
|
gps_cache_updated_at = now;
|
|
gps_cache_valid = true;
|
|
}
|
|
|
|
void SensorManager::maybeStopGpsForTelemetry(unsigned long now) {
|
|
if (gps_transport_available && telemetryGpsActive() && !gps_user_enabled
|
|
&& !gps_acquiring && !gpsTelemetryHoldActive(now)) {
|
|
telemetryGpsStop();
|
|
gps_next_cache_update_at = now + GPS_TELEMETRY_CACHE_INTERVAL_SEC * 1000UL;
|
|
}
|
|
}
|
|
|
|
void SensorManager::cancelGpsTelemetryDemand(unsigned long now) {
|
|
// An explicit user-off request must win over work started by an earlier
|
|
// telemetry query. Otherwise the acquisition/hold state can keep a GPS
|
|
// powered for up to two hours after the user turned it off.
|
|
gps_acquiring = false;
|
|
gps_acquire_has_fix = false;
|
|
gps_hold_until = 0;
|
|
gps_acquire_started_at = 0;
|
|
gps_stable_started_at = 0;
|
|
gps_weighted_lat = 0;
|
|
gps_weighted_lon = 0;
|
|
gps_weighted_altitude = 0;
|
|
gps_weight_sum = 0;
|
|
gps_weight_count = 0;
|
|
// Do not let the background cache refresh immediately undo the command.
|
|
gps_next_cache_update_at = now + GPS_TELEMETRY_CACHE_INTERVAL_SEC * 1000UL;
|
|
}
|
|
|
|
void SensorManager::beginGpsTelemetryAcquisition(unsigned long now) {
|
|
if (!gps_transport_available || !telemetryGpsDetected() || gps_acquiring) return;
|
|
|
|
gps_acquiring = true;
|
|
gps_acquire_has_fix = false;
|
|
gps_acquire_started_at = now;
|
|
gps_stable_started_at = now;
|
|
gps_weighted_lat = 0;
|
|
gps_weighted_lon = 0;
|
|
gps_weighted_altitude = 0;
|
|
gps_weight_sum = 0;
|
|
gps_weight_count = 0;
|
|
if (!telemetryGpsActive()) telemetryGpsStart();
|
|
}
|
|
|
|
void SensorManager::finishGpsTelemetryAcquisition(unsigned long now, bool use_weighted_average) {
|
|
if (use_weighted_average && gps_weight_sum > 0) {
|
|
updateGpsTelemetryCache(gps_weighted_lat / gps_weight_sum,
|
|
gps_weighted_lon / gps_weight_sum,
|
|
gps_weighted_altitude / gps_weight_sum,
|
|
now);
|
|
}
|
|
gps_acquiring = false;
|
|
gps_next_cache_update_at = now + GPS_TELEMETRY_CACHE_INTERVAL_SEC * 1000UL;
|
|
maybeStopGpsForTelemetry(now);
|
|
}
|
|
|
|
bool SensorManager::queryGpsTelemetry(uint8_t requester_permissions, CayenneLPP& telemetry) {
|
|
if (!(requester_permissions & TELEM_PERM_LOCATION)
|
|
|| !telemetryGpsDetected()) return false;
|
|
|
|
unsigned long now = millis();
|
|
if (!gps_transport_available) {
|
|
// A bridge may temporarily own the GPS UART. Authorized callers can still
|
|
// receive the last good fix while it is inside the normal freshness bound,
|
|
// but the query must not create a hold or try to reclaim the UART.
|
|
if (!gpsTelemetryCacheFresh(now)) return false;
|
|
telemetry.addGPS(TELEM_CHANNEL_SELF, gps_cache_lat, gps_cache_lon,
|
|
gps_cache_altitude);
|
|
return true;
|
|
}
|
|
if (gps_stop_after_fix && !gps_user_enabled) {
|
|
// Repeaters acquire only when the cached position is stale. The request
|
|
// returns without GPS while acquisition is in progress; the next request
|
|
// can use the cached fix without waking the receiver again.
|
|
if (!gpsTelemetryCacheFresh(now) && !gps_acquiring) {
|
|
beginGpsTelemetryAcquisition(now);
|
|
}
|
|
if (!gpsTelemetryCacheFresh(now)) return false;
|
|
telemetry.addGPS(TELEM_CHANNEL_SELF, gps_cache_lat, gps_cache_lon,
|
|
gps_cache_altitude);
|
|
return true;
|
|
}
|
|
gps_hold_until = now + GPS_TELEMETRY_HOLD_SEC * 1000UL;
|
|
if (!telemetryGpsActive()) telemetryGpsStart();
|
|
if (!gpsTelemetryCacheFresh(now) && !gps_acquiring) beginGpsTelemetryAcquisition(now);
|
|
|
|
if (!gpsTelemetryCacheFresh(now)) return false;
|
|
telemetry.addGPS(TELEM_CHANNEL_SELF, gps_cache_lat, gps_cache_lon, gps_cache_altitude);
|
|
return true;
|
|
}
|
|
|
|
void SensorManager::processGpsTelemetryFix(float lat, float lon, float altitude, unsigned long now) {
|
|
if (!gps_transport_available) return;
|
|
if (!gps_acquiring) {
|
|
if (gps_user_enabled || gpsTelemetryHoldActive(now)) {
|
|
updateGpsTelemetryCache(lat, lon, altitude, now);
|
|
}
|
|
return;
|
|
}
|
|
|
|
if (!gps_acquire_has_fix) {
|
|
gps_acquire_has_fix = true;
|
|
gps_stable_started_at = now;
|
|
gps_stable_origin_lat = lat;
|
|
gps_stable_origin_lon = lon;
|
|
if (gps_stop_after_fix && !gps_user_enabled) {
|
|
updateGpsTelemetryCache(lat, lon, altitude, now);
|
|
finishGpsTelemetryAcquisition(now, false);
|
|
return;
|
|
}
|
|
}
|
|
|
|
static const float STABLE_RADIUS_M2 =
|
|
GPS_TELEMETRY_STABLE_RADIUS_M * GPS_TELEMETRY_STABLE_RADIUS_M;
|
|
if (gpsDistanceSquaredMeters(gps_stable_origin_lat, gps_stable_origin_lon, lat, lon) > STABLE_RADIUS_M2) {
|
|
updateGpsTelemetryCache(lat, lon, altitude, now);
|
|
finishGpsTelemetryAcquisition(now, false);
|
|
return;
|
|
}
|
|
|
|
float weight = (float)++gps_weight_count;
|
|
gps_weighted_lat += lat * weight;
|
|
gps_weighted_lon += lon * weight;
|
|
gps_weighted_altitude += altitude * weight;
|
|
gps_weight_sum += weight;
|
|
if (millisDue(now, gps_stable_started_at + GPS_TELEMETRY_STABLE_SEC * 1000UL)) {
|
|
finishGpsTelemetryAcquisition(now, true);
|
|
}
|
|
}
|
|
|
|
void SensorManager::loopGpsTelemetry(unsigned long now) {
|
|
if (!gps_transport_available) return;
|
|
if (!gps_user_enabled && !gpsTelemetryHoldActive(now) && !gps_acquiring) {
|
|
maybeStopGpsForTelemetry(now);
|
|
}
|
|
if (gps_location_access_available && telemetryGpsDetected() && !gps_user_enabled &&
|
|
!gpsTelemetryHoldActive(now) && !gps_acquiring &&
|
|
(gps_next_cache_update_at == 0 || millisDue(now, gps_next_cache_update_at))) {
|
|
beginGpsTelemetryAcquisition(now);
|
|
}
|
|
if (gps_acquiring && millisDue(now, gps_acquire_started_at + GPS_TELEMETRY_MAX_ACQUIRE_SEC * 1000UL)) {
|
|
finishGpsTelemetryAcquisition(now, gps_acquire_has_fix && gps_weight_sum > 0);
|
|
}
|
|
}
|
|
|
|
void SensorManager::setGpsTelemetryUserEnabled(bool enabled) {
|
|
gps_user_enabled = enabled;
|
|
unsigned long now = millis();
|
|
if (enabled) {
|
|
if (gps_transport_available && telemetryGpsDetected()
|
|
&& !telemetryGpsActive()) telemetryGpsStart();
|
|
} else {
|
|
cancelGpsTelemetryDemand(now);
|
|
maybeStopGpsForTelemetry(now);
|
|
}
|
|
}
|
|
|
|
void SensorManager::setGpsTelemetryTransportAvailable(bool available) {
|
|
if (gps_transport_available == available) return;
|
|
|
|
gps_transport_available = available;
|
|
if (!available) {
|
|
// The UART is no longer ours. Cancel both the short acquisition and the
|
|
// two-hour remote-query hold so neither can silently reclaim it from a
|
|
// bridge. Preserve the user's preference and last good cache.
|
|
gps_acquiring = false;
|
|
gps_acquire_has_fix = false;
|
|
gps_hold_until = 0;
|
|
gps_acquire_started_at = 0;
|
|
gps_stable_started_at = 0;
|
|
gps_weighted_lat = 0;
|
|
gps_weighted_lon = 0;
|
|
gps_weighted_altitude = 0;
|
|
gps_weight_sum = 0;
|
|
gps_weight_count = 0;
|
|
return;
|
|
}
|
|
|
|
gps_next_cache_update_at = 0;
|
|
if (gps_user_enabled && telemetryGpsDetected() && !telemetryGpsActive()) {
|
|
telemetryGpsStart();
|
|
}
|
|
}
|
|
|
|
void SensorManager::resetGpsTelemetryTransportState() {
|
|
// Hardware discovery may be rerun after a bridge-owned UART was blocked.
|
|
// Reset transient ownership without invoking a provider callback before the
|
|
// new probe has established which provider, if any, is present.
|
|
gps_transport_available = true;
|
|
gps_acquiring = false;
|
|
gps_acquire_has_fix = false;
|
|
gps_hold_until = 0;
|
|
gps_acquire_started_at = 0;
|
|
gps_stable_started_at = 0;
|
|
gps_weighted_lat = 0;
|
|
gps_weighted_lon = 0;
|
|
gps_weighted_altitude = 0;
|
|
gps_weight_sum = 0;
|
|
gps_weight_count = 0;
|
|
gps_next_cache_update_at = 0;
|
|
}
|
|
#endif
|
|
|
|
void SensorManager::setTelemetryLocationAccessAvailable(bool available) {
|
|
#if ENV_INCLUDE_GPS
|
|
unsigned long now = millis();
|
|
if (gps_location_access_available == available) return;
|
|
|
|
gps_location_access_available = available;
|
|
if (available) {
|
|
gps_next_cache_update_at = 0;
|
|
} else if (gps_acquiring && !gps_user_enabled && !gpsTelemetryHoldActive(now)) {
|
|
gps_acquiring = false;
|
|
maybeStopGpsForTelemetry(now);
|
|
}
|
|
#else
|
|
(void)available;
|
|
#endif
|
|
}
|