Sync

src/navico/data.rs

@@ -2,12 +2,14 @@ use bincode::deserialize;
 use log::{debug, trace, warn};
 use protobuf::Message;
 use serde::Deserialize;
+use std::f64::consts::PI;
 use std::time::{SystemTime, UNIX_EPOCH};
 use std::{io, time::Duration};
 use tokio::net::UdpSocket;
 use tokio::sync::mpsc::Receiver;
 use tokio::time::sleep;
 use tokio_graceful_shutdown::SubsystemHandle;
+use trail::TrailBuffer;
 
 use crate::locator::LocatorId;
 use crate::navico::NAVICO_SPOKE_LEN;
@@ -114,30 +116,33 @@ pub struct NavicoDataReceiver {
     key: String,
     statistics: Statistics,
     info: RadarInfo,
-    buf: Vec<u8>,
     sock: Option<UdpSocket>,
     rx: tokio::sync::mpsc::Receiver<DataUpdate>,
     doppler: DopplerMode,
     pixel_to_blob: [[u8; BYTE_LOOKUP_LENGTH]; LOOKUP_SPOKE_LENGTH],
     replay: bool,
+    trails: TrailBuffer,
+    previous_range: u32,
 }
 
 impl NavicoDataReceiver {
     pub fn new(info: RadarInfo, rx: Receiver<DataUpdate>, replay: bool) -> NavicoDataReceiver {
         let key = info.key();
 
         let pixel_to_blob = Self::pixel_to_blob(&info.legend);
+        let trails = TrailBuffer::new(info.legend.clone(), NAVICO_SPOKES, NAVICO_SPOKE_LEN);
 
         NavicoDataReceiver {
             key,
             statistics: Statistics { broken_packets: 0 },
             info: info,
-            buf: Vec::with_capacity(size_of::<RadarFramePkt>()),
             sock: None,
             rx,
             doppler: DopplerMode::None,
             pixel_to_blob,
             replay,
+            trails,
+            previous_range: 0,
         }
     }
 
@@ -205,11 +210,16 @@ impl NavicoDataReceiver {
                 self.pixel_to_blob = Self::pixel_to_blob(&legend);
                 self.info.legend = legend;
             }
+            Some(DataUpdate::RelativeTrail(seconds)) => {
+                self.trails.set_relative_trails_revolutions(seconds);
+            }
             None => {}
         }
     }
 
     async fn socket_loop(&mut self, subsys: &SubsystemHandle) -> Result<(), RadarError> {
+        let mut buf = Vec::with_capacity(size_of::<RadarFramePkt>());
+
         loop {
             tokio::select! { biased;
                 _ = subsys.on_shutdown_requested() => {
@@ -218,18 +228,18 @@ impl NavicoDataReceiver {
                 r = self.rx.recv() => {
                   self.handle_data_update(r);
                 },
-                r = self.sock.as_ref().unwrap().recv_buf_from(&mut self.buf)  => {
+                r = self.sock.as_ref().unwrap().recv_buf_from(&mut buf)  => {
                     match r {
                         Ok(_) => {
-                            self.process_frame();
+                            self.process_frame(&mut buf);
                         },
                         Err(e) => {
                             return Err(RadarError::Io(e));
                         }
                     }
                 },
             }
-            self.buf.clear();
+            buf.clear();
         }
     }
 
@@ -253,9 +263,7 @@ impl NavicoDataReceiver {
         }
     }
 
-    fn process_frame(&mut self) {
-        let data = &self.buf;
-
+    fn process_frame(&mut self, data: &mut Vec<u8>) {
         if data.len() < FRAME_HEADER_LENGTH + RADAR_LINE_LENGTH {
             warn!(
                 "UDP data frame with even less than one spoke, len {} dropped",
@@ -305,7 +313,8 @@ impl NavicoDataReceiver {
         }
 
         if mark_full_rotation {
-            self.info.full_rotation();
+            let ms = self.info.full_rotation();
+            self.trails.set_rotation_speed(ms);
         }
 
         let mut bytes = Vec::new();
@@ -409,7 +418,7 @@ impl NavicoDataReceiver {
     }
 
     fn process_spoke(
-        &self,
+        &mut self,
         range: u32,
         angle: SpokeBearing,
         heading: Option<u16>,
@@ -453,20 +462,38 @@ impl NavicoDataReceiver {
             generic_spoke.len()
         );
 
+        if range != self.previous_range && range != 0 {
+            if self.previous_range != 0 {
+                let zoom_factor = self.previous_range as f64 / range as f64;
+                self.trails.zoom_relative_trails(zoom_factor);
+            }
+            self.previous_range = range;
+        }
+
         // For now, don't send heading in replay mode, signalk-radar-client doesn't
         // handle it well yet.
         let heading = if self.replay {
             None
-        } else {
+        } else if heading.is_some() {
             heading.map(|h| (((h / 2) + angle) % (NAVICO_SPOKES as u16)) as u32)
+        } else {
+            let heading = crate::signalk::get_heading_true();
+            heading.map(|h| {
+                (((h * NAVICO_SPOKES as f64 / (2. * PI)) as u16 + angle) % (NAVICO_SPOKES as u16))
+                    as u32
+            })
         };
 
-        let mut message = RadarMessage::new();
-        message.radar = 1;
+        self.trails
+            .update_relative_trails(angle, &mut generic_spoke);
+
+        let mut message: RadarMessage = RadarMessage::new();
+        message.radar = self.info.id as u32;
         let mut spoke = Spoke::new();
         spoke.range = range;
         spoke.angle = angle as u32;
         spoke.bearing = heading;
+        (spoke.lat, spoke.lon) = crate::signalk::get_position_i64();
         spoke.time = SystemTime::now()
             .duration_since(UNIX_EPOCH)
             .map(|d| d.as_millis() as u64)

src/navico/mod.rs

@@ -42,6 +42,7 @@ const NAVICO_BEACON_ADDRESS: SocketAddr =
 pub enum DataUpdate {
     Doppler(DopplerMode),
     Legend(Legend),
+    RelativeTrail(u16),
 }
 
 #[derive(Deserialize, Debug, Copy, Clone)]

src/navico/report.rs

@@ -397,6 +397,10 @@ impl NavicoReportReceiver {
                         self.radars.update(&self.info);
                         return Ok(());
                     }
+                    ControlType::TargetTrails => {
+                        self.handle_target_trails_value(cv).await?;
+                        return Ok(());
+                    }
                     _ => {} // rest is numeric
                 }
 
@@ -422,6 +426,23 @@ impl NavicoReportReceiver {
         Ok(())
     }
 
+    async fn handle_target_trails_value(&mut self, cv: &ControlValue) -> Result<(), RadarError> {
+        let value = cv.value.parse::<i32>().unwrap_or(0);
+        if self
+            .info
+            .set(&cv.id, value, cv.auto, ControlState::Manual)
+            .is_err()
+        {
+            log::warn!("Cannot set TargetTrails to {}", value);
+        }
+
+        self.data_tx
+            .send(DataUpdate::RelativeTrail(value as u16))
+            .await
+            .map_err(|_| RadarError::CannotSetControlType(cv.id))?;
+        Ok(())
+    }
+
     async fn send_report_requests(&mut self) -> Result<(), RadarError> {
         self.command_sender.send_report_requests().await?;
         self.report_request_timeout += self.report_request_interval;

src/radar.rs

@@ -15,7 +15,7 @@ use std::{
 use thiserror::Error;
 use tokio_graceful_shutdown::SubsystemHandle;
 
-mod trail;
+pub(crate) mod trail;
 
 use crate::config::Persistence;
 use crate::locator::LocatorId;
@@ -58,13 +58,16 @@ impl IntoResponse for RadarError {
     }
 }
 
+//
+// This order of pixeltypes is also how they are stored in the legend.
+//
 #[derive(Serialize, Clone, Debug)]
 enum PixelType {
-    History,
+    Normal,
     TargetBorder,
     DopplerApproaching,
     DopplerReceding,
-    Normal,
+    History,
 }
 
 #[derive(Clone, Debug)]
@@ -255,7 +258,7 @@ impl RadarInfo {
         self.legend = default_legend(doppler, self.pixel_values);
     }
 
-    pub fn full_rotation(&mut self) {
+    pub fn full_rotation(&mut self) -> u32 {
         let now = Instant::now();
         let diff: Duration = now - self.rotation_timestamp;
         let diff = diff.as_millis() as f64;
@@ -265,12 +268,17 @@ impl RadarInfo {
 
         log::debug!("{}: rotation speed {} dRPM", self.key, rpm);
 
-        let _ = self
-            .command_tx
-            .send(ControlMessage::SetValue(ControlValue::new(
-                ControlType::RotationSpeed,
-                rpm,
-            )));
+        if diff < 3000. && diff > 600. {
+            let _ = self
+                .command_tx
+                .send(ControlMessage::SetValue(ControlValue::new(
+                    ControlType::RotationSpeed,
+                    rpm,
+                )));
+            diff as u32
+        } else {
+            0
+        }
     }
 
     ///

src/radar/trail.rs

@@ -2,57 +2,97 @@ use super::{Legend, SpokeBearing, BLOB_HISTORY_COLORS};
 
 const MARGIN: usize = 100;
 
-struct TrailBuffer {
+pub struct TrailBuffer {
     legend: Legend,
     spokes: usize,
     max_spoke_len: usize,
     previous_pixels_per_meter: f64,
     trail_size: usize,
     true_trails: Vec<u8>,
-    relative_trails: Vec<u8>,
+    relative_trails: Vec<u16>,
+    trail_length_ms: u32,
+    rotation_speed_ms: u32,
 }
 
 impl TrailBuffer {
-    pub fn new(legend: Legend, spokes: u32, max_spoke_len: u32) -> Self {
-        let trail_size: usize = max_spoke_len as usize * 2 + MARGIN * 2;
+    pub fn new(legend: Legend, spokes: usize, max_spoke_len: usize) -> Self {
+        let trail_size = max_spoke_len * 2 + MARGIN * 2;
         TrailBuffer {
             legend,
-            spokes: spokes as usize,
-            max_spoke_len: spokes as usize,
+            spokes,
+            max_spoke_len,
             previous_pixels_per_meter: 0.,
             trail_size,
             true_trails: vec![0; trail_size * trail_size],
-            relative_trails: vec![0; spokes as usize * max_spoke_len as usize],
+            relative_trails: vec![0; spokes * max_spoke_len],
+            trail_length_ms: 0,
+            rotation_speed_ms: 0,
         }
     }
 
-    fn update_relative_trails(&mut self, angle: SpokeBearing, data: &mut Vec<u8>, len: usize) {
+    pub fn set_relative_trails_revolutions(&mut self, seconds: u16) {
+        self.trail_length_ms = seconds as u32 * 1000;
+    }
+
+    pub fn set_rotation_speed(&mut self, ms: u32) {
+        self.rotation_speed_ms = ms;
+    }
+
+    pub fn update_relative_trails(&mut self, angle: SpokeBearing, data: &mut Vec<u8>) {
+        if self.trail_length_ms == 0 {
+            return;
+        }
+        let max_trail_value = (self.trail_length_ms / self.rotation_speed_ms) as u16;
+
         let trail = &mut self.relative_trails[angle as usize * self.max_spoke_len as usize
             ..(angle + 1) as usize * self.max_spoke_len];
 
         let mut radius = 0;
 
+        if angle == 0 {
+            log::debug!("Spoke before trails: {:?}", data);
+        }
+
         let update_relative_motion = true; // motion == TARGET_MOTION_RELATIVE;
 
-        while radius < len {
-            if data[radius] >= self.legend.strong_return {
+        while radius < data.len() {
+            if data[radius] >= self.legend.strong_return && data[radius] < self.legend.history_start
+            {
                 trail[radius] = 1;
-            } else if trail[radius] > 0 && trail[radius] < BLOB_HISTORY_COLORS {
-                trail[radius] += 1;
+            } else if trail[radius] > 0 {
+                trail[radius] = trail[radius].wrapping_add(1); // Yes, we want overflow here after 65535 rotations
             }
 
-            if update_relative_motion && data[radius] == 0 {
-                data[radius] = self.legend.history_start + trail[radius];
+            if update_relative_motion
+                && data[radius] == 0
+                && trail[radius] > 0
+                && trail[radius] < max_trail_value
+            {
+                let mut index =
+                    (trail[radius] * BLOB_HISTORY_COLORS as u16 / max_trail_value) as u8;
+                if index >= BLOB_HISTORY_COLORS {
+                    index = BLOB_HISTORY_COLORS;
+                }
+                if index < 1 {
+                    index = 1;
+                }
+
+                data[radius] = self.legend.history_start + index - 1;
             }
             radius += 1;
         }
         while radius < self.max_spoke_len {
             trail[radius] = 0;
         }
+
+        if angle == 0 {
+            log::debug!("Trail after trails: {:?}", trail);
+            log::debug!("Spoke after trails: {:?}", data);
+        }
     }
 
     // zoom_factor > 1 -> zoom in, enlarge image
-    fn zoom_relative_trails(&mut self, zoom_factor: f64) {
+    pub fn zoom_relative_trails(&mut self, zoom_factor: f64) {
         let mut new_trail = vec![0; self.max_spoke_len];
         let mut index_prev = 0;
         for spoke in 0..self.spokes {