634 lines
23 KiB
Rust
634 lines
23 KiB
Rust
use anyhow::{anyhow, Context, Result};
|
|
use chrono::{DateTime, Duration, Utc};
|
|
use log::{debug, error, info, warn};
|
|
use serialport::{SerialPort};
|
|
use std::io::{BufRead, BufReader};
|
|
use std::path::Path;
|
|
use std::sync::{Arc, Mutex};
|
|
use std::thread;
|
|
use std::time;
|
|
use serialport::DataBits::Eight;
|
|
use tokio::sync::broadcast;
|
|
|
|
|
|
#[cfg(feature = "gpio")]
|
|
use rppal::gpio::{Gpio, InputPin, Trigger};
|
|
|
|
#[cfg(not(feature = "gpio"))]
|
|
use rand::Rng;
|
|
|
|
use crate::gps::nmea::{parse_nmea_sentence, NmeaPosition};
|
|
use crate::gps::{GeoPosition, GpsConfig, GpsStatus, SyncStatus, CameraOrientation};
|
|
|
|
/// GPS module state information
|
|
#[derive(Debug, Clone)]
|
|
struct GpsState {
|
|
/// Whether the GPS hardware is initialized
|
|
initialized: bool,
|
|
/// Whether the module is in degraded mode (using fallback values)
|
|
degraded: bool,
|
|
/// Last successful fix time
|
|
last_fix: Option<DateTime<Utc>>,
|
|
/// Initialization failure count
|
|
init_failures: u32,
|
|
}
|
|
|
|
impl Default for GpsState {
|
|
fn default() -> Self {
|
|
Self {
|
|
initialized: false,
|
|
degraded: false,
|
|
last_fix: None,
|
|
init_failures: 0,
|
|
}
|
|
}
|
|
}
|
|
|
|
/// Controller for GPS and time synchronization
|
|
pub struct GpsController {
|
|
/// GPS configuration
|
|
config: GpsConfig,
|
|
/// Serial port for GPS communication
|
|
port: Option<Box<dyn SerialPort>>,
|
|
/// GPIO pin for PPS signal
|
|
#[cfg(feature = "gpio")]
|
|
pps_pin: Option<InputPin>,
|
|
/// Placeholder for non-Linux platforms
|
|
#[cfg(not(target_os = "linux"))]
|
|
pps_pin: Option<()>,
|
|
/// Last known position
|
|
position: Arc<Mutex<GeoPosition>>,
|
|
/// Current synchronization status
|
|
sync_status: Arc<Mutex<SyncStatus>>,
|
|
/// Last PPS pulse timestamp
|
|
last_pps: Arc<Mutex<Option<DateTime<Utc>>>>,
|
|
/// Number of satellites in view
|
|
satellites: Arc<Mutex<u8>>,
|
|
/// Last position update timestamp
|
|
last_update: Arc<Mutex<Option<DateTime<Utc>>>>,
|
|
/// Broadcast channel for position updates
|
|
position_tx: broadcast::Sender<GeoPosition>,
|
|
/// Whether the GPS is running
|
|
is_running: Arc<Mutex<bool>>,
|
|
/// GPS module state
|
|
gps_state: GpsState,
|
|
}
|
|
|
|
impl GpsController {
|
|
/// Create a new GPS controller with the given configuration
|
|
pub async fn new(config: &crate::config::Config) -> Result<Self> {
|
|
// Extract GPS settings from config
|
|
let gps_config = config.gps.clone();
|
|
|
|
// Create broadcast channel for position updates
|
|
let (position_tx, _) = broadcast::channel(10);
|
|
|
|
// Use fallback position as initial position if GPS is not enabled
|
|
let initial_position = if gps_config.enable_gps {
|
|
GeoPosition::default()
|
|
} else {
|
|
gps_config.fallback_position
|
|
};
|
|
|
|
Ok(Self {
|
|
config: gps_config,
|
|
port: None,
|
|
#[cfg(feature = "gpio")]
|
|
pps_pin: None,
|
|
position: Arc::new(Mutex::new(initial_position)),
|
|
sync_status: Arc::new(Mutex::new(SyncStatus::NoSync)),
|
|
last_pps: Arc::new(Mutex::new(None)),
|
|
satellites: Arc::new(Mutex::new(0)),
|
|
last_update: Arc::new(Mutex::new(None)),
|
|
position_tx,
|
|
is_running: Arc::new(Mutex::new(false)),
|
|
gps_state: GpsState::default(),
|
|
})
|
|
}
|
|
|
|
/// Initialize the GPS module
|
|
pub async fn initialize(&mut self) -> Result<()> {
|
|
// Check if we're on a non-Linux platform
|
|
#[cfg(not(target_os = "linux"))]
|
|
{
|
|
info!("GPS module running on non-Linux platform. Using fallback position.");
|
|
self.gps_state.degraded = true;
|
|
|
|
// Set fallback position
|
|
let mut pos = self.position.lock().unwrap();
|
|
*pos = self.config.fallback_position;
|
|
|
|
// Update satellites with simulated value
|
|
let mut sats = self.satellites.lock().unwrap();
|
|
*sats = 8; // Simulate 8 satellites for non-Linux platforms
|
|
|
|
return Ok(());
|
|
}
|
|
|
|
// Continue with normal initialization for Linux
|
|
// Check if GPS is enabled in config
|
|
if !self.config.enable_gps {
|
|
info!("GPS module disabled in configuration. Using fallback position.");
|
|
self.gps_state.degraded = true;
|
|
|
|
// Set fallback position
|
|
let mut pos = self.position.lock().unwrap();
|
|
*pos = self.config.fallback_position;
|
|
|
|
return Ok(());
|
|
}
|
|
|
|
let mut init_failed = false;
|
|
|
|
// Open the serial port
|
|
let port = serialport::new(&self.config.port, self.config.baud_rate)
|
|
.data_bits(Eight)
|
|
.flow_control(serialport::FlowControl::None)
|
|
.parity(serialport::Parity::None)
|
|
.stop_bits(serialport::StopBits::One)
|
|
.timeout(time::Duration::from_millis(1000));
|
|
match port.open() {
|
|
Ok(port) => {
|
|
self.port = Some(port);
|
|
self.gps_state.initialized = true;
|
|
},
|
|
Err(e) => {
|
|
self.gps_state.init_failures += 1;
|
|
|
|
if self.config.allow_degraded_mode {
|
|
warn!("Failed to open GPS port {}: {}. Using fallback position.",
|
|
self.config.port, e);
|
|
self.gps_state.degraded = true;
|
|
init_failed = true;
|
|
|
|
// Set fallback position
|
|
let mut pos = self.position.lock().unwrap();
|
|
*pos = self.config.fallback_position;
|
|
} else {
|
|
return Err(anyhow!("Failed to open GPS port and degraded mode is not allowed: {}", e));
|
|
}
|
|
}
|
|
}
|
|
|
|
// Initialize PPS pin if enabled and GPS initialized successfully
|
|
#[cfg(feature = "gpio")]
|
|
if self.config.use_pps && !self.gps_state.degraded {
|
|
match Gpio::new() {
|
|
Ok(gpio) => {
|
|
match gpio.get(self.config.pps_pin) {
|
|
Ok(pin) => {
|
|
let mut input_pin = pin.into_input();
|
|
|
|
// Set up PPS edge detection (rising edge)
|
|
let last_pps = self.last_pps.clone();
|
|
match input_pin.set_async_interrupt(Trigger::RisingEdge, Some(time::Duration::from_millis(20)), move |_| {
|
|
let now = Utc::now();
|
|
let mut last_pps = last_pps.lock().unwrap();
|
|
*last_pps = Some(now);
|
|
}) {
|
|
Ok(_) => {
|
|
self.pps_pin = Some(input_pin);
|
|
info!("PPS signal detection initialized on GPIO {}", self.config.pps_pin);
|
|
},
|
|
Err(e) => {
|
|
warn!("Failed to set up PPS interrupt: {}. PPS sync disabled.", e);
|
|
}
|
|
}
|
|
},
|
|
Err(e) => {
|
|
warn!("Failed to access GPIO pin {}: {}. PPS sync disabled.",
|
|
self.config.pps_pin, e);
|
|
}
|
|
}
|
|
},
|
|
Err(e) => {
|
|
warn!("Failed to access GPIO: {}. PPS sync disabled.", e);
|
|
}
|
|
}
|
|
} else if !self.config.use_pps {
|
|
info!("PPS signal detection disabled in configuration");
|
|
}
|
|
|
|
if self.gps_state.degraded {
|
|
if self.config.allow_degraded_mode {
|
|
info!("GPS module initialized in degraded mode (using fallback position)");
|
|
Ok(())
|
|
} else {
|
|
Err(anyhow!("GPS initialization failed and degraded mode is not allowed"))
|
|
}
|
|
} else {
|
|
info!("GPS module initialized successfully");
|
|
Ok(())
|
|
}
|
|
}
|
|
|
|
/// Start GPS processing
|
|
pub async fn start(&self) -> Result<()> {
|
|
// Handle simulated mode for non-Linux platforms
|
|
#[cfg(not(target_os = "linux"))]
|
|
{
|
|
info!("Starting GPS in simulated mode on non-Linux platform");
|
|
|
|
// Set fallback position and status
|
|
{
|
|
let mut pos = self.position.lock().unwrap();
|
|
*pos = self.config.fallback_position;
|
|
|
|
let mut status = self.sync_status.lock().unwrap();
|
|
*status = SyncStatus::GpsOnly; // Simulate GPS sync
|
|
|
|
// Send an initial position update with fallback
|
|
let _ = self.position_tx.send(self.config.fallback_position);
|
|
}
|
|
|
|
{
|
|
let mut is_running = self.is_running.lock().unwrap();
|
|
*is_running = true;
|
|
}
|
|
|
|
// Start a simulation thread for periodic updates
|
|
let position_tx = self.position_tx.clone();
|
|
let position = self.position.clone();
|
|
let is_running = self.is_running.clone();
|
|
let fallback_position = self.config.fallback_position;
|
|
|
|
thread::spawn(move || {
|
|
info!("Starting GPS simulation thread");
|
|
|
|
while {
|
|
let is_running = is_running.lock().unwrap();
|
|
*is_running
|
|
} {
|
|
// Sleep for a bit to simulate update interval
|
|
thread::sleep(time::Duration::from_secs(1));
|
|
|
|
// Generate small random variations to the position
|
|
let mut rng = rand::thread_rng();
|
|
let lat_variation = (rng.gen::<f64>() - 0.5) * 0.0001; // ~10m variation
|
|
let lon_variation = (rng.gen::<f64>() - 0.5) * 0.0001;
|
|
|
|
let new_position = GeoPosition {
|
|
latitude: fallback_position.latitude + lat_variation,
|
|
longitude: fallback_position.longitude + lon_variation,
|
|
altitude: fallback_position.altitude + (rng.gen::<f64>() - 0.5) * 2.0,
|
|
};
|
|
|
|
// Update position
|
|
{
|
|
let mut pos = position.lock().unwrap();
|
|
*pos = new_position;
|
|
|
|
// Send position update
|
|
let _ = position_tx.send(new_position);
|
|
}
|
|
}
|
|
|
|
info!("GPS simulation thread stopped");
|
|
});
|
|
|
|
return Ok(());
|
|
}
|
|
|
|
// Handle degraded mode
|
|
if self.gps_state.degraded {
|
|
info!("Starting GPS in degraded mode (using fallback position)");
|
|
|
|
// Set fallback position and status
|
|
{
|
|
let mut pos = self.position.lock().unwrap();
|
|
*pos = self.config.fallback_position;
|
|
|
|
let mut status = self.sync_status.lock().unwrap();
|
|
*status = SyncStatus::NoSync;
|
|
|
|
// Send an initial position update with fallback
|
|
let _ = self.position_tx.send(self.config.fallback_position);
|
|
}
|
|
|
|
{
|
|
let mut is_running = self.is_running.lock().unwrap();
|
|
*is_running = true;
|
|
}
|
|
|
|
return Ok(());
|
|
}
|
|
|
|
// Normal mode - require initialized GPS
|
|
if self.port.is_none() {
|
|
return Err(anyhow!("GPS not initialized"));
|
|
}
|
|
|
|
{
|
|
let mut is_running = self.is_running.lock().unwrap();
|
|
if *is_running {
|
|
warn!("GPS is already running");
|
|
return Ok(());
|
|
}
|
|
*is_running = true;
|
|
}
|
|
|
|
// Clone Arc references for the background task
|
|
let port_name = self.config.port.clone();
|
|
let position = self.position.clone();
|
|
let sync_status = self.sync_status.clone();
|
|
let satellites = self.satellites.clone();
|
|
let last_update = self.last_update.clone();
|
|
let position_tx = self.position_tx.clone();
|
|
let is_running = self.is_running.clone();
|
|
|
|
// Clone fallback position in case we need it later
|
|
let fallback_position = self.config.fallback_position;
|
|
let allow_degraded = self.config.allow_degraded_mode;
|
|
let baud_rate = self.config.baud_rate;
|
|
|
|
// Create a separate thread for GPS processing (blocking I/O)
|
|
thread::spawn(move || {
|
|
info!("Starting GPS processing on port {}", port_name);
|
|
// Open the serial port
|
|
|
|
let port = serialport::new(&port_name, baud_rate)
|
|
.data_bits(Eight)
|
|
.flow_control(serialport::FlowControl::None)
|
|
.parity(serialport::Parity::None)
|
|
.stop_bits(serialport::StopBits::One)
|
|
.timeout(time::Duration::from_millis(1000));
|
|
|
|
// Get the port
|
|
let port = match port.open() {
|
|
Ok(port) => port,
|
|
Err(e) => {
|
|
error!("Failed to open GPS port: {}", e);
|
|
|
|
// Use fallback position if degraded mode allowed
|
|
if allow_degraded {
|
|
warn!("Using fallback position due to GPS port error");
|
|
let mut pos = position.lock().unwrap();
|
|
*pos = fallback_position;
|
|
|
|
let _ = position_tx.send(fallback_position);
|
|
}
|
|
|
|
{
|
|
let mut is_running = is_running.lock().unwrap();
|
|
*is_running = false;
|
|
}
|
|
return;
|
|
}
|
|
};
|
|
|
|
let reader = BufReader::new(port);
|
|
|
|
// Set a timeout for acquiring fix
|
|
let start_time = Utc::now();
|
|
let mut fix_acquired = false;
|
|
|
|
for line in reader.lines() {
|
|
// Check if we should exit
|
|
{
|
|
let is_running = is_running.lock().unwrap();
|
|
if !*is_running {
|
|
break;
|
|
}
|
|
}
|
|
|
|
// Check if we've been waiting too long for a fix
|
|
if !fix_acquired && allow_degraded {
|
|
let elapsed = Utc::now() - start_time;
|
|
if elapsed > Duration::seconds(30) { // 30 second timeout for initial fix
|
|
warn!("Timeout waiting for GPS fix, using fallback position");
|
|
|
|
// Set fallback position
|
|
{
|
|
let mut pos = position.lock().unwrap();
|
|
*pos = fallback_position;
|
|
let _ = position_tx.send(fallback_position);
|
|
}
|
|
|
|
// Continue trying, but we at least have a fallback now
|
|
}
|
|
}
|
|
|
|
match line {
|
|
Ok(sentence) => {
|
|
if let Ok(Some(nmea_pos)) = parse_nmea_sentence(&sentence) {
|
|
// Update position
|
|
{
|
|
let mut pos = position.lock().unwrap();
|
|
*pos = nmea_pos.position;
|
|
|
|
// Send position update
|
|
let _ = position_tx.send(nmea_pos.position);
|
|
}
|
|
|
|
// Update satellites
|
|
{
|
|
let mut sats = satellites.lock().unwrap();
|
|
*sats = nmea_pos.satellites;
|
|
}
|
|
|
|
// Update sync status
|
|
{
|
|
let mut status = sync_status.lock().unwrap();
|
|
if nmea_pos.fix_quality > 0 {
|
|
*status = SyncStatus::GpsOnly;
|
|
fix_acquired = true;
|
|
}
|
|
}
|
|
|
|
// Update last update time
|
|
{
|
|
let mut update = last_update.lock().unwrap();
|
|
*update = Some(Utc::now());
|
|
}
|
|
|
|
debug!("GPS update: lat={:.6}, lon={:.6}, alt={:.1}, satellites={}",
|
|
nmea_pos.position.latitude,
|
|
nmea_pos.position.longitude,
|
|
nmea_pos.position.altitude,
|
|
nmea_pos.satellites);
|
|
}
|
|
}
|
|
Err(e) => {
|
|
error!("Error reading GPS data: {}", e);
|
|
}
|
|
}
|
|
}
|
|
|
|
info!("GPS processing stopped");
|
|
{
|
|
let mut is_running = is_running.lock().unwrap();
|
|
*is_running = false;
|
|
}
|
|
});
|
|
|
|
info!("GPS processing started");
|
|
Ok(())
|
|
}
|
|
|
|
/// Stop GPS processing
|
|
pub async fn stop(&self) -> Result<()> {
|
|
{
|
|
let mut is_running = self.is_running.lock().unwrap();
|
|
if !*is_running {
|
|
warn!("GPS is not running");
|
|
return Ok(());
|
|
}
|
|
*is_running = false;
|
|
}
|
|
|
|
// The background thread will notice the is_running flag and exit
|
|
info!("GPS processing stopping (may take a moment to complete)");
|
|
Ok(())
|
|
}
|
|
|
|
/// Get the current GPS status
|
|
pub fn get_status(&self) -> GpsStatus {
|
|
let position = self.position.lock().unwrap().clone();
|
|
let satellites = *self.satellites.lock().unwrap();
|
|
let sync_status = match *self.sync_status.lock().unwrap() {
|
|
SyncStatus::NoSync => "no_sync",
|
|
SyncStatus::GpsOnly => "gps_only",
|
|
SyncStatus::FullSync => "full_sync",
|
|
}.to_string();
|
|
|
|
// Calculate time accuracy estimate
|
|
let time_accuracy_ms = match *self.sync_status.lock().unwrap() {
|
|
SyncStatus::NoSync => 1000.0, // 1 second
|
|
SyncStatus::GpsOnly => 100.0, // 100 ms
|
|
SyncStatus::FullSync => 1.0, // 1 ms
|
|
};
|
|
|
|
GpsStatus {
|
|
position,
|
|
satellites,
|
|
timestamp: Utc::now(),
|
|
sync_status,
|
|
time_accuracy_ms,
|
|
camera_orientation: self.config.camera_orientation,
|
|
}
|
|
}
|
|
|
|
/// Get a precise UTC timestamp using PPS if available
|
|
pub fn get_precise_time(&self) -> DateTime<Utc> {
|
|
let now = Utc::now();
|
|
|
|
// If we have PPS sync, adjust to the last PPS pulse
|
|
if let SyncStatus::FullSync = *self.sync_status.lock().unwrap() {
|
|
if let Some(last_pps) = *self.last_pps.lock().unwrap() {
|
|
// Only use PPS if it's recent (within last second)
|
|
let elapsed = now - last_pps;
|
|
if elapsed < Duration::seconds(1) {
|
|
return last_pps + elapsed;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Fall back to system time
|
|
now
|
|
}
|
|
|
|
/// Get current position
|
|
pub fn get_position(&self) -> GeoPosition {
|
|
self.position.lock().unwrap().clone()
|
|
}
|
|
|
|
/// Subscribe to position updates
|
|
pub fn subscribe(&self) -> broadcast::Receiver<GeoPosition> {
|
|
self.position_tx.subscribe()
|
|
}
|
|
|
|
/// Check if the GPS has a valid fix
|
|
pub fn has_fix(&self) -> bool {
|
|
// In degraded mode with fallback, pretend we have a fix
|
|
if self.gps_state.degraded && self.config.allow_degraded_mode {
|
|
return true;
|
|
}
|
|
|
|
// Otherwise check actual sync status
|
|
match *self.sync_status.lock().unwrap() {
|
|
SyncStatus::NoSync => false,
|
|
_ => true,
|
|
}
|
|
}
|
|
|
|
/// Update the camera orientation
|
|
pub fn set_camera_orientation(&mut self, orientation: CameraOrientation) {
|
|
self.config.camera_orientation = orientation;
|
|
}
|
|
|
|
/// Get the camera orientation
|
|
pub fn get_camera_orientation(&self) -> CameraOrientation {
|
|
self.config.camera_orientation
|
|
}
|
|
|
|
/// Get current position with complete status information
|
|
pub async fn get_current_position(&self) -> Result<serde_json::Value> {
|
|
// Get the basic GPS status
|
|
let status = self.get_status();
|
|
|
|
// Get additional system time information
|
|
let system_time = Utc::now();
|
|
let precise_time = self.get_precise_time();
|
|
let time_diff_ms = (precise_time - system_time).num_milliseconds().abs() as f64;
|
|
|
|
// Calculate additional derived values
|
|
let fix_age_seconds = if let Some(last_update) = *self.last_update.lock().unwrap() {
|
|
(Utc::now() - last_update).num_seconds()
|
|
} else {
|
|
-1 // No fix yet
|
|
};
|
|
|
|
// Determine if the position is from fallback
|
|
let is_fallback = self.gps_state.degraded ||
|
|
!self.has_fix() ||
|
|
fix_age_seconds > 30; // Stale fix
|
|
|
|
// Construct a detailed status object
|
|
let position_status = serde_json::json!({
|
|
// Basic position data
|
|
"position": {
|
|
"latitude": status.position.latitude,
|
|
"longitude": status.position.longitude,
|
|
"altitude": status.position.altitude,
|
|
"is_fallback": is_fallback,
|
|
"satellites": status.satellites,
|
|
},
|
|
|
|
// Timing information
|
|
"timing": {
|
|
"timestamp": precise_time.to_rfc3339(),
|
|
"sync_status": status.sync_status,
|
|
"time_accuracy_ms": status.time_accuracy_ms,
|
|
"system_time_offset_ms": time_diff_ms,
|
|
"fix_age_seconds": fix_age_seconds,
|
|
},
|
|
|
|
// Camera orientation
|
|
"orientation": {
|
|
"azimuth": status.camera_orientation.azimuth,
|
|
"elevation": status.camera_orientation.elevation,
|
|
},
|
|
|
|
// System status
|
|
"system": {
|
|
"hardware_initialized": self.gps_state.initialized,
|
|
"running": *self.is_running.lock().unwrap(),
|
|
"degraded_mode": self.gps_state.degraded,
|
|
"has_fix": self.has_fix(),
|
|
}
|
|
});
|
|
|
|
Ok(position_status)
|
|
}
|
|
}
|
|
|
|
impl Drop for GpsController {
|
|
fn drop(&mut self) {
|
|
// Ensure the GPS processing is stopped
|
|
let mut is_running = self.is_running.lock().unwrap();
|
|
*is_running = false;
|
|
|
|
// PPS pin will be dropped automatically, removing the interrupt
|
|
}
|
|
}
|