388 lines
13 KiB
Rust
388 lines
13 KiB
Rust
#![no_std]
|
|
#![no_main]
|
|
#![deny(
|
|
clippy::mem_forget,
|
|
reason = "mem::forget is generally not safe to do with esp_hal types, especially those \
|
|
holding buffers for the duration of a data transfer."
|
|
)]
|
|
// #![deny(clippy::large_stack_frames)]
|
|
|
|
use alloc::vec::Vec;
|
|
use core::cell::RefCell;
|
|
|
|
use critical_section::Mutex;
|
|
use crossbeam_queue::ArrayQueue;
|
|
use embassy_executor::Spawner;
|
|
#[cfg(feature = "gnss")]
|
|
use embassy_gps::gps::l76k;
|
|
use esp_backtrace as _;
|
|
use esp_hal::clock::CpuClock;
|
|
use esp_hal::timer::timg::TimerGroup;
|
|
#[cfg(feature = "screen")]
|
|
use esp_hal::{delay, gpio, spi, time};
|
|
#[cfg(all(target_arch = "riscv32", feature = "spat"))]
|
|
use esp_println::println;
|
|
use esp_radio::wifi;
|
|
use log::{debug, error, info, warn};
|
|
|
|
extern crate alloc;
|
|
|
|
mod applogic;
|
|
#[cfg(feature = "spat")]
|
|
mod cache;
|
|
mod geo_alg;
|
|
mod radio;
|
|
#[cfg(feature = "screen")]
|
|
mod screen;
|
|
|
|
const WIFI_CHANNEL: radio::Channel = 180;
|
|
|
|
// 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>
|
|
esp_bootloader_esp_idf::esp_app_desc!();
|
|
|
|
#[cfg(feature = "gnss")]
|
|
static GNSS_UPDATE: Mutex<RefCell<Option<embassy_gps::types::GpsFix>>> =
|
|
Mutex::new(RefCell::new(None));
|
|
static WIFI_RX_QUEUE: Mutex<RefCell<Option<ArrayQueue<Vec<u8>>>>> = Mutex::new(RefCell::new(None));
|
|
|
|
#[allow(
|
|
clippy::large_stack_frames,
|
|
reason = "it's not unusual to allocate larger buffers etc. in main"
|
|
)]
|
|
#[esp_rtos::main]
|
|
async fn main(spawner: Spawner) -> ! {
|
|
// generator version: 1.3.0
|
|
// generator parameters: --chip esp32c5 -o esp32c5-wroom-1-psram -o alloc -o log -o unstable-hal -o wifi -o esp-backtrace -o embassy
|
|
|
|
esp_println::logger::init_logger_from_env();
|
|
|
|
let config = esp_hal::Config::default().with_cpu_clock(CpuClock::max());
|
|
let peripherals = esp_hal::init(config);
|
|
|
|
// The following pins are used to bootstrap the chip. They are available
|
|
// for use, but check the datasheet of the module for more information on them.
|
|
// - GPIO2
|
|
// - GPIO3
|
|
// - GPIO7
|
|
// - GPIO25
|
|
// - GPIO26
|
|
// - GPIO27
|
|
// - GPIO28
|
|
|
|
esp_alloc::heap_allocator!(#[esp_hal::ram(reclaimed)] size: 65536);
|
|
esp_alloc::psram_allocator!(peripherals.PSRAM, esp_hal::psram);
|
|
|
|
let timg0 = TimerGroup::new(peripherals.TIMG0);
|
|
let sw_interrupt =
|
|
esp_hal::interrupt::software::SoftwareInterruptControl::new(peripherals.SW_INTERRUPT);
|
|
esp_rtos::start(timg0.timer0, sw_interrupt.software_interrupt0);
|
|
|
|
info!("Embassy initialized!");
|
|
|
|
// ST7789 172*320px screen
|
|
// SCL: D8/ GPIO8 (SPI_SCK)
|
|
// SDA: D10/ GPIO10 (SPI_MOSI)
|
|
// RES: D3/ GPIO7
|
|
// DC: D4/ GPIO23
|
|
// CS: D5/ GPIO24
|
|
// BL: TODO (hard-wire to high? or D1/ GPIO0)
|
|
#[cfg(feature = "screen")]
|
|
let mut spi_buffer = [0_u8; 512];
|
|
#[cfg(feature = "screen")]
|
|
let mut display = {
|
|
let (spi, cs) = setup_st7789_spi(
|
|
peripherals.GPIO8,
|
|
peripherals.GPIO10,
|
|
peripherals.GPIO24,
|
|
peripherals.GPIO9,
|
|
peripherals.SPI2,
|
|
)
|
|
.expect("Failed to build SPI config");
|
|
let output_config = gpio::OutputConfig::default();
|
|
let dc_output = gpio::Output::new(peripherals.GPIO23, gpio::Level::Low, output_config);
|
|
let rst_output = gpio::Output::new(peripherals.GPIO7, gpio::Level::High, output_config);
|
|
let _ = gpio::Output::new(peripherals.GPIO0, gpio::Level::High, output_config); // backlight
|
|
|
|
let mut display_delay = delay::Delay::new();
|
|
|
|
let spi_device = embedded_hal_bus::spi::ExclusiveDevice::new_no_delay(spi, cs)
|
|
.expect("Failed to initialize SPI device");
|
|
|
|
mipidsi::Builder::new(
|
|
mipidsi::models::ST7789,
|
|
mipidsi::interface::SpiInterface::new(spi_device, dc_output, &mut spi_buffer),
|
|
)
|
|
.display_size(screen::HEIGHT + 34, screen::WIDTH) // somehow this display has 34 px of invisible space on the left (in native orientation)
|
|
.reset_pin(rst_output)
|
|
.invert_colors(mipidsi::options::ColorInversion::Inverted)
|
|
.orientation(mipidsi::options::Orientation::new().rotate(mipidsi::options::Rotation::Deg90))
|
|
.init(&mut display_delay)
|
|
.expect("Failed to initialize display")
|
|
};
|
|
|
|
#[cfg(feature = "screen")]
|
|
let _ = screen::draw_slash_screen(&mut display).inspect_err(log_screen_error);
|
|
|
|
// Setup WiFi
|
|
let (_wifi_controller, interfaces) = esp_radio::wifi::new(
|
|
peripherals.WIFI,
|
|
esp_radio::wifi::ControllerConfig::default(),
|
|
)
|
|
.expect("Failed to initialize Wi-Fi controller");
|
|
|
|
critical_section::with(|cs| {
|
|
WIFI_RX_QUEUE.borrow(cs).replace(Some(ArrayQueue::new(2)));
|
|
});
|
|
|
|
let mut sniffer = interfaces.sniffer;
|
|
radio::setup_wifi_sniffer(WIFI_CHANNEL, &mut sniffer, handle_frame)
|
|
.expect("Fatal error initializing 802.11p sniffer");
|
|
|
|
info!("WiFi promiscuous mode on channel {WIFI_CHANNEL} running");
|
|
|
|
// Seeed XIAO L67K:
|
|
// RX: D7/ GPIO12
|
|
// TX: D6/ GPIO11
|
|
// WAKEUP: D0/ GPIO1 -> HIGH for active, LOW for Sleep
|
|
// RESET: D2/ GPIO25 -> HIGH for normal, LOW for Reset
|
|
#[cfg(feature = "gnss")]
|
|
spawner.spawn(
|
|
gnss_task(
|
|
peripherals.GPIO1,
|
|
peripherals.GPIO25,
|
|
peripherals.UART0,
|
|
peripherals.GPIO12,
|
|
peripherals.GPIO11,
|
|
)
|
|
.expect("Failed to spawn GNSS task"),
|
|
);
|
|
#[cfg(feature = "gnss")]
|
|
info!("GNSS task started, waiting for GNSS fix...");
|
|
|
|
let mut state = applogic::State::new();
|
|
loop {
|
|
embassy_time::Timer::after(embassy_time::Duration::from_millis(5)).await;
|
|
|
|
#[cfg(feature = "gnss")]
|
|
critical_section::with(|cs| {
|
|
let gnss_update_ref = GNSS_UPDATE.borrow(cs);
|
|
|
|
if let Some(fix) = gnss_update_ref.replace(None) {
|
|
state.update_with_gpsfix(&fix);
|
|
|
|
if fix.get_timestamp().is_some() {
|
|
info!("{}", state.print_fix());
|
|
|
|
// run GLOSA algorithm
|
|
#[cfg(feature = "spat")]
|
|
let signal_groups = state.run_glosa();
|
|
|
|
for sig in &signal_groups {
|
|
let duration_str = sig
|
|
.min_end_sec
|
|
.map(|v| alloc::format!("for {v} s"))
|
|
.unwrap_or_default();
|
|
println!("GLOSA: {} {} {duration_str}", sig.maneuver, sig.phase);
|
|
}
|
|
|
|
#[cfg(feature = "screen")]
|
|
let _ = screen::draw_glosa(&mut display, &signal_groups)
|
|
.inspect_err(log_screen_error);
|
|
}
|
|
}
|
|
});
|
|
|
|
critical_section::with(|cs| {
|
|
let queue_rc = WIFI_RX_QUEUE.borrow(cs).borrow();
|
|
// unwrap is fine b/c we stored something in it before
|
|
let queue = queue_rc.as_ref().unwrap();
|
|
|
|
if let Some(data) = queue.pop() {
|
|
match c_its_parser::de::decode(&data, c_its_parser::Headers::IEEE802LlcGnBtp) {
|
|
Ok(msg) => {
|
|
use c_its_parser::ItsMessage;
|
|
use c_its_parser::standards::extensions::ItsMessageId;
|
|
|
|
debug!("Got new {:?} message", ItsMessageId::from(&msg));
|
|
|
|
match msg {
|
|
#[cfg(feature = "spat")]
|
|
ItsMessage::Mapem {
|
|
geonetworking: _,
|
|
transport: _,
|
|
etsi,
|
|
} => state.handle_mapem(&etsi),
|
|
#[cfg(feature = "spat")]
|
|
ItsMessage::Spatem {
|
|
geonetworking: _,
|
|
transport: _,
|
|
etsi,
|
|
} => {
|
|
if state.initialized() {
|
|
state.handle_spatem(&etsi);
|
|
}
|
|
}
|
|
|
|
#[cfg(feature = "cam")]
|
|
ItsMessage::Cam {
|
|
geonetworking: _,
|
|
transport: _,
|
|
etsi,
|
|
} => applogic::cam::handle_cam(&etsi),
|
|
|
|
#[cfg(feature = "denm")]
|
|
ItsMessage::DenmV2 {
|
|
geonetworking: _,
|
|
transport: _,
|
|
etsi,
|
|
} => applogic::denm::handle_denm(&etsi),
|
|
#[cfg(feature = "denm")]
|
|
ItsMessage::DenmV1 {
|
|
geonetworking: _,
|
|
transport: _,
|
|
etsi: _,
|
|
} => {
|
|
// irrelevant since all received DENMs should be parsed as v2
|
|
}
|
|
}
|
|
}
|
|
Err(err) => {
|
|
// will give false-positives when a message is received which wasn't enabled
|
|
debug!("Failed to parse V2X message: {err}");
|
|
}
|
|
}
|
|
}
|
|
});
|
|
|
|
// maintenance
|
|
state.prune();
|
|
}
|
|
}
|
|
|
|
#[allow(
|
|
clippy::needless_pass_by_value,
|
|
reason = "adhering to callback interface"
|
|
)]
|
|
fn handle_frame(frame: wifi::sniffer::PromiscuousPkt<'_>) {
|
|
// Ignore frames with errors and non-data frames
|
|
if frame.rx_cntl.rx_state != 0 {
|
|
warn!("Received frame has RX error: {}", frame.rx_cntl.rx_state);
|
|
return;
|
|
}
|
|
|
|
if !radio::PromiscuousPktType::Data.matches(frame.frame_type) {
|
|
debug!("Received frame is not a DATA frame: {}", frame.frame_type);
|
|
return;
|
|
}
|
|
|
|
// Ignore non-broadcast frames
|
|
if frame.len < (4 + 6) {
|
|
warn!(
|
|
"Received frame too small to be a valid broadcast frame: {} bytes",
|
|
frame.len
|
|
);
|
|
return;
|
|
}
|
|
let dest_mac = &frame.data[4..10];
|
|
if dest_mac != [0xff; 6] {
|
|
warn!("Received frame is not broadcast frame: Dest MAC {dest_mac:02x?}");
|
|
return;
|
|
}
|
|
|
|
debug!("Received frame with {} bytes", frame.len);
|
|
|
|
// Send data to main thread
|
|
critical_section::with(|cs| {
|
|
let queue_rc = WIFI_RX_QUEUE.borrow(cs).borrow();
|
|
// unwrap is fine b/c we stored something in it before
|
|
let queue = queue_rc.as_ref().unwrap();
|
|
|
|
if queue.push(frame.data.to_vec()).is_err() {
|
|
error!("V2X RX queue is full");
|
|
}
|
|
});
|
|
}
|
|
|
|
#[cfg(feature = "gnss")]
|
|
#[allow(clippy::large_stack_frames, reason = "GNSS driver needs some space")]
|
|
#[embassy_executor::task]
|
|
async fn gnss_task(
|
|
gps_wakeup: esp_hal::peripherals::GPIO1<'static>,
|
|
gps_reset: esp_hal::peripherals::GPIO25<'static>,
|
|
uart: esp_hal::peripherals::UART0<'static>,
|
|
uart_rx: esp_hal::peripherals::GPIO12<'static>,
|
|
uart_tx: esp_hal::peripherals::GPIO11<'static>,
|
|
) {
|
|
use embassy_gps::gps::GpsFsm;
|
|
|
|
let mut gps = l76k::esp::L76kFsm::new_sep(
|
|
l76k::esp::GpsHw {
|
|
reinit: gps_reset,
|
|
standby: gps_wakeup,
|
|
},
|
|
|| {
|
|
esp_hal::uart::Uart::new(uart, esp_hal::uart::Config::default().with_baudrate(9600))
|
|
.expect("Failed to create UART for GNSS")
|
|
.with_rx(uart_rx)
|
|
.with_tx(uart_tx)
|
|
.into_async()
|
|
},
|
|
)
|
|
.await;
|
|
|
|
let mut fix_drop_message = false;
|
|
loop {
|
|
if let Ok(Some(embassy_gps::types::GpsEvent::Fix(fix))) = gps.step().await {
|
|
// drop every second message as we always get each fix twice
|
|
fix_drop_message = !fix_drop_message;
|
|
if fix_drop_message {
|
|
// send to main thread
|
|
critical_section::with(|cs| {
|
|
let gnss_update_ref = GNSS_UPDATE.borrow(cs);
|
|
gnss_update_ref.replace(Some(fix));
|
|
});
|
|
}
|
|
} else {
|
|
// FSM will recover automatically from errors and ignore other event types
|
|
}
|
|
}
|
|
}
|
|
|
|
/// Builds the SPI interface for the ST7789 display
|
|
///
|
|
/// Returns (spi master, chip-select) tuple or SPI config error
|
|
#[cfg(feature = "screen")]
|
|
fn setup_st7789_spi(
|
|
scl: esp_hal::peripherals::GPIO8<'static>,
|
|
sda: esp_hal::peripherals::GPIO10<'static>,
|
|
cs: esp_hal::peripherals::GPIO24<'static>,
|
|
miso: esp_hal::peripherals::GPIO9<'static>,
|
|
spi: esp_hal::peripherals::SPI2<'static>,
|
|
) -> Result<
|
|
(
|
|
spi::master::Spi<'static, esp_hal::Blocking>,
|
|
gpio::Output<'static>,
|
|
),
|
|
spi::master::ConfigError,
|
|
> {
|
|
let spi_config = spi::master::Config::default()
|
|
.with_frequency(time::Rate::from_mhz(10))
|
|
.with_mode(spi::Mode::_0);
|
|
let spi: spi::master::Spi<'_, esp_hal::Blocking> = spi::master::Spi::new(spi, spi_config)?
|
|
.with_sck(scl)
|
|
.with_mosi(sda)
|
|
.with_miso(miso);
|
|
|
|
let cs_output = gpio::Output::new(cs, gpio::Level::High, gpio::OutputConfig::default());
|
|
|
|
Ok((spi, cs_output))
|
|
}
|
|
|
|
#[cfg(feature = "screen")]
|
|
fn log_screen_error<E>(error: &E)
|
|
where
|
|
E: core::fmt::Debug,
|
|
{
|
|
error!("graphics failed: {error:?}");
|
|
}
|