jtbx/esp32-c_its-companion
Watch
0
0
Fork
You've already forked esp32-c_its-companion
 0
Code Activity
esp32-c_its-companion/src/main.rs
Jannik Beyerstedt 62997dbfcb BLE: Add raw messages 
BLE can only fit 251 bytes of payload in notifications which isn't nearly enough for a raw V2X message.
So only publish a notification which message was received and let the client
poll the content from the characteristic (since reads can get data in fragments)
2026-08-18 15:26:07 +02:00

997 lines
36 KiB
Rust
Raw Blame History

#![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)]
// we can't use UART I/O and GNSS at the same time
#[cfg(all(feature = "uart", feature = "gnss_ubx", feature = "gnss_l67k"))]
compile_error!(
"feature \"uart\" and features \"gnss_ubx\" or \"gnss_l67k\" cannot be enabled at the same time"
);
// autonomous CAM generation need a time and position source (aka _gnss)
#[cfg(all(feature = "cam_tx", not(feature = "_gnss")))]
compile_error!("feature \"cam_tx\" needs a time and position source (either GNSS or UART)");
// we can't have both GNSS drivers enabled at the same time
#[cfg(all(feature = "gnss_ubx", feature = "gnss_l67k"))]
compile_error!("feature \"gnss_ubx\" and feature \"gnss_l67k\" cannot be enabled at the same time");
// at least one V2X feature needs to be enabled
#[cfg(not(any(feature = "spat", feature = "cam", feature = "denm")))]
compile_error!("at least one V2X feature (\"spat\", \"cam\" or \"denm\") needs to be enabled");
use alloc::string::ToString as _;
use alloc::vec::Vec;
use core::cell::RefCell;
#[cfg(feature = "spat")]
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;
#[cfg(feature = "gnss_l67k")]
use embassy_gps::gps::l76k;
use esp_backtrace as _;
use esp_hal::clock::CpuClock;
use esp_hal::timer::timg::TimerGroup;
#[cfg(target_arch = "riscv32")]
use esp_println::println;
use esp32_cits_core::radio;
use geonetworking::Decode as _;
use log::{error, info, warn};
extern crate alloc;
mod applogic;
#[cfg(feature = "ble")]
mod ble;
mod cache;
mod geo_alg;
#[cfg(any(feature = "uart", feature = "ble"))]
mod io;
#[cfg(feature = "screen")]
mod screen;
#[cfg(feature = "gnss_ubx")]
mod ubx;
const WIFI_CHANNEL: radio::Channel = 180;
const WIFI_TXPOWER: i8 = 10; // dBm, 2..20
#[cfg(feature = "spat")]
const SPAT_RATE_LIMIT: u8 = 5; // keep every n-th message
#[cfg(any(feature = "cam_tx", feature = "uart"))]
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 = 7; // in 10cm steps!
#[cfg(feature = "cam_tx")]
const OWN_VEHICLE_LENGTH: u16 = 18; // in 10cm steps!
#[cfg(feature = "_uart")]
const SERIAL_FIFO_SIZE_THLD: u8 = 100; // hardware has max. 128 byte
// 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!();
// Print a backtrace and reset (instead of halting)
#[panic_handler]
fn panic_handler(info: &core::panic::PanicInfo) -> ! {
println!("");
println!("====================== PANIC ======================");
println!("{}", info);
println!("\nBacktrace:");
let backtrace = esp_backtrace::Backtrace::capture();
for frame in backtrace.frames() {
println!("- 0x{:x}", frame.program_counter());
}
println!("================== BACKTRACE END ==================");
// reboot
println!("-> Doing a SW reset after panic...\n");
esp_hal::system::software_reset();
}
#[cfg(feature = "_gnss")]
static GNSS_UPDATE: Mutex<RefCell<Option<applogic::GnssFix>>> = Mutex::new(RefCell::new(None));
static WIFI_RX_QUEUE: Mutex<RefCell<Option<ArrayQueue<Vec<u8>>>>> = Mutex::new(RefCell::new(None));
#[cfg(feature = "_uart")]
static UART_RX_BUF: Mutex<RefCell<Option<Vec<u8>>>> = Mutex::new(RefCell::new(None));
#[cfg(feature = "_uart")]
static SERIAL: Mutex<RefCell<Option<esp_hal::uart::Uart<'static, esp_hal::Blocking>>>> =
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 -o ble-trouble
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);
// COEX needs more RAM - so we've added some more
esp_alloc::heap_allocator!(size: 64 * 1024);
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!");
// Setup ST7789 screen
// SCL: D8/ GPIO8 (SPI_SCK)
// SDA: D10/ GPIO10 (SPI_MOSI)
// RES: D3/ GPIO7
// DC: D4/ GPIO23 (display clear)
// CS: D5/ GPIO24 (chip select)
// BL: D1/ GPIO0 (backlight)
#[cfg(feature = "screen")]
let mut spi_buffer = [0_u8; 512];
#[cfg(feature = "screen")]
let mut screen = {
// 240*280px in portrait orientation
let (display, size) = screen::make_large_display(
peripherals.GPIO8,
peripherals.GPIO10,
peripherals.GPIO7,
peripherals.GPIO23,
peripherals.GPIO24,
peripherals.GPIO0,
peripherals.SPI2,
&mut spi_buffer,
)
.expect("Fatal error building screen");
let mut screen = screen::Handler::new(display, size);
let _ = screen
.update(screen::ScreenState::Initial)
.inspect_err(log_screen_error);
screen
};
// Setup UART I/O channel
#[cfg(feature = "uart")]
setup_uart0(
115_200,
Some(b'\n'),
peripherals.GPIO12,
peripherals.GPIO11,
peripherals.UART0,
serial_handler,
)
.expect("Failed to initialize UART");
// Setup WiFi
// sadly, it doesn't work any more when we move `esp_radio::wifi::new` to `setup_wifi_sniffer`
let (mut wifi_controller, interfaces) = esp_radio::wifi::new(
peripherals.WIFI,
esp_radio::wifi::ControllerConfig::default(),
)
.expect("Failed to initialize Wi-Fi controller");
if let Err(err) = wifi_controller.set_band_mode(esp_radio::wifi::BandMode::_5G) {
error!("Failed to set WiFi to 5GHz only: {err}");
}
if let Err(err) = wifi_controller.set_max_tx_power(WIFI_TXPOWER * 4) {
error!("Failed to set WiFi TX power: {err}");
}
critical_section::with(|cs| {
WIFI_RX_QUEUE.borrow(cs).replace(Some(ArrayQueue::new(2)));
});
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");
let mac = esp_hal::efuse::base_mac_address();
info!("WiFi MAC: {mac}");
// Setup BLE
#[cfg(feature = "ble")]
{
let controller = ble::make_controller(peripherals.BT);
spawner.spawn(ble::run(controller).expect("Failed to spawn BLE task"));
}
// Setup GNSS:
// 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_l67k")]
spawner.spawn(
gnss_l67k_task(
peripherals.GPIO1,
peripherals.GPIO25,
peripherals.UART0,
peripherals.GPIO12,
peripherals.GPIO11,
)
.expect("Failed to spawn GNSS task"),
);
// U-Blox module:
// RX: D7/ GPIO12
// TX: D6/ GPIO11
// RESET: D2/ GPIO25 -> HIGH for normal, LOW for Reset
#[cfg(feature = "gnss_ubx")]
{
setup_uart0(
9600,
Some(b'\n'),
peripherals.GPIO12,
peripherals.GPIO11,
peripherals.UART0,
serial_handler,
)
.expect("Failed to initialize UART for GNSS");
// do some settings (UART receive and parsing runs in main loop)
spawner
.spawn(gnss_ubx_config(peripherals.GPIO25).expect("Failed to spawn GNSS config task"));
}
#[cfg(feature = "_gnss")]
info!("GNSS task started, waiting for GNSS fix...");
#[cfg(any(feature = "cam_tx", feature = "uart"))]
let (mut seq_no, mac_bytes) = {
// unwrap is fine since [`esp_hal::efuse::MacAddress`] is just an `[u8; 6]`
let mac_bytes = mac.as_bytes().try_into().unwrap();
(0, mac_bytes)
};
let mut state = applogic::State::new();
#[cfg(feature = "cam_tx")]
let mut cam = {
let station_id = make_station_id(mac);
info!("V2X Station ID: {station_id}");
applogic::cam_tx::CamState::new(
station_id,
OWN_STATION_TYPE,
OWN_VEHICLE_LENGTH,
OWN_VEHICLE_WIDTH,
)
};
#[cfg(feature = "uart")]
let mut uart_input = io::Parser::default();
#[cfg(feature = "gnss_ubx")]
let mut ubx_parser = ublox::Parser::default();
loop {
embassy_time::Timer::after(embassy_time::Duration::from_millis(1)).await;
#[cfg(feature = "_uart")]
{
let mut new_data = None;
critical_section::with(|cs| {
let mut buf_rc = UART_RX_BUF.borrow(cs).borrow_mut();
// unwrap is fine b/c we stored something in it before
let buf = buf_rc.as_mut().unwrap();
// we move the buffer to new_data to do the processing outside of the critical section lock
if !buf.is_empty() {
new_data = Some(core::mem::take(buf));
}
});
if let Some(data) = new_data {
#[cfg(feature = "uart")]
{
for msg in uart_input.parse(&data) {
match msg.payload {
Some(io::msg::serial_input_msg::Payload::Position(fix)) => {
let pos_state = fix.into();
critical_section::with(|cs| {
let gnss_update_ref = GNSS_UPDATE.borrow(cs);
gnss_update_ref.replace(Some(pos_state));
});
}
Some(io::msg::serial_input_msg::Payload::TxMsg(tx_request)) => {
use esp32_cits_core::tx::GnItsPayload as _;
info!(
"UART: Got new TX message: Type {} with {} bytes",
tx_request.message_type,
tx_request.payload.len()
);
let own_position = state.pos();
let time = state.time().and_utc();
match tx_request.encode_packet(
mac_bytes,
own_position,
time,
GN_IS_MOBILE,
&mut seq_no,
) {
Ok(data) => {
if let Err(err) = radio::send_80211_bcast_frame(
&mut wlan_iface,
&mac_bytes.clone(),
&data,
) {
warn!("Failed to send 802.11 frame: {err}");
}
}
Err(err) => warn!("Failed to create GN message: {err}"),
}
}
None => {}
}
}
}
#[cfg(feature = "gnss_ubx")]
{
let mut it = ubx_parser.consume_ubx(&data);
loop {
match it.next() {
Some(Ok(ublox::UbxPacket::Proto27(packet_ref))) => match &packet_ref {
ublox::proto27::PacketRef::MonVer(packet) => {
println!("{}", ubx::pretty_print_mon_ver(packet));
}
ublox::proto27::PacketRef::MonComms(packet) => {
println!("{}", ubx::pretty_print_mon_comms(packet));
}
ublox::proto27::PacketRef::MonRf(packet) => {
println!("{}", ubx::pretty_print_mon_rf(packet));
}
ublox::proto27::PacketRef::NavSat(packet) => {
println!("{}", ubx::pretty_print_nav_sat(packet));
}
ublox::proto27::PacketRef::NavPvt(pvt) => {
// error can be ignored b/c it's just when no GNSS fix yet
if let Ok(pos_state) = applogic::GnssFix::try_from(pvt) {
critical_section::with(|cs| {
let gnss_update_ref = GNSS_UPDATE.borrow(cs);
gnss_update_ref.replace(Some(pos_state));
});
}
// update GNSS HB on screen
let _ = screen.update_gnss(pvt);
}
ublox::proto27::PacketRef::AckNak(nak) => {
error!("UBX: {nak:?}");
}
ublox::proto27::PacketRef::AckAck(ack) => {
info!("UBX: {ack:?}");
}
_ => {
println!("UBX: {packet_ref:?}");
}
},
Some(Err(_)) => {
// Received a malformed packet
}
None => {
// The internal buffer is now empty
break;
}
}
}
}
}
}
#[cfg(feature = "ble")]
{
critical_section::with(|cs| {
let mut data_rc = ble::RX_DATA.borrow(cs).borrow_mut();
if let Some(pos) = data_rc.take() {
let gnss_update_ref = GNSS_UPDATE.borrow(cs);
gnss_update_ref.replace(Some(pos.into()));
}
});
}
#[cfg(feature = "_gnss")]
{
let mut new_position = false;
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);
new_position = true;
#[cfg(feature = "cam_tx")]
{
use esp32_cits_core::tx::GnItsPayload as _;
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_bytes.clone(),
&data,
) {
warn!("Failed to send 802.11 frame: {err}");
}
}
Err(err) => warn!("Failed to create CAM: {err}"),
}
}
}
});
if new_position {
info!("{}", state.print_fix());
// run GLOSA algorithm
#[cfg(feature = "spat")]
{
let glosa_data = state.run_glosa();
if let applogic::GlosaOutput::Locked(data) = &glosa_data {
for sig in &data.signal_groups {
let duration_str = sig
.timing
.as_ref()
.map(|v| alloc::format!("for {} s", v.min_end_sec))
.unwrap_or_default();
println!("GLOSA: {} {} {duration_str}", sig.maneuver, sig.phase);
}
}
#[cfg(feature = "screen")]
let _ = screen
.update(glosa_data.into())
.inspect_err(log_screen_error);
}
}
}
let mut new_data = None;
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() {
new_data = Some(data);
}
});
if let Some(data) = new_data {
match c_its_parser::de::decode(&data, c_its_parser::Headers::None) {
Ok(msg) => {
use c_its_parser::ItsMessage;
// use c_its_parser::standards::extensions::ItsMessageId;
// info!("Got new {:?} message", ItsMessageId::from(&msg));
match msg {
#[cfg(feature = "spat")]
ItsMessage::Mapem {
geonetworking: _,
transport: _,
ref etsi,
} => {
state.handle_mapem(etsi);
// Send data to BLE thread
#[cfg(feature = "ble")]
match io::msg::RawMsgRx::try_from(&msg) {
Ok(data) => {
ble::update(ble::BleSendable::MapemRaw(data));
}
Err(err) => warn!("Failed to create RawMsgRx: {err}"),
}
}
#[cfg(feature = "spat")]
ItsMessage::Spatem {
geonetworking: _,
transport: _,
ref etsi,
} => {
if state.initialized() {
state.handle_spatem(etsi);
}
// Send data to BLE thread
#[cfg(feature = "ble")]
match io::msg::RawMsgRx::try_from(&msg) {
Ok(data) => {
ble::update(ble::BleSendable::SpatemRaw(data));
}
Err(err) => warn!("Failed to create RawMsgRx: {err}"),
}
}
#[cfg(any(feature = "cam", feature = "cam_tx"))]
ItsMessage::Cam {
geonetworking: _,
transport: _,
ref etsi,
} => {
#[cfg(feature = "cam")]
applogic::cam::handle_cam(etsi);
// Send data to BLE thread
#[cfg(all(feature = "ble", feature = "cam"))]
{
ble::update(ble::BleSendable::Cam(etsi.into()));
// raw data only when it's a full CAM
if etsi.cam.cam_parameters.low_frequency_container.is_some() {
match io::msg::RawMsgRx::try_from(&msg) {
Ok(data) => {
ble::update(ble::BleSendable::CamRaw(data));
}
Err(err) => warn!("Failed to create RawMsgRx: {err}"),
}
}
}
}
#[cfg(feature = "denm")]
ItsMessage::DenmV2 {
geonetworking: _,
transport: _,
ref etsi,
} => {
applogic::denm::handle_denm(etsi);
// Send data to BLE thread
#[cfg(feature = "ble")]
{
ble::update(ble::BleSendable::Denm(etsi.into()));
match io::msg::RawMsgRx::try_from(&msg) {
Ok(data) => {
ble::update(ble::BleSendable::DenmRaw(data));
}
Err(err) => warn!("Failed to create RawMsgRx: {err}"),
}
}
}
#[cfg(feature = "denm")]
ItsMessage::DenmV1 {
geonetworking: _,
transport: _,
etsi: _,
} => {
// irrelevant since all received DENMs should be parsed as v2
}
}
}
Err(err) => {
// we already filtered out all unsupported message types in the wifi rx callback
// so these are actual errors
warn!("Failed to parse V2X message: {err}");
}
}
}
// maintenance
state.prune();
}
}
#[allow(
clippy::needless_pass_by_value,
reason = "adhering to callback interface"
)]
#[allow(clippy::too_many_lines)]
fn handle_frame(frame: esp_radio::wifi::sniffer::PromiscuousPkt<'_>) {
#[cfg(feature = "spat")]
const PRUNE_INTERVAL: chrono::Duration = chrono::Duration::seconds(60);
#[cfg(feature = "spat")]
static mut RATELIMIT_CACHE_LAST_PRUNE: UnsafeCell<chrono::NaiveDateTime> =
UnsafeCell::new(chrono::NaiveDateTime::MIN);
#[cfg(feature = "spat")]
static mut SPAT_RATELIMIT_CACHE: UnsafeCell<cache::Cache<u32, RatelimitData>> = UnsafeCell::new(
cache::Cache::<u32, RatelimitData>::new(chrono::Duration::seconds(20)),
);
#[cfg(feature = "spat")]
let now = {
let time = esp_hal::time::Instant::now().duration_since_epoch();
// unwrap is fine since value range is u64 microseconds
chrono::DateTime::from_timestamp(time.as_secs().cast_signed(), 0)
.unwrap()
.naive_utc()
};
match esp32_cits_core::rx::is_broadcast_frame(&frame) {
Err(err) => {
warn!("{err}");
return;
}
Ok(false) => {
// drop frame
return;
}
Ok(true) => {
// continue
}
}
// info!("Received frame with {} bytes", frame.len);
// parse GN headers
match c_its_parser::pcap::remove_wlan_headers(frame.data)
.map_err(|err| alloc::format!("Failed to parse WLAN headers: {err}"))
.and_then(|data| {
geonetworking::Packet::decode(data)
.map_err(|err| alloc::format!("Failed to parse GN headers: {err:?}"))
}) {
Err(err) => {
warn!("{err}");
}
Ok(packet) => {
// drop hopped (for now)
if packet.decoded.is_hopped() {
return;
}
// "parse" ITS PDU Header
if let Ok((payload, message_id, station_id)) = get_its_header(&packet) {
// drop all unsupported message IDs and rate-limit SPATEMs
if match message_id {
#[cfg(feature = "denm")]
c_its_parser::standards::extensions::ItsMessageId::Denm => false,
#[cfg(feature = "cam")]
c_its_parser::standards::extensions::ItsMessageId::Cam => false,
#[cfg(feature = "spat")]
c_its_parser::standards::extensions::ItsMessageId::Spatem => {
// reduce SPAT rate per station ID
let mut drop_msg = false;
unsafe {
#[allow(
static_mut_refs,
reason = "this function will only run consecutive"
)]
let spat_cache = SPAT_RATELIMIT_CACHE.get();
(*spat_cache).update_or_init(now, station_id, |i| {
i.msg_count = i.msg_count.wrapping_add(1);
if (i.msg_count % SPAT_RATE_LIMIT) > 0 {
drop_msg = true;
}
});
}
drop_msg
}
#[cfg(feature = "spat")]
c_its_parser::standards::extensions::ItsMessageId::Mapem => false,
_ => true,
} {
return;
}
// 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(payload.to_vec()).is_err() {
error!("V2X RX queue is full");
}
});
// Send data to UART
#[cfg(feature = "uart")]
{
let message = io::msg::RawMsgRx {
msg_type: message_id.as_u8().into(),
payload: frame.data.to_vec(),
}
.serialize_uart_proto();
send_uart0(&message);
}
}
}
}
#[cfg(feature = "spat")]
unsafe {
#[allow(static_mut_refs, reason = "this function will only run consecutive")]
let last_prune_time = RATELIMIT_CACHE_LAST_PRUNE.get();
if *last_prune_time + PRUNE_INTERVAL < now {
#[allow(static_mut_refs, reason = "this function will only run consecutive")]
let spat_cache = SPAT_RATELIMIT_CACHE.get();
(*spat_cache).prune(now);
*last_prune_time = now;
}
}
}
#[cfg(feature = "spat")]
#[derive(Debug, Default, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)]
struct RatelimitData {
station_id: u32,
msg_count: u8,
}
#[cfg(feature = "spat")]
impl cache::Cachable<u32> for RatelimitData {
fn key(&self) -> u32 {
self.station_id
}
}
#[cfg(feature = "spat")]
impl cache::Initable<Self, u32> for RatelimitData {
fn init(key: u32) -> Self {
Self {
station_id: key,
..Default::default()
}
}
}
fn get_its_header<'p>(
packet: &geonetworking::Decoded<geonetworking::Packet<'p>>,
) -> Result<
(
&'p [u8],
c_its_parser::standards::extensions::ItsMessageId,
u32,
),
alloc::string::String,
> {
let payload = packet.decoded.btp_payload()?;
if payload.len() < 6 {
return Err("ITS payload too small for ItsPduHeader".to_string());
}
if let Ok(message_id) = payload[1].try_into() {
let station_id_buf: [u8; 4] = payload[2..6].try_into().unwrap();
let station_id = u32::from_be_bytes(station_id_buf);
Ok((payload, message_id, station_id))
} else {
Err("Unknown message ID in ItsPduHeader".to_string())
}
}
#[cfg(feature = "gnss_l67k")]
#[allow(clippy::large_stack_frames, reason = "GNSS driver needs some space")]
#[embassy_executor::task]
async fn gnss_l67k_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
let pos_state = applogic::GnssFix {
time: fix.get_timestamp(),
latitude_deg: fix.latitude_deg,
longitude_deg: fix.longitude_deg,
heading_deg: fix.true_course_deg,
speed_mps: fix.speed_over_ground,
};
critical_section::with(|cs| {
let gnss_update_ref = GNSS_UPDATE.borrow(cs);
gnss_update_ref.replace(Some(pos_state));
});
}
} else {
// FSM will recover automatically from errors and ignore other event types
}
}
}
#[cfg(feature = "gnss_ubx")]
#[embassy_executor::task]
async fn gnss_ubx_config(reset_pin: esp_hal::peripherals::GPIO25<'static>) {
// reset the module
let mut gnss_reset = esp_hal::gpio::Output::new(
reset_pin,
esp_hal::gpio::Level::Low,
esp_hal::gpio::OutputConfig::default(),
);
embassy_time::Timer::after(embassy_time::Duration::from_millis(2)).await;
gnss_reset.set_high();
embassy_time::Timer::after(embassy_time::Duration::from_secs(1)).await;
info!("UBX: Enabling ubx protocol with NAV-PVT and NAV-SAT");
let buffer = ublox::packets::cfg_val::CfgValSetBuilder {
version: 1,
layers: ublox::packets::cfg_val::CfgLayerSet::RAM,
reserved1: 0,
cfg_data: &[
ublox::cfg_val::CfgVal::Uart1OutProtNmea(false),
ublox::cfg_val::CfgVal::Uart1OutProtUbx(true),
// CFG-RATE-MEAS
ublox::cfg_val::CfgVal::RateMeas(500),
// CFG-MSGOUT-UBX_NAV_PVT_UART1
ublox::cfg_val::CfgVal::MsgOutUbxNavPvtUart1(1),
// CFG-MSGOUT-UBX_NAV_SAT_UART1
// ublox::cfg_val::CfgVal::MsgOutUbxNavSatUart1(10),
],
}
.into_packet_vec();
send_uart0(&buffer);
}
#[cfg(feature = "screen")]
fn log_screen_error<E>(error: &E)
where
E: core::fmt::Debug,
{
error!("graphics failed: {error:?}");
}
#[cfg(feature = "_uart")]
/// Configures UART0 with an interrupt handler and saves it to `SERIAL`
fn setup_uart0(
baud: u32,
cmd_char: Option<u8>,
gpio12: esp_hal::peripherals::GPIO12<'static>,
gpio11: esp_hal::peripherals::GPIO11<'static>,
uart0: esp_hal::peripherals::UART0<'static>,
handler: esp_hal::interrupt::InterruptHandler,
) -> Result<(), alloc::string::String> {
critical_section::with(|cs| {
UART_RX_BUF
.borrow(cs)
.replace(Some(alloc::vec::Vec::with_capacity(255)));
});
let mut uart = esp_hal::uart::Uart::new(
uart0,
esp_hal::uart::Config::default()
.with_baudrate(baud)
.with_rx(
esp_hal::uart::RxConfig::default()
.with_fifo_full_threshold(u16::from(SERIAL_FIFO_SIZE_THLD))
.with_timeout(50),
),
)
.map_err(|err| alloc::format!("UART0 config error: {err}"))?
.with_rx(gpio12)
.with_tx(gpio11);
if let Some(char) = cmd_char {
uart.set_at_cmd(
esp_hal::uart::AtCmdConfig::default()
.with_cmd_char(char)
.with_char_num(1)
.with_pre_idle_count(0) // command start can be right after other data
.with_post_idle_count(0) // we don't expect a gap between the command start and subsequent text
.with_gap_timeout(5), // needs to be >0 to work
);
}
uart.set_interrupt_handler(handler);
uart.listen(
esp_hal::uart::UartInterrupt::AtCmd
| esp_hal::uart::UartInterrupt::RxTimeout
| esp_hal::uart::UartInterrupt::RxFifoFull, // just to be save
);
critical_section::with(|cs| SERIAL.borrow_ref_mut(cs).replace(uart));
Ok(())
}
#[cfg(feature = "_uart")]
#[esp_hal::handler]
fn serial_handler() {
critical_section::with(|cs| {
let mut serial = SERIAL.borrow_ref_mut(cs);
let mut uart_buf = UART_RX_BUF.borrow_ref_mut(cs);
if let (Some(serial), Some(uart_buf)) = (serial.as_mut(), uart_buf.as_mut()) {
let mut buf = [0u8; SERIAL_FIFO_SIZE_THLD as usize];
match serial.read_buffered(&mut buf) {
Ok(0) => {}
Ok(len) => {
uart_buf.extend_from_slice(&buf[..len]);
}
Err(err) => error!("Failed to read from UART: {err}"),
}
serial.clear_interrupts(
esp_hal::uart::UartInterrupt::RxFifoFull
| esp_hal::uart::UartInterrupt::RxTimeout
| esp_hal::uart::UartInterrupt::AtCmd,
);
}
});
}
#[cfg(feature = "_uart")]
fn send_uart0(data: &[u8]) {
critical_section::with(|cs| {
let mut serial = SERIAL.borrow_ref_mut(cs);
if let Some(serial) = serial.as_mut()
&& let Err(err) = serial.write(data)
{
error!("Failed to write to UART: {err:?}");
}
});
}
#[cfg(all(feature = "esp", feature = "cam_tx"))]
/// 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.
///
/// # Panics
/// Won't panic in practive b/c of pre-conditions
#[must_use]
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)
}
View git blame Copy permalink