Enable V2X transmission, add optional stand-alone CAM transmission

src/applogic/cam_tx.rs

@@ -0,0 +1,197 @@
+//! C-ITS CAM Transmission
+
+use alloc::vec::Vec;
+
+use c_its_parser::gn as geonetworking;
+use c_its_parser::standards::cam_1_4_1::cam_pdu_descriptions;
+use c_its_parser::standards::cdd_1_3_1_1::its_container;
+use c_its_parser::standards::extensions;
+
+#[derive(Debug)]
+pub struct CamState {
+    station_id: u32,
+    station_type: its_container::StationType,
+
+    vehicle_length_dm: u16, // vehicle length is in 10cm steps
+    vehicle_width_dm: u8,   // vehicle width is in 10cm steps
+
+    time: chrono::DateTime<chrono::Utc>,
+    pos_state: super::PositionState,
+}
+
+impl CamState {
+    pub fn new(
+        station_id: u32,
+        station_type: c_its_parser::standards::extensions::ItsStationType,
+        vehicle_length_dm: u16,
+        vehicle_width_dm: u8,
+    ) -> Self {
+        Self {
+            station_id,
+            station_type: station_type.into(),
+            vehicle_length_dm,
+            vehicle_width_dm,
+            time: chrono::DateTime::<chrono::Utc>::default(),
+            pos_state: super::PositionState::default(),
+        }
+    }
+
+    pub fn update(&mut self, time: chrono::DateTime<chrono::Utc>, pos: super::PositionState) {
+        self.time = time;
+        self.pos_state = pos;
+
+        // TODO: build CAM trace
+    }
+
+    #[allow(clippy::too_many_lines)]
+    fn make_cam(&self) -> Result<cam_pdu_descriptions::CAM, alloc::string::String> {
+        let header = its_container::ItsPduHeader::new(
+            2,
+            extensions::ItsMessageId::Cam.as_u8(),
+            its_container::StationID(self.station_id),
+        );
+
+        let generation_delta_time = {
+            let its_timestamp = c_its_parser::time_utils::TimestampIts::from(self.time);
+
+            #[allow(clippy::cast_possible_truncation)]
+            let ts_mod = (its_timestamp.0 % 65_536) as u16;
+
+            cam_pdu_descriptions::GenerationDeltaTime(ts_mod)
+        };
+
+        let heading = match self.pos_state.heading_deg {
+            Some(value) => its_container::Heading {
+                heading_value: its_container::HeadingValue::from_deg(value)
+                    .map_err(|err| alloc::format!("Failed to make HeadingValue: {err}"))?,
+                heading_confidence: its_container::HeadingConfidence(127), // unavailable for now
+            },
+            None => its_container::Heading {
+                heading_value: its_container::HeadingValue(0),
+                heading_confidence: its_container::HeadingConfidence(127), // unavailable for now
+            },
+        };
+        let speed = match self.pos_state.speed_mps {
+            Some(value) => its_container::Speed {
+                speed_value: its_container::SpeedValue::from_mps(value)
+                    .map_err(|err| alloc::format!("Failed to make SpeedValue: {err}"))?,
+                speed_confidence: its_container::SpeedConfidence(127), // unavailable for now
+            },
+            None => its_container::Speed {
+                speed_value: its_container::SpeedValue(0),
+                speed_confidence: its_container::SpeedConfidence(127), // unavailable for now
+            },
+        };
+        let reference_position = its_container::ReferencePosition {
+            latitude: its_container::Latitude::from_deg(self.pos_state.position.y()),
+            longitude: its_container::Longitude::from_deg(self.pos_state.position.x()),
+            position_confidence_ellipse: its_container::PosConfidenceEllipse {
+                semi_major_confidence: its_container::SemiAxisLength(4095), // unavailable for now
+                semi_minor_confidence: its_container::SemiAxisLength(4095), // unavailable for now
+                semi_major_orientation: its_container::HeadingValue::unavailable(),
+            },
+            altitude: its_container::Altitude {
+                altitude_value: its_container::AltitudeValue(800_001), // unavailable for now
+                altitude_confidence: its_container::AltitudeConfidence::unavailable,
+            },
+        };
+
+        let basic_container = cam_pdu_descriptions::BasicContainer::new(
+            self.station_type.clone(),
+            reference_position,
+        );
+        let high_frequency_container =
+            cam_pdu_descriptions::HighFrequencyContainer::basicVehicleContainerHighFrequency(
+                cam_pdu_descriptions::BasicVehicleContainerHighFrequency {
+                    heading,
+                    speed,
+                    drive_direction: its_container::DriveDirection::unavailable,
+                    vehicle_length: its_container::VehicleLength {
+                        vehicle_length_value: its_container::VehicleLengthValue(
+                            self.vehicle_length_dm,
+                        ),
+                        vehicle_length_confidence_indication:
+                            its_container::VehicleLengthConfidenceIndication::unavailable,
+                    },
+                    vehicle_width: its_container::VehicleWidth(self.vehicle_width_dm),
+                    longitudinal_acceleration: its_container::LongitudinalAcceleration {
+                        longitudinal_acceleration_value:
+                            its_container::LongitudinalAccelerationValue::unavailable(),
+                        longitudinal_acceleration_confidence: its_container::AccelerationConfidence(
+                            102, // unavailable
+                        ),
+                    },
+                    curvature: its_container::Curvature {
+                        curvature_value: its_container::CurvatureValue::unavailable(),
+                        curvature_confidence: its_container::CurvatureConfidence::unavailable,
+                    },
+                    curvature_calculation_mode:
+                        its_container::CurvatureCalculationMode::unavailable,
+                    yaw_rate: its_container::YawRate {
+                        yaw_rate_value: its_container::YawRateValue::unavailable(),
+                        yaw_rate_confidence: its_container::YawRateConfidence::unavailable,
+                    },
+                    acceleration_control: None,
+                    lane_position: None,
+                    steering_wheel_angle: None,
+                    lateral_acceleration: None,
+                    vertical_acceleration: None,
+                    performance_class: None,
+                    cen_dsrc_tolling_zone: None,
+                },
+            );
+        let low_frequency_container =
+            cam_pdu_descriptions::LowFrequencyContainer::basicVehicleContainerLowFrequency(
+                cam_pdu_descriptions::BasicVehicleContainerLowFrequency {
+                    vehicle_role: its_container::VehicleRole::default,
+                    exterior_lights: its_container::ExteriorLights::default(),
+                    path_history: its_container::PathHistory(alloc::vec![]), // TODO: fill trace
+                },
+            );
+
+        let cam_parameters = cam_pdu_descriptions::CamParameters::new(
+            basic_container,
+            high_frequency_container,
+            Some(low_frequency_container),
+            None,
+        );
+
+        Ok(cam_pdu_descriptions::CAM::new(
+            header,
+            cam_pdu_descriptions::CoopAwareness::new(generation_delta_time, cam_parameters),
+        ))
+    }
+}
+
+impl crate::v2x_tx::GnItsPayload for CamState {
+    fn make_eh(
+        &self,
+        address: [u8; 6],
+        own_position: &crate::applogic::PositionState,
+        time: chrono::DateTime<chrono::Utc>,
+        _seq_no: &mut u16,
+    ) -> (
+        geonetworking::en302636_4_1::ExtendedHeader,
+        Option<geonetworking::en302636_4_1::AreaType>,
+        u8,
+    ) {
+        let station_type = geonetworking::en302636_4_1::StationType::try_from(self.station_type.0)
+            .unwrap_or_default();
+
+        // CAM always uses SHB, hop-limit 1
+        (
+            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>
+    {
+        let msg = self.make_cam()?;
+        let uper = msg.encode_to_uper()?;
+        Ok((uper, c_its_parser::standards::extensions::ItsMessageId::Cam))
+    }
+}

src/applogic/mod.rs

@@ -18,6 +18,8 @@ pub mod v2x;
 
 #[cfg(feature = "cam")]
 pub mod cam;
+#[cfg(feature = "cam_tx")]
+pub mod cam_tx;
 #[cfg(feature = "denm")]
 pub mod denm;
 #[cfg(feature = "spat")]

src/main.rs

@@ -14,6 +14,8 @@ use core::cell::RefCell;
 use core::cell::UnsafeCell;
 
 use c_its_parser::gn as geonetworking;
+#[cfg(feature = "cam_tx")]
+use c_its_parser::standards::extensions::ItsStationType;
 use critical_section::Mutex;
 use crossbeam_queue::ArrayQueue;
 use embassy_executor::Spawner;
@@ -37,10 +39,20 @@ mod io;
 mod radio;
 #[cfg(feature = "screen")]
 mod screen;
+#[cfg(feature = "cam_tx")]
+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")]
+const GN_IS_MOBILE: bool = true;
+#[cfg(feature = "cam_tx")]
+const OWN_STATION_TYPE: ItsStationType = ItsStationType::Cyclist;
+#[cfg(feature = "cam_tx")]
+const OWN_VEHICLE_WIDTH: u8 = 5; // in 10cm steps!
+#[cfg(feature = "cam_tx")]
+const OWN_VEHICLE_LENGTH: u16 = 20; // in 10cm steps!
 
 // This creates a default app-descriptor required by the esp-idf bootloader.
 // For more information see: <https://docs.espressif.com/projects/esp-idf/en/stable/esp32/api-reference/system/app_image_format.html#application-description>
@@ -119,7 +131,7 @@ async fn main(spawner: Spawner) -> ! {
 
     // Setup WiFi
     // sadly, it doesn't work any more when we move `esp_radio::wifi::new` to `setup_wifi_sniffer`
-    let (mut wifi_controller, mut interfaces) = esp_radio::wifi::new(
+    let (mut wifi_controller, interfaces) = esp_radio::wifi::new(
         peripherals.WIFI,
         esp_radio::wifi::ControllerConfig::default(),
     )
@@ -131,7 +143,8 @@ async fn main(spawner: Spawner) -> ! {
     critical_section::with(|cs| {
         WIFI_RX_QUEUE.borrow(cs).replace(Some(ArrayQueue::new(2)));
     });
-    radio::setup_wifi_sniffer(WIFI_CHANNEL, &mut interfaces.sniffer, handle_frame)
+    let mut wlan_iface = interfaces.sniffer;
+    radio::setup_wifi_sniffer(WIFI_CHANNEL, &mut wlan_iface, handle_frame)
         .expect("Fatal error initializing 802.11p sniffer");
 
     info!("WiFi promiscuous mode on channel {WIFI_CHANNEL} running");
@@ -159,6 +172,23 @@ async fn main(spawner: Spawner) -> ! {
     info!("GNSS task started, waiting for GNSS fix...");
 
     let mut state = applogic::State::new();
+    #[cfg(feature = "cam_tx")]
+    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(
+            station_id,
+            OWN_STATION_TYPE,
+            OWN_VEHICLE_LENGTH,
+            OWN_VEHICLE_WIDTH,
+        );
+
+        (0, cam, mac_bytes)
+    };
     loop {
         embassy_time::Timer::after(embassy_time::Duration::from_millis(1)).await;
 
@@ -171,6 +201,33 @@ async fn main(spawner: Spawner) -> ! {
                 if let Some(fix) = gnss_update_ref.replace(None) {
                     state.update_with_gpsfix(&fix);
                     new_position = true;
+
+                    #[cfg(feature = "cam_tx")]
+                    {
+                        use v2x_tx::GnItsPayload;
+                        let time = state.time().and_utc();
+
+                        cam.update(time, fix.clone().into());
+
+                        match cam.encode_packet(
+                            mac_bytes,
+                            &fix.into(),
+                            time,
+                            GN_IS_MOBILE,
+                            &mut seq_no,
+                        ) {
+                            Ok(data) => {
+                                if let Err(err) = radio::send_80211_bcast_frame(
+                                    &mut wlan_iface,
+                                    mac.as_bytes(),
+                                    &data,
+                                ) {
+                                    warn!("Failed to send 802.11 frame: {err}");
+                                }
+                            }
+                            Err(err) => warn!("Failed to create CAM: {err}"),
+                        }
+                    }
                 }
             });
 
@@ -236,12 +293,15 @@ async fn main(spawner: Spawner) -> ! {
                             }
                         }
 
-                        #[cfg(feature = "cam")]
+                        #[cfg(any(feature = "cam", feature = "cam_tx"))]
                         ItsMessage::Cam {
                             geonetworking: _,
                             transport: _,
                             etsi,
-                        } => applogic::cam::handle_cam(&etsi),
+                        } => {
+                            #[cfg(feature = "cam")]
+                            applogic::cam::handle_cam(&etsi);
+                        }
 
                         #[cfg(feature = "denm")]
                         ItsMessage::DenmV2 {

src/v2x_tx/gn.rs

@@ -0,0 +1,94 @@
+//! Geonetworking utilities
+
+use c_its_parser::gn as geonetworking;
+
+#[allow(unused)]
+pub fn make_shb_eh(
+    address: [u8; 6],
+    station_type: geonetworking::en302636_4_1::StationType,
+    own_position: &crate::applogic::PositionState,
+    time: chrono::DateTime<chrono::Utc>,
+) -> geonetworking::en302636_4_1::ExtendedHeader {
+    let source_position_vector = make_lpv(address, station_type, own_position, time);
+
+    geonetworking::en302636_4_1::ExtendedHeader::SHB(
+        geonetworking::en302636_4_1::SingleHopBroadcast {
+            source_position_vector,
+            media_dependent_data: [0; 4],
+        },
+    )
+}
+
+#[allow(unused)]
+pub fn make_tsb_eh(
+    address: [u8; 6],
+    station_type: geonetworking::en302636_4_1::StationType,
+    own_position: &crate::applogic::PositionState,
+    time: chrono::DateTime<chrono::Utc>,
+    sequence_number: u16,
+) -> geonetworking::en302636_4_1::ExtendedHeader {
+    let source_position_vector = make_lpv(address, station_type, own_position, time);
+
+    geonetworking::en302636_4_1::ExtendedHeader::TSB(
+        geonetworking::en302636_4_1::TopologicallyScopedBroadcast::new(
+            sequence_number,
+            source_position_vector,
+        ),
+    )
+}
+
+#[allow(unused)]
+pub fn make_gbc_eh(
+    address: [u8; 6],
+    station_type: geonetworking::en302636_4_1::StationType,
+    own_position: &crate::applogic::PositionState,
+    time: chrono::DateTime<chrono::Utc>,
+    target_pos: geo_types::Point,
+    radius_m: u16,
+    sequence_number: u16,
+) -> geonetworking::en302636_4_1::ExtendedHeader {
+    let source_position_vector = make_lpv(address, station_type, own_position, time);
+    #[allow(clippy::cast_possible_truncation)]
+    let latitude_deg = target_pos.x() as f32;
+    #[allow(clippy::cast_possible_truncation)]
+    let longitude_deg = target_pos.y() as f32;
+
+    geonetworking::en302636_4_1::ExtendedHeader::GBC(
+        geonetworking::en302636_4_1::GeoBroadcast::try_from_values(
+            sequence_number,
+            source_position_vector,
+            latitude_deg,
+            longitude_deg,
+            radius_m,
+            0,
+            0,
+        )
+        .expect("Failed to create circular GBC"),
+    )
+}
+
+fn make_lpv(
+    address: [u8; 6],
+    station_type: geonetworking::en302636_4_1::StationType,
+    own_position: &crate::applogic::PositionState,
+    time: chrono::DateTime<chrono::Utc>,
+) -> geonetworking::en302636_4_1::LongPositionVector {
+    let timestamp_its = c_its_parser::time_utils::TimestampIts::from(time);
+    #[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 speed_mps = own_position.speed_mps.unwrap_or_default();
+    let heading_deg = own_position.heading_deg.unwrap_or_default();
+
+    geonetworking::en302636_4_1::LongPositionVector::try_from_values(
+        geonetworking::en302636_4_1::Address::new(false, station_type, address),
+        timestamp_its.0,
+        latitude_deg,
+        longitude_deg,
+        true,
+        speed_mps,
+        heading_deg,
+    )
+    .expect("Failed to create LongPositionVector")
+}

src/v2x_tx/mod.rs

@@ -0,0 +1,160 @@
+//! V2X Transmission
+
+use alloc::vec::Vec;
+
+use c_its_parser::gn::{self as geonetworking, Encode};
+
+pub mod gn;
+
+#[cfg(feature = "esp")]
+/// 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.
+pub fn make_station_id(mac: esp_hal::efuse::MacAddress) -> u32 {
+    // unwrap is fine here since we can be sure that the MacAddress is 6 byte
+    // and a slice of 2 bytes can be converted to [u8; 2]
+    let mac_last_16bit = u16::from_be_bytes(mac.as_bytes().get(4..6).unwrap().try_into().unwrap());
+
+    0xC173_0000 | u32::from(mac_last_16bit)
+}
+
+pub trait GnItsPayload {
+    // common GN parameters
+    const LIFETIME_MILLIS: u32 = 60_000; // itsGnDefaultPacketLifetime is 60 seconds according to ETSI EN 302 636-4-1 V1.4.1
+
+    // ---------------------------------------------
+    // methods to build a GN packet with ITS payload
+    // ---------------------------------------------
+
+    /// Builds the [`geonetworking::en302636_4_1::ExtendedHeader`] and additional information for the ITS message
+    ///
+    /// Output:
+    /// - Extended header
+    /// - Area type (when using GAC or GBC)
+    /// - hop limit
+    ///
+    /// Note: Make sure to increase the sequence number on multi-hop packets.
+    fn make_eh(
+        &self,
+        address: [u8; 6],
+        own_position: &crate::applogic::PositionState,
+        time: chrono::DateTime<chrono::Utc>,
+        seq_no: &mut u16,
+    ) -> (
+        geonetworking::en302636_4_1::ExtendedHeader,
+        Option<geonetworking::en302636_4_1::AreaType>,
+        u8,
+    );
+
+    /// Creates the ITS payload (without BTP header)
+    fn make_payload(
+        &self,
+    ) -> Result<(Vec<u8>, c_its_parser::standards::extensions::ItsMessageId), alloc::string::String>;
+
+    // ---------------------------------------------
+    // common methods (with implementation)
+    // ---------------------------------------------
+
+    /// Encodes the GN packet
+    fn encode_packet(
+        &self,
+        address: [u8; 6],
+        own_position: &crate::applogic::PositionState,
+        time: chrono::DateTime<chrono::Utc>,
+        is_mobile: bool,
+        seq_no: &mut u16,
+    ) -> Result<Vec<u8>, alloc::string::String> {
+        // Build payload (including BTP header)
+        let (mut its_payload, msg_type) = self.make_payload()?;
+        let mut payload = make_btp_b(msg_type).encode()?;
+
+        payload.append(&mut its_payload);
+
+        #[allow(clippy::cast_possible_truncation)]
+        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);
+
+        // Build GN headers
+        let traffic_class = geonetworking::en302636_4_1::TrafficClass::try_new(
+            false,
+            false,
+            make_traffic_class(msg_type),
+        )
+        .map_err(|err| alloc::format!("Failed to create TrafficClass: {err:?}"))?;
+        let common = geonetworking::en302636_4_1::CommonHeader::from_values(
+            geonetworking::en302636_4_1::NextAfterCommon::BTPB,
+            geonetworking::en302636_4_1::HeaderType::new_from(&eh, area_type)?,
+            traffic_class,
+            is_mobile,
+            payload_length,
+            maximum_hop_limit,
+        );
+
+        let remaining_hop_limit = maximum_hop_limit;
+        let basic = geonetworking::en302636_4_1::BasicHeader::try_new(
+            1,
+            geonetworking::en302636_4_1::NextAfterBasic::CommonHeader,
+            geonetworking::en302636_4_1::Lifetime::from_milliseconds(Self::LIFETIME_MILLIS),
+            remaining_hop_limit,
+        )
+        .map_err(|err| alloc::format!("Failed to create BasicHeader: {err}"))?;
+
+        // Assemble and encode packet
+        let packet = geonetworking::Packet::Unsecured {
+            basic,
+            common,
+            extended: Some(eh),
+            payload: &payload,
+        };
+
+        packet
+            .encode_to_vec()
+            .map_err(|err| alloc::format!("Failed to encode GN packet: {err:?}"))
+    }
+}
+
+/// Creates a BTP-B header based on the ITS message ID (message type)
+///
+/// Port numbers and destination port info according to ETSI TS 103 248
+fn make_btp_b(
+    msg_type: c_its_parser::standards::extensions::ItsMessageId,
+) -> c_its_parser::transport::TransportHeader {
+    let (destination_port, destination_port_info) = match msg_type {
+        c_its_parser::standards::extensions::ItsMessageId::Denm => (2002, 0),
+        c_its_parser::standards::extensions::ItsMessageId::Cam => (2001, 0),
+        c_its_parser::standards::extensions::ItsMessageId::Mapem => (2003, 0),
+        c_its_parser::standards::extensions::ItsMessageId::Spatem => (2004, 0),
+        c_its_parser::standards::extensions::ItsMessageId::Saem => (2005, 0),
+        c_its_parser::standards::extensions::ItsMessageId::Ivim => (2006, 0),
+        c_its_parser::standards::extensions::ItsMessageId::Srem => (2007, 0),
+        c_its_parser::standards::extensions::ItsMessageId::Ssem => (2008, 0),
+        c_its_parser::standards::extensions::ItsMessageId::Cpm => (2009, 0),
+        c_its_parser::standards::extensions::ItsMessageId::Poi => (2010, 0),
+        c_its_parser::standards::extensions::ItsMessageId::EvRsr => (2012, 0),
+        c_its_parser::standards::extensions::ItsMessageId::Rtcmem => (2013, 0),
+        _ => (0, 0), // no information found for the subsequent types
+    };
+
+    c_its_parser::transport::TransportHeader::BtpB(c_its_parser::transport::BasicTransportBHeader {
+        destination_port,
+        destination_port_info,
+    })
+}
+
+/// Creates a traffic class ID based on the ITS message ID (message type)
+fn make_traffic_class(msg_type: c_its_parser::standards::extensions::ItsMessageId) -> u8 {
+    #[allow(clippy::match_same_arms)]
+    match msg_type {
+        c_its_parser::standards::extensions::ItsMessageId::Denm => 1,
+        c_its_parser::standards::extensions::ItsMessageId::Cam => 2,
+        c_its_parser::standards::extensions::ItsMessageId::Mapem => 3, // from C-Roads RS_RSP_063(1)
+        c_its_parser::standards::extensions::ItsMessageId::Spatem => 3, // from C-Roads RS_RSP_063(1)
+        c_its_parser::standards::extensions::ItsMessageId::Ivim => 3, // from C-Roads RS_RSP_063(1)
+        c_its_parser::standards::extensions::ItsMessageId::Srem => 2, // from C-Roads RS_RSP_063(1)
+        c_its_parser::standards::extensions::ItsMessageId::Ssem => 2, // from C-Roads RS_RSP_063(1)
+        c_its_parser::standards::extensions::ItsMessageId::Rtcmem => 3, // CSP_MaxPrio=252 -> 3
+        _ => 3, // fall-back to lowest priority
+    }
+}