stm_32/peripherals/
sbus.rs

1// ******************************************************************************
2// * File     : platforms/stm_32/src/peripherals/sbus.rs
3// * Date     : June 28, 2026
4// ******************************************************************************
5// *
6// * Copyright (c) 2023, AeroVironment, Inc.
7// * All rights reserved.
8// *
9// * Redistribution and use in source and binary forms, with or without
10// * modification, are permitted provided that the following conditions are met:
11// *
12// * 1.Redistributions of source code must retain the above copyright notice, this
13// * list of conditions and the following disclaimer.
14// *
15// * 2.Redistributions in binary form must reproduce the above copyright notice,
16// * this list of conditions and the following disclaimer in the documentation
17// * and/or other materials provided with the distribution.
18// *
19// * 3.Neither the name of the copyright holder nor the names of its
20// * contributors may be used to endorse or promote products derived from
21// * this software without specific prior written permission.
22// *
23// * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
24// * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
25// * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
26// * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
27// * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
28// * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
29// * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30// * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
31// * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
32// * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
33// *
34// ******************************************************************************
35
36use core::sync::atomic::{AtomicU32, Ordering};
37
38use embassy_stm32::mode::Async;
39use embassy_stm32::usart::UartRx;
40use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
41use embassy_sync::signal::Signal;
42use embassy_time::Duration;
43use embassy_time::Instant;
44
45use veloxity_core::errors;
46use veloxity_core::packets;
47
48pub static RC_SIGNAL: Signal<
49    CriticalSectionRawMutex,
50    Result<packets::RcPacket, errors::SensorError>,
51> = Signal::<CriticalSectionRawMutex, Result<packets::RcPacket, errors::SensorError>>::new();
52
53pub const SBUS_11_BIT_CHANNELS: usize = 16;
54pub const SBUS_BINARY_CHANNELS: usize = 2; // does not include status information
55pub const SBUS_CHANNELS: usize = SBUS_11_BIT_CHANNELS + SBUS_BINARY_CHANNELS;
56
57static SBUS_READ_OK: AtomicU32 = AtomicU32::new(0);
58static SBUS_READ_ERR: AtomicU32 = AtomicU32::new(0);
59static SBUS_LAST_READ_SIZE: AtomicU32 = AtomicU32::new(0);
60static SBUS_SIZE_25: AtomicU32 = AtomicU32::new(0);
61static SBUS_VALID_FRAME: AtomicU32 = AtomicU32::new(0);
62static SBUS_BAD_HEADER: AtomicU32 = AtomicU32::new(0);
63static SBUS_BAD_FOOTER: AtomicU32 = AtomicU32::new(0);
64static SBUS_SIGNAL: AtomicU32 = AtomicU32::new(0);
65static SBUS_TIMEOUT: AtomicU32 = AtomicU32::new(0);
66static SBUS_LAST_STATUS: AtomicU32 = AtomicU32::new(0);
67
68#[derive(Clone, Copy, Debug, Default)]
69pub struct SbusDiagnostics {
70    pub read_ok: u32,
71    pub read_err: u32,
72    pub last_read_size: u32,
73    pub size_25: u32,
74    pub valid_frame: u32,
75    pub bad_header: u32,
76    pub bad_footer: u32,
77    pub signal: u32,
78    pub timeout: u32,
79    pub last_status: u32,
80}
81
82pub fn diagnostics() -> SbusDiagnostics {
83    SbusDiagnostics {
84        read_ok: SBUS_READ_OK.load(Ordering::Relaxed),
85        read_err: SBUS_READ_ERR.load(Ordering::Relaxed),
86        last_read_size: SBUS_LAST_READ_SIZE.load(Ordering::Relaxed),
87        size_25: SBUS_SIZE_25.load(Ordering::Relaxed),
88        valid_frame: SBUS_VALID_FRAME.load(Ordering::Relaxed),
89        bad_header: SBUS_BAD_HEADER.load(Ordering::Relaxed),
90        bad_footer: SBUS_BAD_FOOTER.load(Ordering::Relaxed),
91        signal: SBUS_SIGNAL.load(Ordering::Relaxed),
92        timeout: SBUS_TIMEOUT.load(Ordering::Relaxed),
93        last_status: SBUS_LAST_STATUS.load(Ordering::Relaxed),
94    }
95}
96
97pub struct SbusRC {
98    pub uart: UartRx<'static, Async>,
99}
100
101// Brute force extract of 11 bits
102pub fn extract_chan(bytes: &[u8], bit_offset: usize) -> f32 {
103    // Calculate the start byte and the bit position within that byte
104    let byte_offset = bit_offset / 8;
105    let bit_start = bit_offset % 8;
106
107    let mut value: u16 = 0;
108    for i in 0..11 {
109        let byte_index = byte_offset + (bit_start + i) / 8;
110        let bit_index = (bit_start + i) % 8;
111        let bit = (bytes[byte_index] >> bit_index) & 1;
112        value |= (bit as u16) << i;
113    }
114
115    value as f32
116}
117
118impl SbusRC {
119    pub async fn run(&mut self) {
120        let mut buffer = [0u8; 25];
121
122        let mut chan = [0f32; SBUS_CHANNELS];
123        let mut timeout = Instant::now() + Duration::from_secs(1);
124        let mut rc_chan = [0.0f32; packets::RC_PACKET_CHANNELS];
125        loop {
126            // Read a packet
127            let result = self.uart.read_until_idle(&mut buffer).await;
128            if let Ok(size) = result {
129                SBUS_READ_OK.fetch_add(1, Ordering::Relaxed);
130                SBUS_LAST_READ_SIZE.store(size as u32, Ordering::Relaxed);
131                if size == buffer.len() {
132                    SBUS_SIZE_25.fetch_add(1, Ordering::Relaxed);
133                }
134                timeout = Instant::now() + Duration::from_secs(1);
135                let valid_header = buffer[0] == 0x0F;
136                let valid_footer = (buffer[24] == 0x00)
137                    || (buffer[24] == 0x04)
138                    || (buffer[24] == 0x14)
139                    || (buffer[24] == 0x24)
140                    || (buffer[24] == 0x34);
141                if valid_header && valid_footer {
142                    SBUS_VALID_FRAME.fetch_add(1, Ordering::Relaxed);
143                    let dig = buffer[23] as u16;
144                    SBUS_LAST_STATUS.store(dig as u32, Ordering::Relaxed);
145
146                    // get 16 servo (11-bit) channels
147                    for i in 0..SBUS_11_BIT_CHANNELS {
148                        chan[i] = (extract_chan(&buffer, 8 + i * 11) - 172.0) / 1639.0;
149                    }
150                    // get the two binary channels
151                    chan[0 + SBUS_11_BIT_CHANNELS] = ((((dig) & 0x01) as f32) - 172.0) / 1639.0; // rosflight weird scaling
152                    chan[1 + SBUS_11_BIT_CHANNELS] =
153                        ((((dig >> 1) & 0x01) as f32) - 172.0) / 1639.0; // rosflight weird scaling
154
155                    let header = packets::RosflightPacketHeader {
156                        timestamp: Instant::now().as_micros(),
157                        status: dig,
158                    };
159
160                    rc_chan = [0.0f32; packets::RC_PACKET_CHANNELS];
161
162                    let mut len = SBUS_CHANNELS;
163
164                    if SBUS_CHANNELS > packets::RC_PACKET_CHANNELS {
165                        len = packets::RC_PACKET_CHANNELS;
166                    }
167
168                    for i in 0..len {
169                        rc_chan[i] = chan[i];
170                    }
171
172                    let rc_packet = packets::RcPacket {
173                        header,
174                        n_chan: 24,
175                        chan: rc_chan,
176                        lol: (dig & 0x0C) != 0, // either bit 2 or 3 will signal a loss of link.
177                    };
178                    SBUS_SIGNAL.fetch_add(1, Ordering::Relaxed);
179                    RC_SIGNAL.signal(Ok(rc_packet));
180                } else {
181                    if !valid_header {
182                        SBUS_BAD_HEADER.fetch_add(1, Ordering::Relaxed);
183                    }
184                    if valid_header && !valid_footer {
185                        SBUS_BAD_FOOTER.fetch_add(1, Ordering::Relaxed);
186                    }
187                }
188            } else {
189                SBUS_READ_ERR.fetch_add(1, Ordering::Relaxed);
190            }
191
192            if Instant::now() > timeout {
193                timeout = Instant::now() + Duration::from_secs(1);
194                let dig = 0x1C; // set bitfield for timeout
195                SBUS_TIMEOUT.fetch_add(1, Ordering::Relaxed);
196                SBUS_LAST_STATUS.store(dig as u32, Ordering::Relaxed);
197                let header = packets::RosflightPacketHeader {
198                    timestamp: Instant::now().as_micros(),
199                    status: dig,
200                };
201                let rc_packet = packets::RcPacket {
202                    header,
203                    n_chan: 24,
204                    chan: rc_chan,          // last known good values
205                    lol: (dig & 0x1C) != 0, // signal a loss of link bits
206                };
207                SBUS_SIGNAL.fetch_add(1, Ordering::Relaxed);
208                RC_SIGNAL.signal(Ok(rc_packet));
209            }
210        }
211    }
212}
213
214#[embassy_executor::task]
215pub async fn task(mut sbus: SbusRC) {
216    sbus.run().await;
217}
stm_32/peripherals/sbus.rs

stm_32/peripherals/
sbus.rs
