452 lines
14 KiB
Rust
452 lines
14 KiB
Rust
use crate::{
|
|
ChimemonMessage, ChimemonSource, ChimemonSourceChannel, Config, SourceMetric, SourceReport,
|
|
SourceReportDetails, SourceStatus,
|
|
};
|
|
|
|
use std::collections::HashMap;
|
|
use std::f64;
|
|
use std::fmt::Debug;
|
|
use std::net::SocketAddr;
|
|
use std::sync::Arc;
|
|
use std::time::Duration;
|
|
|
|
use async_trait::async_trait;
|
|
use backoff::ExponentialBackoff;
|
|
use futures::{SinkExt, Stream};
|
|
use futures::{StreamExt, task::Context};
|
|
use gpsd_proto::{
|
|
Device, Gst, Mode, Pps, ResponseHandshake, Sky, Tpv, UnifiedResponse, Version, Watch,
|
|
};
|
|
use serde::{Deserialize, Deserializer, Serialize};
|
|
use serde_json;
|
|
use tokio::net::{TcpStream, ToSocketAddrs, lookup_host};
|
|
use tokio::time::{Interval, interval, timeout};
|
|
use tokio_util::codec::{Framed, LinesCodec};
|
|
use tracing::{debug, debug_span, info, warn};
|
|
|
|
pub struct GpsdSource {
|
|
pub config: Config,
|
|
conn: GpsdTransport,
|
|
devices: HashMap<String, Device>,
|
|
last_gst: Option<Gst>,
|
|
last_pps: Option<Pps>,
|
|
last_tpv: Option<Tpv>,
|
|
last_sky: Option<Sky>,
|
|
}
|
|
|
|
#[derive(Eq, PartialEq, Clone, Copy, Debug)]
|
|
pub enum GpsdFixType {
|
|
Unknown,
|
|
NoFix,
|
|
Fix2D,
|
|
Fix3D,
|
|
Surveyed,
|
|
}
|
|
|
|
impl From<u32> for GpsdFixType {
|
|
fn from(value: u32) -> Self {
|
|
match value {
|
|
0 => Self::Unknown,
|
|
1 => Self::NoFix,
|
|
2 => Self::Fix2D,
|
|
3 => Self::Fix3D,
|
|
_ => panic!("Invalid fix mode {value}"),
|
|
}
|
|
}
|
|
}
|
|
|
|
impl From<Mode> for GpsdFixType {
|
|
fn from(value: Mode) -> Self {
|
|
match value {
|
|
Mode::NoFix => Self::NoFix,
|
|
Mode::Fix2d => Self::Fix2D,
|
|
Mode::Fix3d => Self::Fix3D,
|
|
}
|
|
}
|
|
}
|
|
|
|
#[derive(Clone, Debug)]
|
|
pub struct GpsdSourceReport {
|
|
fix_type: GpsdFixType,
|
|
sats_visible: u8,
|
|
sats_tracked: u8,
|
|
tdop: f64,
|
|
}
|
|
|
|
impl SourceReportDetails for GpsdSourceReport {
|
|
fn is_healthy(&self) -> bool {
|
|
self.fix_type != GpsdFixType::Unknown && self.fix_type != GpsdFixType::NoFix
|
|
}
|
|
fn to_metrics(&self) -> Vec<SourceMetric> {
|
|
let no_tags = Arc::new(vec![]);
|
|
vec![
|
|
SourceMetric::new_int("sats_visible", self.sats_visible as i64, no_tags.clone()),
|
|
SourceMetric::new_int("sats_tracked", self.sats_tracked as i64, no_tags.clone()),
|
|
SourceMetric::new_float("tdop", self.tdop, no_tags.clone()),
|
|
]
|
|
}
|
|
}
|
|
|
|
impl GpsdSource {
|
|
pub async fn new(config: Config) -> Result<Self, std::io::Error> {
|
|
let conn = GpsdTransport::new(&config.sources.gpsd.host).await?;
|
|
Ok(Self {
|
|
config,
|
|
conn,
|
|
devices: HashMap::new(),
|
|
last_gst: None,
|
|
last_pps: None,
|
|
last_tpv: None,
|
|
last_sky: None,
|
|
})
|
|
}
|
|
}
|
|
|
|
impl GpsdSource {
|
|
async fn send_status(&self, chan: &mut ChimemonSourceChannel) {
|
|
let sky = self.last_sky.as_ref();
|
|
let tpv = self.last_tpv.as_ref();
|
|
|
|
let (sats_tracked, sats_visible) = sky.map_or((0, 0), |sky| {
|
|
let sats = sky.satellites.as_deref().unwrap_or_default();
|
|
(
|
|
sats.iter().filter(|s| s.used).count() as u8,
|
|
sats.len() as u8,
|
|
)
|
|
});
|
|
|
|
let tdop = sky
|
|
.and_then(|sky| sky.tdop)
|
|
.map_or(f64::INFINITY, |tdop| tdop as f64);
|
|
|
|
chan.send(ChimemonMessage::SourceReport(SourceReport {
|
|
name: "gpsd".into(),
|
|
status: SourceStatus::Unknown,
|
|
details: Arc::new(GpsdSourceReport {
|
|
fix_type: tpv.map_or(GpsdFixType::Unknown, |tpv| tpv.mode.into()),
|
|
sats_tracked,
|
|
sats_visible,
|
|
tdop,
|
|
}),
|
|
}))
|
|
.unwrap();
|
|
}
|
|
|
|
fn handle_msg(&mut self, msg: String) -> Result<(), Box<dyn std::error::Error>> {
|
|
let _span = debug_span!("handle_msg").entered();
|
|
let parsed = serde_json::from_str::<UnifiedResponse>(&msg)?;
|
|
debug!("Received {parsed:?}");
|
|
match parsed {
|
|
UnifiedResponse::Device(d) => {
|
|
if let Some(path) = &d.path {
|
|
self.devices.insert(path.to_owned(), d);
|
|
} else {
|
|
warn!("No path on DEVICE response, ignoring.");
|
|
}
|
|
}
|
|
UnifiedResponse::Gst(g) => self.last_gst = Some(g),
|
|
UnifiedResponse::Pps(p) => self.last_pps = Some(p),
|
|
UnifiedResponse::Sky(s) => {
|
|
self.last_sky = Some({
|
|
let mut s = s;
|
|
if s.satellites.is_none() {
|
|
s.satellites = self.last_sky.as_mut().and_then(|old| old.satellites.take());
|
|
}
|
|
s
|
|
})
|
|
}
|
|
UnifiedResponse::Tpv(t) => self.last_tpv = Some(t),
|
|
_ => warn!("Unhandled response `{parsed:?}`"),
|
|
}
|
|
|
|
Ok(())
|
|
}
|
|
}
|
|
|
|
#[async_trait]
|
|
impl ChimemonSource for GpsdSource {
|
|
async fn run(mut self, mut chan: ChimemonSourceChannel) {
|
|
info!("gpsd task started");
|
|
self.conn.ensure_connection().await.unwrap();
|
|
let mut ticker = interval(Duration::from_secs(self.config.sources.gpsd.interval));
|
|
|
|
let mut params = WatchParams::default();
|
|
params.json = Some(true);
|
|
self.conn
|
|
.cmd_response(&GpsdCommand::Watch(Some(params)))
|
|
.await
|
|
.unwrap();
|
|
loop {
|
|
let framed = self.conn.framed.as_mut().expect("must be connected");
|
|
tokio::select! {
|
|
_ = ticker.tick() => {
|
|
self.send_status(&mut chan).await
|
|
},
|
|
maybe_msg = framed.next() => {
|
|
if let Some(Ok(msg)) = maybe_msg {
|
|
self.handle_msg(msg).unwrap()
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
struct GpsdTransport {
|
|
host: SocketAddr,
|
|
framed: Option<Framed<TcpStream, LinesCodec>>,
|
|
conn_backoff: ExponentialBackoff,
|
|
}
|
|
|
|
#[derive(Clone, Copy, Debug, Serialize)]
|
|
#[repr(u8)]
|
|
pub enum RawMode {
|
|
Off = 0,
|
|
RawHex = 1,
|
|
RawBin = 2,
|
|
}
|
|
|
|
#[derive(Clone, Copy, Debug, Serialize)]
|
|
#[repr(u8)]
|
|
pub enum NativeMode {
|
|
Nmea = 0,
|
|
Alt = 1,
|
|
}
|
|
|
|
#[derive(Clone, Copy, Debug, Serialize)]
|
|
#[repr(u8)]
|
|
pub enum ParityMode {
|
|
None = b'N',
|
|
Odd = b'O',
|
|
Even = b'E',
|
|
}
|
|
|
|
#[derive(Clone, Debug, Serialize)]
|
|
struct WatchParams {
|
|
#[serde(skip_serializing_if = "Option::is_none")]
|
|
enable: Option<bool>,
|
|
#[serde(skip_serializing_if = "Option::is_none")]
|
|
json: Option<bool>,
|
|
#[serde(skip_serializing_if = "Option::is_none")]
|
|
nmea: Option<bool>,
|
|
#[serde(skip_serializing_if = "Option::is_none")]
|
|
raw: Option<RawMode>,
|
|
#[serde(skip_serializing_if = "Option::is_none")]
|
|
scaled: Option<bool>,
|
|
#[serde(skip_serializing_if = "Option::is_none")]
|
|
split24: Option<bool>,
|
|
#[serde(skip_serializing_if = "Option::is_none")]
|
|
pps: Option<bool>,
|
|
#[serde(skip_serializing_if = "Option::is_none")]
|
|
device: Option<String>,
|
|
}
|
|
|
|
impl Default for WatchParams {
|
|
fn default() -> Self {
|
|
WatchParams {
|
|
enable: Some(true),
|
|
json: Some(false),
|
|
nmea: None,
|
|
raw: None,
|
|
scaled: None,
|
|
split24: None,
|
|
pps: None,
|
|
device: None,
|
|
}
|
|
}
|
|
}
|
|
|
|
#[derive(Clone, Debug, Serialize)]
|
|
struct DeviceParams {
|
|
#[serde(skip_serializing_if = "Option::is_none")]
|
|
bps: Option<u64>,
|
|
#[serde(skip_serializing_if = "Option::is_none")]
|
|
cycle: Option<f64>,
|
|
#[serde(skip_serializing_if = "Option::is_none")]
|
|
flags: Option<u8>,
|
|
#[serde(skip_serializing_if = "Option::is_none")]
|
|
hexdata: Option<String>,
|
|
#[serde(skip_serializing_if = "Option::is_none")]
|
|
native: Option<NativeMode>,
|
|
#[serde(skip_serializing_if = "Option::is_none")]
|
|
parity: Option<ParityMode>,
|
|
#[serde(skip_serializing_if = "Option::is_none")]
|
|
path: Option<String>,
|
|
#[serde(skip_serializing_if = "Option::is_none")]
|
|
readonly: Option<bool>,
|
|
#[serde(skip_serializing_if = "Option::is_none")]
|
|
sernum: Option<String>,
|
|
#[serde(skip_serializing_if = "Option::is_none")]
|
|
stopbits: Option<u8>,
|
|
}
|
|
|
|
#[derive(Clone, Debug)]
|
|
enum GpsdCommand {
|
|
Version, // no params
|
|
Devices, // no params
|
|
Watch(Option<WatchParams>),
|
|
Poll, // I don't understand the protocol for this one
|
|
Device(Option<DeviceParams>),
|
|
}
|
|
|
|
impl GpsdCommand {
|
|
fn command_string(&self) -> &str {
|
|
match self {
|
|
Self::Version => "?VERSION",
|
|
Self::Devices => "?DEVICES",
|
|
Self::Watch(_) => "?WATCH",
|
|
Self::Poll => "?POLL",
|
|
Self::Device(_) => "?DEVICE",
|
|
}
|
|
}
|
|
|
|
fn expected_responses(&self) -> usize {
|
|
match self {
|
|
Self::Version => 1,
|
|
Self::Devices => 1,
|
|
Self::Watch(_) => 2,
|
|
Self::Poll => 1,
|
|
Self::Device(_) => 1,
|
|
}
|
|
}
|
|
}
|
|
|
|
impl ToString for GpsdCommand {
|
|
fn to_string(&self) -> String {
|
|
let s = self.command_string().to_owned();
|
|
match self {
|
|
Self::Version | Self::Devices | Self::Poll | Self::Watch(None) | Self::Device(None) => {
|
|
s + ";"
|
|
}
|
|
Self::Watch(Some(w)) => s + "=" + &serde_json::to_string(w).unwrap() + ";",
|
|
Self::Device(Some(d)) => s + "=" + &serde_json::to_string(d).unwrap() + ";",
|
|
}
|
|
}
|
|
}
|
|
|
|
impl GpsdTransport {
|
|
async fn new<T: ToSocketAddrs + Debug>(host: &T) -> Result<Self, std::io::Error> {
|
|
// TODO: implement proper handling of multiple responses
|
|
let host_addr = lookup_host(host).await?.next().ok_or(std::io::Error::new(
|
|
std::io::ErrorKind::NotFound,
|
|
format!("No response looking up `{:?}`", host),
|
|
))?;
|
|
Ok(Self {
|
|
host: host_addr,
|
|
framed: None,
|
|
conn_backoff: ExponentialBackoff::default(),
|
|
})
|
|
}
|
|
async fn connect(&mut self) -> Result<(), Box<dyn std::error::Error>> {
|
|
info!("Connecting to gpsd @ {}", self.host);
|
|
let mut framed = backoff::future::retry_notify(
|
|
self.conn_backoff.clone(),
|
|
async || {
|
|
Ok(Framed::new(
|
|
TcpStream::connect(self.host).await?,
|
|
LinesCodec::new(),
|
|
))
|
|
},
|
|
|e, d| warn!("Failed to connect to {} after {:?}: `{}`", self.host, d, e),
|
|
)
|
|
.await?;
|
|
debug!("Waiting for initial VERSION");
|
|
if let Ok(Some(Ok(r))) = timeout(Duration::from_secs(5), framed.next()).await {
|
|
if let Ok(version) = serde_json::from_str::<Version>(&r) {
|
|
info!(
|
|
"Connected to gpsd @ {}, release {}",
|
|
self.host, version.release
|
|
)
|
|
} else {
|
|
warn!("Got unexpected non-VERSION response after connection (`{r}`)")
|
|
}
|
|
self.framed = Some(framed);
|
|
Ok(())
|
|
} else {
|
|
Err("Unexpected failure to receive initial handshake response".into())
|
|
}
|
|
}
|
|
async fn ensure_connection(&mut self) -> Result<(), Box<dyn std::error::Error>> {
|
|
if let Some(conn) = &self.framed {
|
|
Ok(())
|
|
} else {
|
|
self.connect().await
|
|
}
|
|
}
|
|
async fn cmd_response(
|
|
&mut self,
|
|
cmd: &GpsdCommand,
|
|
) -> Result<Vec<UnifiedResponse>, Box<dyn std::error::Error>> {
|
|
debug!("Command: `{cmd:?}`");
|
|
self.ensure_connection().await?;
|
|
|
|
let mut responses = Vec::new();
|
|
if let Some(conn) = &mut self.framed {
|
|
debug!("Raw command: `{}`", cmd.to_string());
|
|
conn.send(cmd.to_string()).await?;
|
|
for _ in 0..cmd.expected_responses() {
|
|
match conn.next().await {
|
|
None => return Err("Connection lost".into()),
|
|
Some(Err(e)) => return Err(format!("Unable to parse response {e}").into()),
|
|
Some(Ok(r)) => {
|
|
debug!("Raw response: `{r}`");
|
|
responses.push(serde_json::from_str::<UnifiedResponse>(&r)?)
|
|
}
|
|
}
|
|
}
|
|
} else {
|
|
return Err("Missing connection despite ensure".into());
|
|
}
|
|
|
|
Ok(responses)
|
|
}
|
|
|
|
async fn stream(
|
|
&mut self,
|
|
) -> Result<
|
|
impl Stream<Item = Result<UnifiedResponse, Box<dyn std::error::Error>>>,
|
|
Box<dyn std::error::Error>,
|
|
> {
|
|
self.ensure_connection().await?;
|
|
if let Some(conn) = &mut self.framed {
|
|
Ok(conn.map(|line| Ok(serde_json::from_str::<UnifiedResponse>(&line?)?)))
|
|
} else {
|
|
Err("No connection after connecting.".into())
|
|
}
|
|
}
|
|
}
|
|
|
|
mod tests {
|
|
use gpsd_proto::{ResponseData, UnifiedResponse};
|
|
use tokio_stream::StreamExt;
|
|
|
|
use crate::gpsd::{GpsdCommand, GpsdTransport, WatchParams};
|
|
use std::sync::Once;
|
|
|
|
static INIT: Once = Once::new();
|
|
|
|
fn init_logger() {
|
|
INIT.call_once(|| tracing_subscriber::fmt::init());
|
|
}
|
|
|
|
#[tokio::test]
|
|
async fn test_gpsd() {
|
|
init_logger();
|
|
let mut gpsd = GpsdTransport::new(&"192.168.65.93:2947").await.unwrap();
|
|
gpsd.connect().await.unwrap();
|
|
let mut params = WatchParams::default();
|
|
params.enable = Some(true);
|
|
params.json = Some(true);
|
|
let res = {
|
|
gpsd.cmd_response(&GpsdCommand::Watch(Some(params)))
|
|
.await
|
|
.unwrap()
|
|
};
|
|
println!("{res:?}");
|
|
while let Some(Ok(s)) = gpsd.framed.as_mut().unwrap().next().await {
|
|
let res2 = serde_json::from_str::<UnifiedResponse>(&s);
|
|
println!("{res2:?}");
|
|
}
|
|
}
|
|
}
|