Enable V2X transmission, add optional stand-alone CAM transmission

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 {