Implement UART I/O protocol

Cargo.toml

@@ -34,11 +34,11 @@ esp = [
 std = ["c-its-parser/std"]
 
 # enable L67K GNSS module
-gnss = ["dep:embassy_gps"]
+gnss = ["_gnss", "dep:embassy_gps"]
 # echo CAM data on serial
 cam = ["c-its-parser/cam"]
 # send CAM autonomously
-cam_tx = ["c-its-parser/cam", "gnss"]
+cam_tx = ["c-its-parser/cam"]
 # echo DENM events on serial and BLE
 denm = ["c-its-parser/denm"]
 # evaluate SPAT information
@@ -47,9 +47,13 @@ spat = ["c-its-parser/spatem", "c-its-parser/mapem"]
 spat_debug = []
 # enable ST7789 based display
 screen = ["dep:mipidsi", "dep:embedded-graphics", "dep:embedded-hal-bus", "dep:profont"]
+# enable I/O via UART
+uart = ["_uart", "_gnss", "dep:prost"]
 
 # internal feature, that any UART I/O is enabled
 _uart = []
+# internal feature, that any GNSS source is enabled
+_gnss = []
 
 [dependencies]
 esp-hal = { version = "~1.1.0", optional = true, features = ["esp32c5", "log-04", "unstable"] }
@@ -102,7 +106,7 @@ geo-types = { version = "0.7.19", default-features = false }
 mipidsi = { version = "0.10.0", optional = true }
 num-traits = { version = "0.2.19", default-features = false, features = ["libm"] }
 profont = { version = "0.7.0", optional = true }
-prost = { version = "0.14", default-features = false, features = ["derive"] }
+prost = { version = "0.14", optional = true, default-features = false, features = ["derive"] }
 
 [dev-dependencies]
 assert_float_eq = "1.2.0"

Readme.md

@@ -32,6 +32,9 @@ Just prints a line for each received CAM.
 Generates and transmits a CAM when a GNSS update was received.
 Depends on the `gnss` feature.
 
+### UART
+Enable UART I/O protocol according to [docs/uart-protocol.md](./docs/uart-protocol.md).
+
 
 ## Usage
 
@@ -64,13 +67,13 @@ Additional features are available through feature flags:
 - `spat_debug`: Enable additional debug information for the SPAT/ GLOSA use case
 - `denm`: Enable DENMs
 - `cam`: Enable CAMs
-- `cam_tx`: Enable CAM generation and transmission
+- `cam_tx`: Enable CAM generation and transmission (needs time and position source)
 
 Officially supported feature combinations are:
 - `esp,spat,gnss,screen` (default features)
 - `esp,spat,gnss,screen,cam_tx`
 - `esp,spat,gnss,screen,cam_tx,cam,denm`
-- To be implemented: No GNSS and screen when BLE/ UART interface is implemented
+- `esp,uart,cam,denm`
 
 Connect your ESP32-C5 and run:
 

feat-permut.sh

@@ -8,7 +8,7 @@ extended=${2:-false}
 
 
 features_base=(
-  #cam,denm,uart
+  uart,cam,denm
   spat,gnss,screen
   spat,gnss,screen,cam_tx
 )
@@ -17,7 +17,7 @@ features_more=(
   spat,gnss,screen,cam_tx,cam,denm
 )
 features_test=(
-  spat,cam,denm
+  spat,cam,denm,uart
 )
 
 

src/applogic/cam_tx.rs

@@ -300,20 +300,23 @@ impl crate::v2x_tx::GnItsPayload for CamState {
         own_position: &crate::applogic::PositionState,
         time: chrono::DateTime<chrono::Utc>,
         _seq_no: &mut u16,
-    ) -> (
+    ) -> Result<
+        (
             geonetworking::en302636_4_1::ExtendedHeader,
             Option<geonetworking::en302636_4_1::AreaType>,
             u8,
-    ) {
+        ),
+        alloc::string::String,
+    > {
         let station_type = geonetworking::en302636_4_1::StationType::try_from(self.station_type.0)
             .unwrap_or_default();
 
         // CAM always uses SHB, hop-limit 1
-        (
+        Ok((
             crate::v2x_tx::gn::make_shb_eh(address, station_type, own_position, time),
             None,
             1,
-        )
+        ))
     }
 
     fn make_payload(

src/applogic/mod.rs

@@ -639,6 +639,11 @@ impl State {
     pub fn time(&self) -> chrono::prelude::NaiveDateTime {
         self.time
     }
+
+    #[allow(unused)]
+    pub fn pos(&self) -> &PositionState {
+        &self.pos
+    }
 }
 
 #[cfg(test)]

src/io.rs

@@ -5,6 +5,12 @@ pub mod msg {
     include!(concat!(env!("OUT_DIR"), "/c_its_io.rs"));
 }
 
+use alloc::string::ToString;
+use alloc::vec::Vec;
+
+use c_its_parser::gn as geonetworking;
+use log::error;
+
 impl From<msg::RawMsgRx> for msg::SerialOutputMsg {
     fn from(value: msg::RawMsgRx) -> Self {
         use msg::serial_output_msg::Payload;
@@ -25,6 +31,144 @@ impl From<msg::DenmEvent> for msg::SerialOutputMsg {
     }
 }
 
+impl msg::RawMsgRx {
+    pub fn serialize_uart_proto(&self) -> Vec<u8> {
+        serialize_serial_msg(&Into::<msg::SerialOutputMsg>::into(self.clone()))
+    }
+}
+impl msg::DenmEvent {
+    #[allow(unused)]
+    pub fn serialize_uart_proto(&self) -> Vec<u8> {
+        serialize_serial_msg(&Into::<msg::SerialOutputMsg>::into(*self))
+    }
+}
+
+#[derive(Debug, Default)]
+pub struct Parser {
+    buffer: alloc::vec::Vec<u8>,
+}
+
+impl Parser {
+    const SOF_LEN: u16 = 4;
+    const SOF1: u8 = 0x56;
+    const SOF2: u8 = 0x32;
+    const SOF3: u8 = 0x58;
+    const SOF4: u8 = 0x2B;
+    const EOF_LEN: u16 = 2;
+    #[allow(unused)]
+    const EOF1: u8 = 0x0d;
+    #[allow(unused)]
+    const EOF2: u8 = 0x0a;
+    const LENGTH_LEN: u16 = 2;
+
+    /// Parses incoming data and return list of messages
+    pub fn parse(&mut self, input: &[u8]) -> alloc::vec::Vec<msg::SerialInputMsg> {
+        use prost::Message as _;
+
+        // combine with existing data
+        self.buffer.append(&mut input.to_vec());
+
+        // find start of frame and parse
+        let mut output = alloc::vec![];
+        while let Some(pos) = Self::find_sof(&self.buffer) {
+            // drop everything before the SOF
+            self.buffer.drain(..pos);
+
+            // get length field and check buffer size
+            if let Some(len_bytes) = self
+                .buffer
+                .get(usize::from(Self::SOF_LEN)..usize::from(Self::SOF_LEN + Self::LENGTH_LEN))
+            {
+                // unwrap is fine since we queried 2 bytes before, so we can also convert to 2-byte slice
+                let msg_size = u16::from_be_bytes(len_bytes.try_into().unwrap());
+
+                let required_buf_size = Self::SOF_LEN + Self::LENGTH_LEN + msg_size + Self::EOF_LEN;
+                if let Some(payload) = self.buffer.get(
+                    usize::from(Self::SOF_LEN + Self::LENGTH_LEN)
+                        ..usize::from(required_buf_size - 2),
+                ) {
+                    match msg::SerialInputMsg::decode(payload) {
+                        Ok(data) => {
+                            output.push(data);
+                        }
+                        Err(err) => error!("Failed to parse input: {err:?}"),
+                    }
+
+                    // consume buffer
+                    self.buffer.drain(..usize::from(required_buf_size));
+                } else {
+                    break;
+                }
+            }
+        }
+
+        output
+    }
+
+    /// Returns the start index of the SOF sequence, if present
+    fn find_sof(input: &[u8]) -> Option<usize> {
+        // search for SOF
+        if let Some(sof1_pos) = input.iter().position(|i| *i == Self::SOF1) {
+            // drop everything before sof1_pos and check if remainder continues with SOF sequence
+            let (_, msg) = input.split_at(sof1_pos);
+            if msg.starts_with(&[Self::SOF1, Self::SOF2, Self::SOF3, Self::SOF4]) {
+                // found the SOF sequence
+                return Some(sof1_pos);
+            }
+        }
+
+        // if we reach this position, SOF sequence wasn't found
+        None
+    }
+}
+
+fn serialize_serial_msg<T>(msg: &T) -> Vec<u8>
+where
+    T: prost::Message,
+{
+    let mut msg_buf = msg.encode_to_vec();
+
+    let sof = [0x56u8, 0x32, 0x58, 0x2B];
+    let eof = [0x0du8, 0x0a];
+    #[allow(clippy::cast_possible_truncation)]
+    let len: [u8; 2] = (msg_buf.len() as u16).to_be_bytes();
+
+    let mut out = sof.to_vec();
+    out.extend_from_slice(&len);
+    out.append(&mut msg_buf);
+    out.extend_from_slice(&eof);
+
+    out
+}
+
+impl From<msg::PositionState> for crate::applogic::GnssFix {
+    fn from(value: msg::PositionState) -> Self {
+        let time =
+            chrono::DateTime::from_timestamp_millis(value.timestamp_ms).map(|v| v.naive_utc());
+
+        let latitude_deg = value.position.latitude_deg.into();
+        let longitude_deg = value.position.longitude_deg.into();
+        let heading_deg = value.heading.map(|v| {
+            #[allow(clippy::cast_precision_loss)]
+            let val = (v as f32) / 10.;
+            val
+        });
+        let speed_mps = value.speed.map(|v| {
+            #[allow(clippy::cast_precision_loss)]
+            let val = (v as f32) / 100.;
+            val
+        });
+
+        Self {
+            time,
+            latitude_deg,
+            longitude_deg,
+            heading_deg,
+            speed_mps,
+        }
+    }
+}
+
 impl From<msg::PositionState> for crate::applogic::PositionState {
     fn from(value: msg::PositionState) -> Self {
         let position = geo_types::Point::new(
@@ -49,3 +193,323 @@ impl From<msg::PositionState> for crate::applogic::PositionState {
         }
     }
 }
+
+impl From<msg::ItsPosition> for crate::applogic::PositionState {
+    fn from(value: msg::ItsPosition) -> Self {
+        let latitude_deg = f64::from(value.latitude) / 10_000_000.;
+        let longitude_deg = f64::from(value.longitude) / 10_000_000.;
+        let position = geo_types::Point::new(longitude_deg, latitude_deg);
+
+        Self {
+            position,
+            heading_deg: None,
+            speed_mps: None,
+        }
+    }
+}
+
+impl crate::v2x_tx::GnItsPayload for msg::RawMsgTx {
+    fn make_eh(
+        &self,
+        address: [u8; 6],
+        own_position: &crate::applogic::PositionState,
+        time: chrono::DateTime<chrono::Utc>,
+        seq_no: &mut u16,
+    ) -> Result<
+        (
+            c_its_parser::gn::en302636_4_1::ExtendedHeader,
+            Option<c_its_parser::gn::en302636_4_1::AreaType>,
+            u8,
+        ),
+        alloc::string::String,
+    > {
+        let station_type = self
+            .station_type
+            .and_then(|v| {
+                #[allow(clippy::cast_possible_truncation)]
+                let raw_val = v as u8;
+                geonetworking::en302636_4_1::StationType::try_from(raw_val).ok()
+            })
+            .unwrap_or_default();
+        #[allow(clippy::cast_possible_truncation)]
+        let hop_limit = self.hop_limit as u8;
+
+        Ok(match self.gn_transport() {
+            msg::GnTransport::Gac => {
+                if self.gn_area.position.is_none() {
+                    return Err("Failed to create GAC: gnArea.position missing".to_string());
+                }
+
+                *seq_no += 1;
+
+                let (atype, geoanycast) = Self::make_geobcast(
+                    address,
+                    station_type,
+                    own_position,
+                    time,
+                    self.gn_area,
+                    *seq_no,
+                )?;
+                let eh = geonetworking::en302636_4_1::ExtendedHeader::GAC(geoanycast);
+                (eh, Some(atype), hop_limit)
+            }
+            msg::GnTransport::Gbc => {
+                if self.gn_area.position.is_none() {
+                    return Err("Failed to create GAC: gnArea.position missing".to_string());
+                }
+
+                *seq_no += 1;
+
+                let (atype, geoanycast) = Self::make_geobcast(
+                    address,
+                    station_type,
+                    own_position,
+                    time,
+                    self.gn_area,
+                    *seq_no,
+                )?;
+                let eh = geonetworking::en302636_4_1::ExtendedHeader::GBC(geoanycast);
+                (eh, Some(atype), hop_limit)
+            }
+            msg::GnTransport::Tsb => {
+                *seq_no += 1;
+
+                (
+                    crate::v2x_tx::gn::make_tsb_eh(
+                        address,
+                        station_type,
+                        own_position,
+                        time,
+                        *seq_no,
+                    ),
+                    None,
+                    hop_limit,
+                )
+            }
+            msg::GnTransport::Shb => (
+                crate::v2x_tx::gn::make_shb_eh(address, station_type, own_position, time),
+                None,
+                1,
+            ),
+        })
+    }
+
+    fn make_payload(
+        &self,
+    ) -> Result<(Vec<u8>, c_its_parser::standards::extensions::ItsMessageId), alloc::string::String>
+    {
+        #[allow(clippy::cast_possible_truncation)]
+        let message_id = (self.message_type as u8).try_into()?;
+        Ok((self.payload.clone(), message_id))
+    }
+}
+
+impl msg::RawMsgTx {
+    fn make_geobcast(
+        address: [u8; 6],
+        station_type: geonetworking::en302636_4_1::StationType,
+        own_position: &crate::applogic::PositionState,
+        time: chrono::DateTime<chrono::Utc>,
+        gn_area: msg::GnArea,
+        sequence_number: u16,
+    ) -> Result<
+        (
+            c_its_parser::gn::en302636_4_1::AreaType,
+            geonetworking::en302636_4_1::GeoBroadcast,
+        ),
+        alloc::string::String,
+    > {
+        let source_position_vector =
+            crate::v2x_tx::gn::make_lpv(address, station_type, own_position, time);
+
+        let (atype, dist_a, dist_b, angle) = match gn_area.area() {
+            msg::GnAreaShape::EtsiAreashapeCircle => (
+                c_its_parser::gn::en302636_4_1::AreaType::Circular,
+                gn_area.dist_a,
+                None,
+                None,
+            ),
+            msg::GnAreaShape::EtsiAreashapeRectangle => (
+                c_its_parser::gn::en302636_4_1::AreaType::Rectangular,
+                gn_area.dist_a,
+                gn_area.dist_b,
+                gn_area.angle,
+            ),
+            msg::GnAreaShape::EtsiAreashapeEllipsis => (
+                c_its_parser::gn::en302636_4_1::AreaType::Ellipsoidal,
+                gn_area.dist_a,
+                gn_area.dist_b,
+                gn_area.angle,
+            ),
+        };
+
+        #[allow(clippy::cast_possible_truncation)]
+        let latitude_deg = own_position.position.y() as f32;
+        #[allow(clippy::cast_possible_truncation)]
+        let longitude_deg = own_position.position.x() as f32;
+        let distance_a = dist_a
+            .map(|v| {
+                #[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)]
+                let val = v as u16;
+                val
+            })
+            .unwrap_or_default();
+        let distance_b = dist_b
+            .map(|v| {
+                #[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)]
+                let val = v as u16;
+                val
+            })
+            .unwrap_or_default();
+        let angle = angle
+            .map(|v| {
+                #[allow(clippy::cast_possible_truncation, clippy::cast_sign_loss)]
+                let val = v as u16;
+                val
+            })
+            .unwrap_or_default();
+
+        let gac = geonetworking::en302636_4_1::GeoAnycast::try_from_values(
+            sequence_number,
+            source_position_vector,
+            latitude_deg,
+            longitude_deg,
+            distance_a,
+            distance_b,
+            angle,
+        )
+        .map_err(|err| alloc::format!("{err}"))?;
+
+        Ok((atype, gac))
+    }
+}
+
+#[cfg(test)]
+mod tests {
+
+    use super::*;
+    use crate::init_test_env_logger;
+
+    impl From<msg::RawMsgTx> for msg::SerialInputMsg {
+        fn from(value: msg::RawMsgTx) -> Self {
+            use msg::serial_input_msg::Payload;
+
+            Self {
+                payload: Some(Payload::TxMsg(value)),
+            }
+        }
+    }
+
+    impl From<msg::PositionState> for msg::SerialInputMsg {
+        fn from(value: msg::PositionState) -> Self {
+            use msg::serial_input_msg::Payload;
+
+            Self {
+                payload: Some(Payload::Position(value)),
+            }
+        }
+    }
+
+    // manual example data:
+    // SerialInputMsg(RawMsgTx): 5632582b0040123e0a340202c173256817284029d5b3400f1feaf01ffffffc23b7743e00000fc0007e8138250737feebfff600000dffff7ffff1ce40400010011807220028020d0a
+    // SerialInputMsg(PositionState): 5632582b00170a1509a0bbe2a29f010000120a0d00002842150000b8410d0a
+
+    #[test]
+    fn uart_parser() {
+        // generate some test UART input messages
+        let raw_tx = msg::RawMsgTx {
+            payload: crate::testdata::CAM_SMALL.to_vec(),
+            message_type: 1,
+            gn_transport: msg::GnTransport::Shb.into(),
+            gn_area: msg::GnArea {
+                area: None,
+                position: None,
+                dist_a: None,
+                dist_b: None,
+                angle: None,
+            },
+            hop_limit: 2,
+            station_type: None,
+        };
+        let raw_tx_buf = serialize_serial_msg::<msg::SerialInputMsg>(&raw_tx.clone().into());
+
+        let pos_state = msg::PositionState {
+            timestamp_ms: 1785144196000,
+            position: msg::Position {
+                latitude_deg: 42.,
+                longitude_deg: 23.,
+            },
+            heading: None,
+            speed: None,
+        };
+        let pos_state_buf = serialize_serial_msg::<msg::SerialInputMsg>(&pos_state.clone().into());
+
+        let mut parser = Parser::default();
+
+        // feed individual complete messages to parser
+        let output = parser.parse(&raw_tx_buf);
+        assert_eq!(1, output.len());
+        assert_eq!(
+            &msg::SerialInputMsg::from(raw_tx.clone()),
+            output.get(0).unwrap()
+        );
+
+        let output = parser.parse(&pos_state_buf);
+        assert_eq!(1, output.len());
+        assert_eq!(
+            &msg::SerialInputMsg::from(pos_state.clone()),
+            output.get(0).unwrap()
+        );
+
+        // feed multiple complete messages to parser
+        let mut combined_buf = pos_state_buf.clone();
+        combined_buf.extend_from_slice(&raw_tx_buf);
+
+        let output = parser.parse(&combined_buf);
+        assert_eq!(2, output.len());
+        assert_eq!(
+            &msg::SerialInputMsg::from(pos_state.clone()),
+            output.get(0).unwrap()
+        );
+        assert_eq!(
+            &msg::SerialInputMsg::from(raw_tx.clone()),
+            output.get(1).unwrap()
+        );
+
+        // feed partial messages to parser
+        let buf1 = &raw_tx_buf[0..3];
+        let buf2 = &raw_tx_buf[3..];
+
+        let output = parser.parse(buf1);
+        assert_eq!(0, output.len());
+
+        let output = parser.parse(buf2);
+        assert_eq!(1, output.len());
+        assert_eq!(
+            &msg::SerialInputMsg::from(raw_tx.clone()),
+            output.get(0).unwrap()
+        );
+
+        // feed messages with garbage in between
+        let mut buf1 = vec![0xabu8, 0xcd]; // prepend some garbage
+        buf1.extend_from_slice(&raw_tx_buf[0..3]);
+        let mut buf2 = (&raw_tx_buf[3..]).to_vec();
+        buf2.extend_from_slice(&[0xfeu8, 0x42]); // append some garbage
+        buf2.extend_from_slice(&pos_state_buf); // append second message
+        buf2.extend_from_slice(&[0x11u8, 0x22]); // append some more garbage
+
+        let output = parser.parse(&buf1);
+        assert_eq!(0, output.len());
+
+        let output = parser.parse(&buf2);
+        assert_eq!(2, output.len());
+        assert_eq!(
+            &msg::SerialInputMsg::from(raw_tx.clone()),
+            output.get(0).unwrap()
+        );
+        assert_eq!(
+            &msg::SerialInputMsg::from(pos_state.clone()),
+            output.get(1).unwrap()
+        );
+    }
+}

src/main.rs

@@ -7,6 +7,13 @@
 )]
 // #![deny(clippy::large_stack_frames)]
 
+// we can't use UART I/O and GNSS at the same time
+#[cfg(all(feature = "uart", feature = "gnss"))]
+compile_error!("feature \"uart\" and feature \"gnss\" cannot be enabled at the same time");
+// autonomous CAM generation need a time and position source (aka _gnss)
+#[cfg(all(feature = "cam_tx", not(feature = "_gnss")))]
+compile_error!("feature \"cam_tx\" needs a time and position source (either GNSS or UART)");
+
 use alloc::string::ToString as _;
 use alloc::vec::Vec;
 use core::cell::RefCell;
@@ -35,17 +42,18 @@ extern crate alloc;
 mod applogic;
 mod cache;
 mod geo_alg;
+#[cfg(feature = "uart")]
 mod io;
 mod radio;
 #[cfg(feature = "screen")]
 mod screen;
-#[cfg(feature = "cam_tx")]
+#[cfg(any(feature = "cam_tx", feature = "uart"))]
 mod v2x_tx;
 
 const WIFI_CHANNEL: radio::Channel = 180;
 #[cfg(feature = "spat")]
 const SPAT_RATE_LIMIT: u8 = 5; // keep every n-th message
-#[cfg(feature = "cam_tx")]
+#[cfg(any(feature = "cam_tx", feature = "uart"))]
 const GN_IS_MOBILE: bool = true;
 #[cfg(feature = "cam_tx")]
 const OWN_STATION_TYPE: ItsStationType = ItsStationType::Cyclist;
@@ -60,7 +68,7 @@ const SERIAL_FIFO_SIZE_THLD: u8 = 100; // hardware has max. 128 byte
 // For more information see: <https://docs.espressif.com/projects/esp-idf/en/stable/esp32/api-reference/system/app_image_format.html#application-description>
 esp_bootloader_esp_idf::esp_app_desc!();
 
-#[cfg(feature = "gnss")]
+#[cfg(feature = "_gnss")]
 static GNSS_UPDATE: Mutex<RefCell<Option<applogic::GnssFix>>> = Mutex::new(RefCell::new(None));
 static WIFI_RX_QUEUE: Mutex<RefCell<Option<ArrayQueue<Vec<u8>>>>> = Mutex::new(RefCell::new(None));
 #[cfg(feature = "_uart")]
@@ -136,6 +144,18 @@ async fn main(spawner: Spawner) -> ! {
         screen
     };
 
+    // Setup UART I/O channel
+    #[cfg(feature = "uart")]
+    setup_uart0(
+        115_200,
+        Some(b'\n'),
+        peripherals.GPIO12,
+        peripherals.GPIO11,
+        peripherals.UART0,
+        serial_handler,
+    )
+    .expect("Failed to initialize UART");
+
     // Setup WiFi
     // sadly, it doesn't work any more when we move `esp_radio::wifi::new` to `setup_wifi_sniffer`
     let (mut wifi_controller, interfaces) = esp_radio::wifi::new(
@@ -178,24 +198,29 @@ async fn main(spawner: Spawner) -> ! {
     #[cfg(feature = "gnss")]
     info!("GNSS task started, waiting for GNSS fix...");
 
-    let mut state = applogic::State::new();
+    #[cfg(any(feature = "cam_tx", feature = "uart"))]
-    #[cfg(feature = "cam_tx")]
+    let (mut seq_no, mac_bytes) = {
-    let (mut seq_no, mut cam, mac_bytes) = {
-        let station_id = v2x_tx::make_station_id(mac);
-        info!("V2X Station ID: {station_id}");
-
         // unwrap is fine since [`esp_hal::efuse::MacAddress`] is just an `[u8; 6]`
         let mac_bytes = mac.as_bytes().try_into().unwrap();
 
-        let cam = applogic::cam_tx::CamState::new(
+        (0, mac_bytes)
+    };
+
+    let mut state = applogic::State::new();
+    #[cfg(feature = "cam_tx")]
+    let mut cam = {
+        let station_id = v2x_tx::make_station_id(mac);
+        info!("V2X Station ID: {station_id}");
+
+        applogic::cam_tx::CamState::new(
             station_id,
             OWN_STATION_TYPE,
             OWN_VEHICLE_LENGTH,
             OWN_VEHICLE_WIDTH,
-        );
+        )
-
-        (0, cam, mac_bytes)
     };
+    #[cfg(feature = "uart")]
+    let mut uart_input = io::Parser::default();
     loop {
         embassy_time::Timer::after(embassy_time::Duration::from_millis(1)).await;
 
@@ -214,11 +239,56 @@ async fn main(spawner: Spawner) -> ! {
             });
 
             if let Some(data) = new_data {
-                warn!("TODO: New UART data: {data:x?}");
+                #[cfg(feature = "uart")]
+                {
+                    for msg in uart_input.parse(&data) {
+                        match msg.payload {
+                            Some(io::msg::serial_input_msg::Payload::Position(fix)) => {
+                                let pos_state = fix.into();
+
+                                critical_section::with(|cs| {
+                                    let gnss_update_ref = GNSS_UPDATE.borrow(cs);
+                                    gnss_update_ref.replace(Some(pos_state));
+                                });
+                            }
+                            Some(io::msg::serial_input_msg::Payload::TxMsg(tx_request)) => {
+                                use v2x_tx::GnItsPayload as _;
+
+                                info!(
+                                    "UART: Got new TX message: Type {} with {} bytes",
+                                    tx_request.message_type,
+                                    tx_request.payload.len()
+                                );
+                                let own_position = state.pos();
+                                let time = state.time().and_utc();
+
+                                match tx_request.encode_packet(
+                                    mac_bytes,
+                                    own_position,
+                                    time,
+                                    GN_IS_MOBILE,
+                                    &mut seq_no,
+                                ) {
+                                    Ok(data) => {
+                                        if let Err(err) = radio::send_80211_bcast_frame(
+                                            &mut wlan_iface,
+                                            &mac_bytes.clone(),
+                                            &data,
+                                        ) {
+                                            warn!("Failed to send 802.11 frame: {err}");
+                                        }
+                                    }
+                                    Err(err) => warn!("Failed to create GN message: {err}"),
+                                }
+                            }
+                            None => {}
+                        }
+                    }
+                }
             }
         }
 
-        #[cfg(feature = "gnss")]
+        #[cfg(feature = "_gnss")]
         {
             let mut new_position = false;
             critical_section::with(|cs| {
@@ -245,7 +315,7 @@ async fn main(spawner: Spawner) -> ! {
                             Ok(data) => {
                                 if let Err(err) = radio::send_80211_bcast_frame(
                                     &mut wlan_iface,
-                                    mac.as_bytes(),
+                                    &mac_bytes.clone(),
                                     &data,
                                 ) {
                                     warn!("Failed to send 802.11 frame: {err}");
@@ -472,6 +542,18 @@ fn handle_frame(frame: wifi::sniffer::PromiscuousPkt<'_>) {
                         error!("V2X RX queue is full");
                     }
                 });
+
+                // Send data to UART
+                #[cfg(feature = "uart")]
+                {
+                    let message = io::msg::RawMsgRx {
+                        msg_type: message_id.as_u8().into(),
+                        payload: frame.data.to_vec(),
+                    }
+                    .serialize_uart_proto();
+
+                    send_uart0(&message);
+                }
             }
         }
     }

src/test.rs

@@ -6,6 +6,7 @@ extern crate alloc;
 mod applogic;
 mod cache;
 mod geo_alg;
+mod io;
 mod radio;
 mod testdata;
 mod v2x_tx;

src/testdata.rs

@@ -455,3 +455,11 @@ pub const CAM: &[u8] = &[
     0x00, 0x06, 0xc6, 0x70, 0x00, 0xdf, 0x80, 0x8d, 0x5f, 0xde, 0x66, 0x27, 0x00, 0x07, 0x3c, 0x04,
     0x76, 0xfd, 0x67, 0x31, 0x9c, 0x00, 0x58, 0x60, 0x41, 0x37, 0xd3, 0x15, 0x89, 0xc0, 0x06, 0xdc,
 ];
+
+#[cfg(any(feature = "cam_tx", feature = "uart"))]
+pub const CAM_SMALL: &[u8] = &[
+    0x2, 0x2, 0xc1, 0x73, 0x25, 0x68, 0x17, 0x28, 0x40, 0x29, 0xd5, 0xb3, 0x40, 0xf, 0x1f, 0xea,
+    0xf0, 0x1f, 0xff, 0xff, 0xfc, 0x23, 0xb7, 0x74, 0x3e, 0x0, 0x0, 0xf, 0xc0, 0x0, 0x7e, 0x81,
+    0x38, 0x25, 0x7, 0x37, 0xfe, 0xeb, 0xff, 0xf6, 0x0, 0x0, 0xd, 0xff, 0xff, 0x7f, 0xff, 0xf1,
+    0xce, 0x40, 0x40, 0x0,
+];

src/v2x_tx/gn.rs

@@ -67,7 +67,7 @@ pub fn make_gbc_eh(
     )
 }
 
-fn make_lpv(
+pub fn make_lpv(
     address: [u8; 6],
     station_type: geonetworking::en302636_4_1::StationType,
     own_position: &crate::applogic::PositionState,

src/v2x_tx/mod.rs

@@ -6,7 +6,7 @@ use c_its_parser::gn::{self as geonetworking, Encode};
 
 pub mod gn;
 
-#[cfg(feature = "esp")]
+#[cfg(all(feature = "esp", feature = "cam_tx"))]
 /// Creates a station ID from the WLAN MAC address
 ///
 /// Will use `0xC173` as static prefix and uses last 16 bit from MAC address for the remainder.
@@ -40,11 +40,14 @@ pub trait GnItsPayload {
         own_position: &crate::applogic::PositionState,
         time: chrono::DateTime<chrono::Utc>,
         seq_no: &mut u16,
-    ) -> (
+    ) -> Result<
+        (
             geonetworking::en302636_4_1::ExtendedHeader,
             Option<geonetworking::en302636_4_1::AreaType>,
             u8,
-    );
+        ),
+        alloc::string::String,
+    >;
 
     /// Creates the ITS payload (without BTP header)
     fn make_payload(
@@ -74,7 +77,8 @@ pub trait GnItsPayload {
         let payload_length = (payload.len()) as u16;
 
         // get extended header (depending on message type and sometimes content as well)
-        let (eh, area_type, maximum_hop_limit) = self.make_eh(address, own_position, time, seq_no);
+        let (eh, area_type, maximum_hop_limit) =
+            self.make_eh(address, own_position, time, seq_no)?;
 
         // Build GN headers
         let traffic_class = geonetworking::en302636_4_1::TrafficClass::try_new(