jtbx/esp32-cits-core
0
0
Fork 0
Code Activity

initial commit

Browse source
This commit is contained in:
Jannik Beyerstedt 2026-07-31 15:51:10 +02:00
commit 72762ca558
11 changed files with 796 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

111
src/tx/gn.rs Normal file
View file

@ -0,0 +1,111 @@
//! Geonetworking utilities
use c_its_parser::gn as geonetworking;
#[allow(unused)]
/// Creates a Single-Hop Broadcast (SHB) Extended Header from parts
///
/// # Errors
/// Returns a human-readable error description if values are out of range
pub fn make_shb_eh(
address: [u8; 6],
station_type: geonetworking::en302636_4_1::StationType,
own_position: &crate::PosVel,
time: chrono::DateTime<chrono::Utc>,
) -> Result<geonetworking::en302636_4_1::ExtendedHeader, alloc::string::String> {
let source_position_vector = make_lpv(address, station_type, own_position, time)?;
Ok(geonetworking::en302636_4_1::ExtendedHeader::SHB(
geonetworking::en302636_4_1::SingleHopBroadcast {
source_position_vector,
media_dependent_data: [0; 4],
},
))
}
#[allow(unused)]
/// Creates a Topo-Scoped Broadcast (TSB) Extended Header from parts
///
/// # Errors
/// Returns a human-readable error description if values are out of range
pub fn make_tsb_eh(
address: [u8; 6],
station_type: geonetworking::en302636_4_1::StationType,
own_position: &crate::PosVel,
time: chrono::DateTime<chrono::Utc>,
sequence_number: u16,
) -> Result<geonetworking::en302636_4_1::ExtendedHeader, alloc::string::String> {
let source_position_vector = make_lpv(address, station_type, own_position, time)?;
Ok(geonetworking::en302636_4_1::ExtendedHeader::TSB(
geonetworking::en302636_4_1::TopologicallyScopedBroadcast::new(
sequence_number,
source_position_vector,
),
))
}
// #[allow(unused)]
// /// Creates a Geo-Broadcast Extended Header
// ///
// /// # Panics
// /// Can't panic in practice b/c angle is zero
// #[must_use]
// pub fn make_gbc_eh(
// address: [u8; 6],
// station_type: geonetworking::en302636_4_1::StationType,
// own_position: &crate::PosVel,
// time: chrono::DateTime<chrono::Utc>,
// target_pos: geo_types::Point,
// radius_m: u16,
// sequence_number: u16,
// ) -> geonetworking::en302636_4_1::ExtendedHeader {
// let source_position_vector = make_lpv(address, station_type, own_position, time);
// #[allow(clippy::cast_possible_truncation)]
// let latitude_deg = target_pos.x() as f32;
// #[allow(clippy::cast_possible_truncation)]
// let longitude_deg = target_pos.y() as f32;
// geonetworking::en302636_4_1::ExtendedHeader::GBC(
// geonetworking::en302636_4_1::GeoBroadcast::try_from_values(
// sequence_number,
// source_position_vector,
// latitude_deg,
// longitude_deg,
// radius_m,
// 0,
// 0,
// )
// .expect("Failed to create circular GBC"),
// )
// }
/// Creates a Long Position Vector from parts
///
/// # Errors
/// Returns a human-readable error description if values are out of range
pub fn make_lpv(
address: [u8; 6],
station_type: geonetworking::en302636_4_1::StationType,
own_position: &crate::PosVel,
time: chrono::DateTime<chrono::Utc>,
) -> Result<geonetworking::en302636_4_1::LongPositionVector, alloc::string::String> {
let timestamp_its = c_its_parser::time_utils::TimestampIts::from(time);
#[allow(clippy::cast_possible_truncation)]
let latitude_deg = own_position.position.y() as f32;
#[allow(clippy::cast_possible_truncation)]
let longitude_deg = own_position.position.x() as f32;
let speed_mps = own_position.speed_mps.unwrap_or_default();
let heading_deg = own_position.heading_deg.unwrap_or_default();
geonetworking::en302636_4_1::LongPositionVector::try_from_values(
geonetworking::en302636_4_1::Address::new(false, station_type, address),
timestamp_its.0,
latitude_deg,
longitude_deg,
true,
speed_mps,
heading_deg,
)
.map_err(|err| alloc::format!("LPV invalid input: {err}"))
}

164
src/tx/mod.rs Normal file
View file

