cu_bdshot/
bridge.rs

1use cu29::prelude::*;
2
3use crate::board::{encode_frame, BdshotBoard};
4use crate::messages::{EscCommand, EscTelemetry};
5
6pub trait BdshotBoardProvider {
7    type Board: BdshotBoard;
8
9    fn create_board() -> CuResult<Self::Board>;
10}
11
12// channel mappings generation
13tx_channels! {
14    esc0_tx => EscCommand,
15    esc1_tx => EscCommand,
16    esc2_tx => EscCommand,
17    esc3_tx => EscCommand,
18}
19
20rx_channels! {
21    esc0_rx => EscTelemetry,
22    esc1_rx => EscTelemetry,
23    esc2_rx => EscTelemetry,
24    esc3_rx => EscTelemetry,
25}
26
27const MAX_ESC_CHANNELS: usize = 4;
28
29pub struct CuBdshotBridge<P: BdshotBoardProvider> {
30    board: P::Board,
31    telemetry_cache: [Option<EscTelemetry>; MAX_ESC_CHANNELS],
32    active_channels: [bool; MAX_ESC_CHANNELS],
33}
34
35impl<P: BdshotBoardProvider> Freezable for CuBdshotBridge<P> {}
36
37impl<P: BdshotBoardProvider> CuBridge for CuBdshotBridge<P> {
38    type Tx = TxChannels;
39    type Rx = RxChannels;
40
41    fn new(
42        _config: Option<&ComponentConfig>,
43        tx: &[BridgeChannelConfig<TxId>],
44        _rx: &[BridgeChannelConfig<RxId>],
45    ) -> CuResult<Self>
46    where
47        Self: Sized,
48    {
49        let board = P::create_board()?;
50        if P::Board::CHANNEL_COUNT > MAX_ESC_CHANNELS {
51            return Err(CuError::from(
52                "BDShot board exposes more channels than supported",
53            ));
54        }
55        if tx.len() > P::Board::CHANNEL_COUNT {
56            return Err(CuError::from("Too many Tx channels for board"));
57        }
58        let mut active = [false; MAX_ESC_CHANNELS];
59        for ch in tx {
60            active[ch.channel.id.as_index()] = true;
61        }
62        Ok(Self {
63            board,
64            telemetry_cache: Default::default(),
65            active_channels: active,
66        })
67    }
68
69    fn start(&mut self, _clock: &RobotClock) -> CuResult<()> {
70        let idle_frame = encode_frame(EscCommand::disarm());
71
72        let mut ready = true;
73        for i in 0..2048 {
74            // Arbitrary long, TODO(gbin): move that to the config
75            for idx in 0..P::Board::CHANNEL_COUNT {
76                if !self.active_channels[idx] {
77                    continue;
78                }
79                debug!("Sending disarm frames {}", idx);
80                self.board.delay(200);
81                if let Some(sample) = self.board.exchange(idx, idle_frame) {
82                    self.telemetry_cache[idx] = Some(EscTelemetry {
83                        sample: Some(sample),
84                    });
85                } else {
86                    ready = false;
87                }
88            }
89            if ready {
90                break;
91            }
92            info!("Waiting for ESCs startup {}...", i);
93        }
94
95        if !ready {
96            error!("ESC TIMEOUT");
97            return Err(CuError::from("Timeout waiting for ESC to start up"));
98        }
99
100        let telemetry_cmd = EscCommand {
101            throttle: 13,
102            request_telemetry: true,
103        };
104        let telemetry_frame = encode_frame(telemetry_cmd);
105        for _ in 0..6 {
106            for idx in 0..P::Board::CHANNEL_COUNT {
107                if !self.active_channels[idx] {
108                    continue;
109                }
110                self.board.delay(200);
111                let _ = self.board.exchange(idx, telemetry_frame);
112            }
113        }
114
115        Ok(())
116    }
117
118    fn send<'a, Payload>(
119        &mut self,
120        _clock: &RobotClock,
121        channel: &'static BridgeChannel<<Self::Tx as BridgeChannelSet>::Id, Payload>,
122        msg: &CuMsg<Payload>,
123    ) -> CuResult<()>
124    where
125        Payload: CuMsgPayload + 'a,
126    {
127        let idx = channel.id().as_index();
128        if idx >= P::Board::CHANNEL_COUNT {
129            return Err(CuError::from("Channel not supported by board"));
130        }
131        if !self.active_channels[idx] {
132            return Ok(());
133        }
134        let payload: &CuMsg<EscCommand> = msg.downcast_ref()?;
135        let command = payload.payload().cloned().unwrap_or_default();
136        let frame = encode_frame(command);
137        if let Some(telemetry) = self.board.exchange(idx, frame) {
138            self.telemetry_cache[idx] = Some(EscTelemetry {
139                sample: Some(telemetry),
140            });
141        }
142        Ok(())
143    }
144
145    fn receive<'a, Payload>(
146        &mut self,
147        _clock: &RobotClock,
148        channel: &'static BridgeChannel<<Self::Rx as BridgeChannelSet>::Id, Payload>,
149        msg: &mut CuMsg<Payload>,
150    ) -> CuResult<()>
151    where
152        Payload: CuMsgPayload + 'a,
153    {
154        let idx = channel.id().as_index();
155        if idx >= P::Board::CHANNEL_COUNT {
156            msg.clear_payload();
157            return Ok(());
158        }
159        if !self.active_channels[idx] {
160            msg.clear_payload();
161            return Ok(());
162        }
163        let telemetry_msg: &mut CuMsg<EscTelemetry> = msg.downcast_mut()?;
164        if let Some(sample) = self.telemetry_cache[idx].take() {
165            telemetry_msg.set_payload(sample);
166        } else {
167            telemetry_msg.clear_payload();
168        }
169        Ok(())
170    }
171}
cu_bdshot/bridge.rs

cu_bdshot/
bridge.rs
