MicrOBU/obu-cam-transmistter/main/main.c

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#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);
}