use cu29::prelude::*;
use crate::board::{BdshotBoard, encode_frame};
#[cfg(feature = "messages-only")]
use crate::messages::DShotTelemetry;
use crate::messages::{EscCommand, EscTelemetry};
pub trait BdshotBoardProvider {
type Board: BdshotBoard;
fn create_board() -> CuResult<Self::Board>;
}
tx_channels! {
esc0_tx => EscCommand,
esc1_tx => EscCommand,
esc2_tx => EscCommand,
esc3_tx => EscCommand,
}
rx_channels! {
esc0_rx => EscTelemetry,
esc1_rx => EscTelemetry,
esc2_rx => EscTelemetry,
esc3_rx => EscTelemetry,
}
const MAX_ESC_CHANNELS: usize = 4;
#[cfg(feature = "messages-only")]
fn telemetry_status(idx: usize, telemetry: Option<DShotTelemetry>) -> CuCompactString {
match telemetry {
Some(DShotTelemetry::Erpm(v)) => format!("esc{} {}rpm", idx, v).into(),
Some(DShotTelemetry::Temp(v)) => format!("esc{} t{}C", idx, v).into(),
Some(DShotTelemetry::Voltage(v)) => format!("esc{} {}V", idx, v).into(),
Some(DShotTelemetry::Amps(v)) => format!("esc{} {}A", idx, v).into(),
Some(DShotTelemetry::EncodingError) => format!("esc{} enc_err", idx).into(),
Some(DShotTelemetry::Debug1(v)) => format!("esc{} d1 {}", idx, v).into(),
Some(DShotTelemetry::Debug2(v)) => format!("esc{} d2 {}", idx, v).into(),
Some(DShotTelemetry::Debug3(v)) => format!("esc{} d3 {}", idx, v).into(),
Some(DShotTelemetry::Event(v)) => format!("esc{} ev {}", idx, v).into(),
None => format!("esc{} no_tlm", idx).into(),
}
}
#[derive(Reflect)]
#[reflect(from_reflect = false, no_field_bounds, type_path = false)]
pub struct CuBdshotBridge<P: BdshotBoardProvider + 'static>
where
P::Board: Send + 'static,
{
#[reflect(ignore)]
board: spin::Mutex<P::Board>,
#[reflect(ignore)]
telemetry_cache: [Option<EscTelemetry>; MAX_ESC_CHANNELS],
active_channels: [bool; MAX_ESC_CHANNELS],
send_interval: Option<CuDuration>,
last_send: [Option<CuTime>; MAX_ESC_CHANNELS],
}
impl<P: BdshotBoardProvider + 'static> cu29::reflect::TypePath for CuBdshotBridge<P>
where
P::Board: Send + 'static,
{
fn type_path() -> &'static str {
"cu_bdshot::CuBdshotBridge"
}
fn short_type_path() -> &'static str {
"CuBdshotBridge"
}
fn type_ident() -> Option<&'static str> {
Some("CuBdshotBridge")
}
fn crate_name() -> Option<&'static str> {
Some("cu_bdshot")
}
fn module_path() -> Option<&'static str> {
Some("cu_bdshot")
}
}
impl<P: BdshotBoardProvider + 'static> Freezable for CuBdshotBridge<P> where P::Board: Send + 'static
{}
impl<P: BdshotBoardProvider + 'static> CuBridge for CuBdshotBridge<P>
where
P::Board: Send + 'static,
{
type Resources<'r> = ();
type Tx = TxChannels;
type Rx = RxChannels;
fn new(
config: Option<&ComponentConfig>,
tx: &[BridgeChannelConfig<TxId>],
_rx: &[BridgeChannelConfig<RxId>],
_resources: Self::Resources<'_>,
) -> CuResult<Self>
where
Self: Sized,
{
let board = P::create_board()?;
if P::Board::CHANNEL_COUNT > MAX_ESC_CHANNELS {
return Err(CuError::from(
"BDShot board exposes more channels than supported",
));
}
if tx.len() > P::Board::CHANNEL_COUNT {
return Err(CuError::from("Too many Tx channels for board"));
}
let mut active = [false; MAX_ESC_CHANNELS];
for ch in tx {
active[ch.channel.id.as_index()] = true;
}
let send_interval = if let Some(cfg) = config {
if let Some(rate_hz) = cfg.get::<f64>("rate_hz")? {
if rate_hz <= 0.0 {
return Err(CuError::from("BDShot rate_hz must be > 0"));
}
Some(CuDuration::from((1e9 / rate_hz) as u64))
} else {
None
}
} else {
None
};
Ok(Self {
board: spin::Mutex::new(board),
telemetry_cache: Default::default(),
active_channels: active,
send_interval,
last_send: [None; MAX_ESC_CHANNELS],
})
}
fn start(&mut self, _ctx: &CuContext) -> CuResult<()> {
let idle_frame = encode_frame(EscCommand::disarm());
let mut ready = true;
for i in 0..2048 {
for idx in 0..P::Board::CHANNEL_COUNT {
if !self.active_channels[idx] {
continue;
}
debug!("Sending disarm frames {}", idx);
let sample = {
let mut board = self.board.lock();
board.delay(200);
board.exchange(idx, idle_frame)
};
if let Some(sample) = sample {
self.telemetry_cache[idx] = Some(EscTelemetry {
sample: Some(sample),
});
} else {
ready = false;
}
}
if ready {
break;
}
debug!("Waiting for ESCs startup {}...", i);
}
if !ready {
error!("Timeout waiting for ESC to start up");
return Err(CuError::from("Timeout waiting for ESC to start up"));
}
let telemetry_cmd = EscCommand {
throttle: 13,
request_telemetry: true,
};
let telemetry_frame = encode_frame(telemetry_cmd);
for _ in 0..6 {
for idx in 0..P::Board::CHANNEL_COUNT {
if !self.active_channels[idx] {
continue;
}
let mut board = self.board.lock();
board.delay(200);
let _ = board.exchange(idx, telemetry_frame);
}
}
Ok(())
}
fn send<'a, Payload>(
&mut self,
ctx: &CuContext,
channel: &'static BridgeChannel<<Self::Tx as BridgeChannelSet>::Id, Payload>,
msg: &CuMsg<Payload>,
) -> CuResult<()>
where
Payload: CuMsgPayload + 'a,
{
let idx = channel.id().as_index();
if idx >= P::Board::CHANNEL_COUNT {
return Err(CuError::from("Channel not supported by board"));
}
if !self.active_channels[idx] {
return Ok(());
}
if let Some(interval) = self.send_interval {
let now = ctx.recent();
if let Some(last) = self.last_send[idx]
&& now - last < interval
{
return Ok(());
}
self.last_send[idx] = Some(now);
}
let payload: &CuMsg<EscCommand> = msg.downcast_ref()?;
let command = payload.payload().cloned().unwrap_or_default();
let frame = encode_frame(command);
let telemetry = {
let mut board = self.board.lock();
board.exchange(idx, frame)
};
if let Some(telemetry) = telemetry {
self.telemetry_cache[idx] = Some(EscTelemetry {
sample: Some(telemetry),
});
}
Ok(())
}
fn receive<'a, Payload>(
&mut self,
ctx: &CuContext,
channel: &'static BridgeChannel<<Self::Rx as BridgeChannelSet>::Id, Payload>,
msg: &mut CuMsg<Payload>,
) -> CuResult<()>
where
Payload: CuMsgPayload + 'a,
{
msg.tov = Tov::Time(ctx.now());
let idx = channel.id().as_index();
if idx >= P::Board::CHANNEL_COUNT {
msg.clear_payload();
return Ok(());
}
if !self.active_channels[idx] {
msg.clear_payload();
return Ok(());
}
let telemetry_msg: &mut CuMsg<EscTelemetry> = msg.downcast_mut()?;
if let Some(sample) = self.telemetry_cache[idx].take() {
#[cfg(feature = "messages-only")]
telemetry_msg
.metadata
.set_status(telemetry_status(idx, sample.sample));
telemetry_msg.set_payload(sample);
} else {
#[cfg(feature = "messages-only")]
telemetry_msg
.metadata
.set_status(telemetry_status(idx, None));
telemetry_msg.clear_payload();
}
Ok(())
}
}