#include #include #include #include #include #include "freertos/FreeRTOS.h" #include "freertos/task.h" #include "esp_timer.h" #include "driver/gpio.h" #include "esp_wifi.h" #include "esp_event.h" #include "esp_netif.h" #include "nvs_flash.h" #include "esp_log.h" #include "hal/modem_syscon_ll.h" // modem_syscon_ll_enable_fe_40m_clock() - see initialize_wifi #include "denm.h" #include "cam.h" #include "geonet.h" #include "dot11p.h" #include "tx_custom.h" #include "route.h" #include "route_points.h" static const char *TAG = "obu-tx"; // CAM beacon for a simulated car driving round a block in Hamburg (see route.c). The CAM // generation rules follow ETSI EN 302 637-2 clause 6.1.3: every CHECK_INTERVAL_MS the car's state // is compared with the last CAM sent, and a new CAM goes out when the heading changed by more than // 4 degrees, the position by more than 4 m, the speed by more than 0.5 m/s, or 1 s has passed. // There is no hazard-light gating - CAM is a continuous beacon, unlike the event-triggered DENM. // Transmits on 5900 MHz like the working Rust reference (esp32-c_its-companion, feat/tx-cam). // ISOLATION TEST for whether tx_custom.c is the blocker. // 1 = transmit via the STANDARD, well-tested esp_wifi_80211_tx() using a // plain (non-QoS) Data frame, which that API accepts. This path is known // to actually key the PA. If the sniffer sees frames with this = 1 but // not with = 0, then tx_custom.c (its reverse-engineered driver-struct // offsets) is the problem, not the RF/channel/regulatory setup. // 0 = original path: QoS Data frame via esp_wifi_80211_tx_custom(). // Non-QoS Data is non-standard for ITS-G5, but this is purely a "does any RF // leave the chip" test - your capture-all sniffer logs it regardless. // // A/B TEST for the bursty-SDR symptom. Console is stable and tx_custom returns // OK every second, but the SDR only sees sporadic bursts - the fingerprint of // tx_custom.c's reverse-engineered driver-struct offsets not matching THIS IDF // (v5.5.4) as opposed to the reference's bundled IDF. Setting this to 1 routes // TX through the official, well-tested esp_wifi_80211_tx() (non-QoS Data), which // uses NO reverse-engineered structs. If the SDR becomes a steady 1 Hz with // this = 1, tx_custom's struct layout is confirmed as the culprit. #define USE_STANDARD_TX 1 // Target frequency: 5900 MHz (ITS-G5 G5-CCH, channel 180). This is what the // working Rust reference transmits on, proving the C5 PA reaches it despite the // 5885 datasheet max. The reference sets band-mode 5G, then phy_11p_set + // phy_change_channel(5900) directly - it does NOT call esp_wifi_set_channel at // all, so we don't either (channel 180 isn't a normal Wi-Fi channel anyway). #define TX_FREQ_MHZ 5900 // ---------------------------------------------------------------------------- // ---- CAM beacon profile ---- #define STATION_ID 0x0BADC0DE // placeholder 32-bit station id - pick your own #define STATION_TYPE 5 // passengerCar (TS 102 894-2 StationType) #define VEHICLE_LENGTH_DM 40 // VehicleLengthValue, 10cm steps (4.0 m) #define VEHICLE_WIDTH_DM 18 // VehicleWidth, 10cm steps (1.8 m) #define BTP_PORT_CAM 2001 // BTP-B destination port for CAM (ETSI TS 103 248) #define CHECK_INTERVAL_MS 100 // T_CheckCamGen: how often the generation rules are evaluated #define CAM_MAX_INTERVAL_MS 1000 // T_GenCamMax: a CAM goes out at least this often #define CAM_HEADING_DDEG 40 // > 4 degrees heading change triggers a CAM #define CAM_POSITION_M 4.0 // > 4 m position change triggers a CAM #define CAM_SPEED_CM_S 50 // > 0.5 m/s speed change triggers a CAM // ---- Simulated drive ---- // route_points (main/route_points.h) is the street geometry of a driving loop through six waypoints // in St. Georg, generated by tools/make_route.py from OpenStreetMap via OSRM. To change the route, // edit WAYPOINTS in that script and rerun it. No GNSS is wired in; replace with real fixes once // there is one. #define CRUISE_MPS (50.0 / 3.6) // 50 km/h, the urban limit #define MIN_CORNER_MPS (10.0 / 3.6) // slowest the car goes, for hairpins and U-turns // Single source of truth for the pseudonym/link-layer address: used both as // the 802.11 source MAC (Addr2) and as GN_ADDR's MID field, since the GN // spec defines those as being the same address. Locally-administered bit // set (0x02) per normal MAC convention. Fixed/non-rotating for now - real // stacks rotate this every 5-15 min for privacy. static const uint8_t pseudonym_mac[6] = {0x02, 0x00, 0x00, 0x00, 0x00, 0x01}; // Undocumented libphy.a calls that push the radio into 802.11p OCB mode on // the 5.9 GHz ITS-G5 band. See docs/04-transmit-setup.md for source + what // to do if the linker can't find these symbols in your ESP-IDF version. extern void phy_11p_set(int enable, int unused); extern void phy_change_channel(int freq_mhz, int bw_mode, int sec_chan_offset, int unused); static void send_cam(const route_state_t *car, uint16_t gen_delta) { uint8_t frame[300]; cam_fields_t fields = { .station_id = STATION_ID, .station_type = STATION_TYPE, .generation_delta_time = gen_delta, .latitude_tenmicrodeg = car->latitude_tenmicrodeg, .longitude_tenmicrodeg = car->longitude_tenmicrodeg, .speed_cm_s = car->speed_cm_s, .heading_ddeg = car->heading_ddeg, .vehicle_length_dm = VEHICLE_LENGTH_DM, .vehicle_width_dm = VEHICLE_WIDTH_DM, }; uint8_t cam_payload[96]; int cam_len = cam_encode(&fields, cam_payload, sizeof(cam_payload)); uint8_t gn_payload[160]; int gn_len = geonet_wrap_shb(cam_payload, cam_len, pseudonym_mac, STATION_TYPE, car->latitude_tenmicrodeg, car->longitude_tenmicrodeg, car->speed_cm_s, car->heading_ddeg, BTP_PORT_CAM, gn_payload, sizeof(gn_payload)); // qos=false for the standard-TX path (esp_wifi_80211_tx accepts only non-QoS // Data - which is exactly what the Rust reference transmits); qos=true would // be a real ITS-G5 QoS Data frame for the tx_custom path. int frame_len = dot11p_build_frame(gn_payload, gn_len, pseudonym_mac, frame, sizeof(frame), USE_STANDARD_TX ? false : true); // PHY/OCB/channel is configured ONCE at boot in app_main and left alone, // matching the working Rust reference (band-mode 5G + phy_11p_set + // phy_change_channel(5900), set once). if (frame_len > 0) { #if USE_STANDARD_TX // Standard, well-tested raw-TX API with a non-QoS Data frame - the same // transmit path the Rust reference uses (esp-radio send_raw_frame wraps // esp_wifi_80211_tx). err 258 ("unsupport QoS frame type") would mean the // frame wasn't built as non-QoS. esp_err_t err = esp_wifi_80211_tx(WIFI_IF_STA, frame, frame_len, true); if (err != ESP_OK) { ESP_LOGW(TAG, "esp_wifi_80211_tx (standard) failed: %d", err); } else { ESP_LOGI(TAG, "CAM sent (%d bytes) @ %d MHz genDeltaT=%u pos=%.7f,%.7f %.1f km/h heading %.1f pt%d", frame_len, TX_FREQ_MHZ, gen_delta, car->latitude_tenmicrodeg / 1e7, car->longitude_tenmicrodeg / 1e7, car->speed_cm_s * 0.036, car->heading_ddeg / 10.0, car->segment + 1); } #else // tx_custom path: submits to the driver's internal HMAC TX path, // bypassing the QoS-frame gate. 11A legacy OFDM, 12M rate. wifi_tx_rate_config_t tx_rate_cfg = { .phymode = WIFI_PHY_MODE_11A, .rate = WIFI_PHY_RATE_12M, .ersu = false, .dcm = false, }; esp_err_t err = esp_wifi_80211_tx_custom(WIFI_IF_STA, frame, frame_len, true, &tx_rate_cfg, WIFI_BAND_5G, WIFI_BW20); if (err != ESP_OK) { ESP_LOGW(TAG, "esp_wifi_80211_tx_custom failed: %d", err); } else { ESP_LOGI(TAG, "CAM sent (%d bytes) @ %d MHz genDeltaT=%u", frame_len, TX_FREQ_MHZ, gen_delta); } #endif } else { ESP_LOGE(TAG, "CAM frame build failed (cam_len=%d gn_len=%d)", cam_len, gn_len); } } static bool cam_due(const route_state_t *car, const route_state_t *last, int64_t since_last_ms) { if (since_last_ms >= CAM_MAX_INTERVAL_MS) { return true; } int dh = abs((int)car->heading_ddeg - (int)last->heading_ddeg); if (dh > 1800) { dh = 3600 - dh; } if (dh > CAM_HEADING_DDEG) { return true; } if (abs((int)car->speed_cm_s - (int)last->speed_cm_s) > CAM_SPEED_CM_S) { return true; } // Flat-earth distance is plenty for a 4 m threshold. double north_m = (car->latitude_tenmicrodeg - last->latitude_tenmicrodeg) * 0.0111194930; double east_m = (car->longitude_tenmicrodeg - last->longitude_tenmicrodeg) * 0.0111194930 * cos(car->latitude_tenmicrodeg / 1e7 * M_PI / 180.0); return north_m * north_m + east_m * east_m > CAM_POSITION_M * CAM_POSITION_M; } static void tx_task(void *arg) { route_state_t car; route_state_t last_sent; int64_t last_sent_ms = 0; bool sent_any = false; TickType_t wake = xTaskGetTickCount(); route_step(0.0, &car); while (1) { int64_t now_ms = esp_timer_get_time() / 1000; if (!sent_any || cam_due(&car, &last_sent, now_ms - last_sent_ms)) { // GenerationDeltaTime is TimestampIts mod 65536 (ms). No real clock here, so use // milliseconds since boot, which advances at the right rate. send_cam(&car, (uint16_t)now_ms); last_sent = car; last_sent_ms = now_ms; sent_any = true; } vTaskDelayUntil(&wake, pdMS_TO_TICKS(CHECK_INTERVAL_MS)); route_step(CHECK_INTERVAL_MS / 1000.0, &car); } } void app_main(void) { ESP_ERROR_CHECK(nvs_flash_init()); ESP_ERROR_CHECK(esp_netif_init()); ESP_ERROR_CHECK(esp_event_loop_create_default()); // Enable the modem FRONT-END 40 MHz clock BEFORE esp_wifi_init(). This is // the one step the proven-working receiver firmware // (its-g5-receiver-firmware_txenabled, main/main.c -> initialize_wifi()) // performs that this OBU was missing. Without the FE clock enabled the // 5 GHz front-end / transmit chain is not fully clocked - which matches the // exact symptom here: the radio calibrates (boot RF ping) and receives // fine, but data frames are accepted by the API and never actually key the // PA. This is a low-level modem_syscon register write via the HAL LL layer, // copied verbatim from the reference firmware. modem_syscon_ll_enable_fe_40m_clock(&MODEM_SYSCON, 1); wifi_init_config_t wifi_cfg = WIFI_INIT_CONFIG_DEFAULT(); ESP_ERROR_CHECK(esp_wifi_init(&wifi_cfg)); ESP_ERROR_CHECK(esp_wifi_set_storage(WIFI_STORAGE_RAM)); // match reference initialize_wifi() ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA)); ESP_ERROR_CHECK(esp_wifi_start()); // ---- Regulatory / TX-authorization override ----------------------------- // THE fix for "RX works but TX is silent". By default the driver uses // WIFI_COUNTRY_POLICY_AUTO, whose 5 GHz regulatory table does NOT authorize // transmit on the 5.9 GHz ITS band (and treats DFS channels as no-IR / // radar-gated). Receiving is never gated - which is exactly why the sniffer // hears traffic but our own frames never key the PA, and why the only RF // seen from this board is the uninhibited PHY-calibration burst at boot. // // Switching to WIFI_COUNTRY_POLICY_MANUAL with an explicit 5 GHz channel // mask (wifi_5g_channel_mask, which only takes effect under manual policy) // tells the driver these channels are permitted and lifts the transmit // gate. WIFI_CHANNEL_177 (BIT(28)) = 5885 MHz; we enable the full 5 GHz set // (bits 1..28) so both the primer channel and the target are authorized. // Manual policy = the operator asserts regulatory responsibility, which is // appropriate for licensed/university research on the ITS band. wifi_country_t ctry = { .cc = "US", // nominal under manual policy .schan = 1, .nchan = 11, .policy = WIFI_COUNTRY_POLICY_MANUAL, .wifi_5g_channel_mask = 0x1FFFFFFE, // all 5 GHz channels, bits 1..28 (incl. 140 and 177) }; esp_err_t ctry_err = esp_wifi_set_country(&ctry); if (ctry_err != ESP_OK) { ESP_LOGW(TAG, "esp_wifi_set_country(MANUAL) failed: %d (continuing)", ctry_err); } // Ensure the PA runs at full configured power (not a reduced regulatory // default). Units are 0.25 dBm; 80 = 20 dBm. esp_wifi_set_max_tx_power(80); // ------------------------------------------------------------------------- // Force the dual-band C5 onto its 5 GHz PHY. This MUST be called after // esp_wifi_start() - calling it before returns ESP_ERR_WIFI_NOT_STARTED // (0x3002 / 12290). Locking the band to 5G explicitly keeps the driver // from ever falling back to 2.4 GHz ch1 (the old "stuck at primary=1" // symptom), which would key the wrong PHY and make us inaudible to a // 5.9 GHz sniffer. Valid 5 GHz channels on the C5 are 36..177. Not // ESP_ERROR_CHECK'd: log and continue if a given IDF build differs. esp_err_t band_err = esp_wifi_set_band_mode(WIFI_BAND_MODE_5G_ONLY); if (band_err != ESP_OK) { ESP_LOGW(TAG, "esp_wifi_set_band_mode(5G_ONLY) failed: %d (continuing)", band_err); } // Disable Wi-Fi power save. An unassociated STA with the default // WIFI_PS_MIN_MODEM power save sleeps its radio between beacons it will // never receive (we're not joined to any AP), and drops outbound raw // frames while asleep - the classic "esp_wifi_80211_tx returns OK but // nothing goes on air". Must be called after esp_wifi_start(). ESP_ERROR_CHECK(esp_wifi_set_ps(WIFI_PS_NONE)); // Enable promiscuous mode. This is the single most important change: our // *receiver* firmware (V2X2MAP) - which demonstrably works at 5.9 GHz, // 13k+ frames captured - runs promiscuous, and ESP-IDF documents that the // raw-frame TX path only actually emits when the MAC is in promiscuous // mode or associated to an AP. Plain STA (what this firmware used before) // is neither, so frames were being accepted by the API and then dropped // by the driver. Putting the OBU in the same radio state as the working // sniffer, then injecting, is the whole fix. Must be after start. ESP_ERROR_CHECK(esp_wifi_set_promiscuous(true)); // Force 802.11p OCB mode on the ITS-G5 channel, exactly like the working // Rust reference (esp32-c_its-companion, src/radio.rs setup_wifi_sniffer): // enable 802.11p, then jump straight to the target frequency. With band-mode // already locked to 5 GHz above, NO esp_wifi_set_channel priming is needed - // the reference doesn't call it, and channel 180 (5900 MHz) isn't a normal // Wi-Fi channel anyway. phy_change_channel takes the frequency in MHz. ESP_LOGI(TAG, "about to call phy_11p_set..."); phy_11p_set(1, 0); ESP_LOGI(TAG, "phy_11p_set returned, about to call phy_change_channel(%d)...", TX_FREQ_MHZ); phy_change_channel(TX_FREQ_MHZ, 1, 0, 0); ESP_LOGI(TAG, "phy_change_channel returned"); if (route_init(route_points, sizeof(route_points) / sizeof(route_points[0]), CRUISE_MPS, MIN_CORNER_MPS) != 0) { ESP_LOGE(TAG, "route_init failed - check route_points"); return; } ESP_LOGW(TAG, "OCB @ %d MHz - CAM beacon armed, driving a %d-point street loop", TX_FREQ_MHZ, (int)(sizeof(route_points) / sizeof(route_points[0]))); xTaskCreate(tx_task, "tx_task", 4096, NULL, 5, NULL); }