@ -0,0 +1,164 @@
//! V2X Transmission Utilities
use alloc::vec::Vec;
use c_its_parser::gn as geonetworking;
pub mod gn;
/// Interface for ITS Payloads inside a Geonetworking header
pub trait GnItsPayload {
// common GN parameters
const LIFETIME_MILLIS: u32 = 60_000; // itsGnDefaultPacketLifetime is 60 seconds according to ETSI EN 302 636-4-1 V1.4.1
// ---------------------------------------------
// methods to build a GN packet with ITS payload
// ---------------------------------------------
/// Builds the [`geonetworking::en302636_4_1::ExtendedHeader`] and additional information for the ITS message
///
/// Output:
/// - Extended header
/// - Area type (when using GAC or GBC)
/// - hop limit
///
/// Note: Make sure to increase the sequence number on multi-hop packets.
///
/// # Errors
/// Returns a human-readable error description if values are out of range
fn make_eh(
&self,
address: [u8; 6],
own_position: &crate::PosVel,
time: chrono::DateTime<chrono::Utc>,
seq_no: &mut u16,
) -> Result<
(
geonetworking::en302636_4_1::ExtendedHeader,
Option<geonetworking::en302636_4_1::AreaType>,
u8,
),
alloc::string::String,
>;
/// Creates the ITS payload (without BTP header)
///
/// # Errors
/// Returns a human-readable error description if values are out of range
fn make_payload(
&self,
) -> Result<(Vec<u8>, c_its_parser::standards::extensions::ItsMessageId), alloc::string::String>;
// ---------------------------------------------
// common methods (with implementation)
// ---------------------------------------------
/// Encodes the GN packet
///
/// # Errors
/// Returns a human-readable error description if values are out of range
fn encode_packet(
&self,
address: [u8; 6],
own_position: &crate::PosVel,
time: chrono::DateTime<chrono::Utc>,
is_mobile: bool,
seq_no: &mut u16,
) -> Result<Vec<u8>, alloc::string::String> {
use geonetworking::Encode as _;
// Build payload (including BTP header)
let (mut its_payload, msg_type) = self.make_payload()?;
let mut payload = make_btp_b(msg_type).encode()?;
payload.append(&mut its_payload);
#[allow(clippy::cast_possible_truncation)]
let payload_length = (payload.len()) as u16;
// get extended header (depending on message type and sometimes content as well)
let (eh, area_type, maximum_hop_limit) =
self.make_eh(address, own_position, time, seq_no)?;
// Build GN headers
let traffic_class = geonetworking::en302636_4_1::TrafficClass::try_new(
false,
false,
make_traffic_class(msg_type),
)
.map_err(|err| alloc::format!("Failed to create TrafficClass: {err:?}"))?;
let common = geonetworking::en302636_4_1::CommonHeader::from_values(
geonetworking::en302636_4_1::NextAfterCommon::BTPB,
geonetworking::en302636_4_1::HeaderType::new_from(&eh, area_type)?,
traffic_class,
is_mobile,
payload_length,
maximum_hop_limit,
);
let remaining_hop_limit = maximum_hop_limit;
let basic = geonetworking::en302636_4_1::BasicHeader::try_new(
1,
geonetworking::en302636_4_1::NextAfterBasic::CommonHeader,
geonetworking::en302636_4_1::Lifetime::from_milliseconds(Self::LIFETIME_MILLIS),
remaining_hop_limit,
)
.map_err(|err| alloc::format!("Failed to create BasicHeader: {err}"))?;
// Assemble and encode packet
let packet = geonetworking::Packet::Unsecured {
basic,
common,
extended: Some(eh),
payload: &payload,
};
packet
.encode_to_vec()
.map_err(|err| alloc::format!("Failed to encode GN packet: {err:?}"))
}
}
/// Creates a BTP-B header based on the ITS message ID (message type)
///
/// Port numbers and destination port info according to ETSI TS 103 248
fn make_btp_b(
msg_type: c_its_parser::standards::extensions::ItsMessageId,
) -> c_its_parser::transport::TransportHeader {
let (destination_port, destination_port_info) = match msg_type {
c_its_parser::standards::extensions::ItsMessageId::Denm => (2002, 0),
c_its_parser::standards::extensions::ItsMessageId::Cam => (2001, 0),
c_its_parser::standards::extensions::ItsMessageId::Mapem => (2003, 0),
c_its_parser::standards::extensions::ItsMessageId::Spatem => (2004, 0),
c_its_parser::standards::extensions::ItsMessageId::Saem => (2005, 0),
c_its_parser::standards::extensions::ItsMessageId::Ivim => (2006, 0),
c_its_parser::standards::extensions::ItsMessageId::Srem => (2007, 0),
c_its_parser::standards::extensions::ItsMessageId::Ssem => (2008, 0),
c_its_parser::standards::extensions::ItsMessageId::Cpm => (2009, 0),
c_its_parser::standards::extensions::ItsMessageId::Poi => (2010, 0),
c_its_parser::standards::extensions::ItsMessageId::EvRsr => (2012, 0),
c_its_parser::standards::extensions::ItsMessageId::Rtcmem => (2013, 0),
_ => (0, 0), // no information found for the subsequent types
};
c_its_parser::transport::TransportHeader::BtpB(c_its_parser::transport::BasicTransportBHeader {
destination_port,
destination_port_info,
})
}
/// Creates a traffic class ID based on the ITS message ID (message type)
fn make_traffic_class(msg_type: c_its_parser::standards::extensions::ItsMessageId) -> u8 {
#[allow(clippy::match_same_arms)]
match msg_type {
c_its_parser::standards::extensions::ItsMessageId::Denm => 1,
c_its_parser::standards::extensions::ItsMessageId::Cam => 2,
c_its_parser::standards::extensions::ItsMessageId::Mapem => 3, // from C-Roads RS_RSP_063(1)
c_its_parser::standards::extensions::ItsMessageId::Spatem => 3, // from C-Roads RS_RSP_063(1)
c_its_parser::standards::extensions::ItsMessageId::Ivim => 3, // from C-Roads RS_RSP_063(1)
c_its_parser::standards::extensions::ItsMessageId::Srem => 2, // from C-Roads RS_RSP_063(1)
c_its_parser::standards::extensions::ItsMessageId::Ssem => 2, // from C-Roads RS_RSP_063(1)
c_its_parser::standards::extensions::ItsMessageId::Rtcmem => 3, // CSP_MaxPrio=252 -> 3
_ => 3, // fall-back to lowest priority
}
}