350 lines
12 KiB
Rust
350 lines
12 KiB
Rust
#![no_std]
|
|
#![no_main]
|
|
use arbitrary_int::{u11, u14};
|
|
use cortex_m_semihosting::debug::{self, EXIT_FAILURE, EXIT_SUCCESS};
|
|
use satrs_stm32f3_disco_rtic::{create_tm_packet, tm_size, CcsdsPacketId, Request, Response};
|
|
use spacepackets::{CcsdsPacketCreationError, SpHeader};
|
|
|
|
use defmt_rtt as _; // global logger
|
|
|
|
use panic_probe as _;
|
|
|
|
use rtic::app;
|
|
|
|
#[allow(unused_imports)]
|
|
use rtic_monotonics::fugit::{MillisDurationU32, TimerInstantU32};
|
|
use rtic_monotonics::systick::prelude::*;
|
|
|
|
use crate::app::Mono;
|
|
|
|
const UART_BAUD: u32 = 115200;
|
|
const DEFAULT_BLINK_FREQ_MS: u32 = 1000;
|
|
const TX_HANDLER_FREQ_MS: u32 = 20;
|
|
const MAX_TC_LEN: usize = 128;
|
|
const MAX_TM_LEN: usize = 128;
|
|
|
|
pub const PUS_APID: u11 = u11::new(0x02);
|
|
|
|
// This is the predictable maximum overhead of the COBS encoding scheme.
|
|
// It is simply the maximum packet lenght dividied by 254 rounded up.
|
|
const COBS_TM_OVERHEAD: usize = cobs::max_encoding_overhead(MAX_TM_LEN);
|
|
|
|
const TM_BUF_LEN: usize = MAX_TC_LEN + COBS_TM_OVERHEAD;
|
|
|
|
const TC_DMA_BUF_LEN: usize = 512;
|
|
|
|
type TmPacket = heapless::Vec<u8, MAX_TM_LEN>;
|
|
|
|
static TM_QUEUE: heapless::mpmc::Queue<TmPacket, 16> = heapless::mpmc::Queue::new();
|
|
|
|
#[derive(Debug, defmt::Format, thiserror::Error)]
|
|
pub enum TmSendError {
|
|
#[error("packet creation error: {0}")]
|
|
PacketCreation(#[from] CcsdsPacketCreationError),
|
|
#[error("queue error")]
|
|
Queue,
|
|
}
|
|
|
|
#[derive(Debug, defmt::Format)]
|
|
pub struct RequestWithTcId {
|
|
pub request: Request,
|
|
pub tc_id: CcsdsPacketId,
|
|
}
|
|
|
|
#[app(device = embassy_stm32)]
|
|
mod app {
|
|
use core::time::Duration;
|
|
|
|
use super::*;
|
|
use arbitrary_int::u14;
|
|
use rtic::Mutex;
|
|
use rtic_sync::{
|
|
channel::{Receiver, Sender},
|
|
make_channel,
|
|
};
|
|
use satrs_stm32f3_disco_rtic::{CcsdsPacketId, LedPinSet, Request, Response};
|
|
use spacepackets::CcsdsPacketReader;
|
|
|
|
systick_monotonic!(Mono, 1000);
|
|
|
|
embassy_stm32::bind_interrupts!(struct Irqs {
|
|
USART2 => embassy_stm32::usart::InterruptHandler<embassy_stm32::peripherals::USART2>;
|
|
});
|
|
|
|
#[shared]
|
|
struct Shared {
|
|
blink_freq: Duration,
|
|
}
|
|
|
|
#[local]
|
|
struct Local {
|
|
leds: satrs_stm32f3_disco_rtic::Leds,
|
|
current_dir: satrs_stm32f3_disco_rtic::Direction,
|
|
seq_count: u14,
|
|
tx: embassy_stm32::usart::UartTx<'static, embassy_stm32::mode::Async>,
|
|
rx: embassy_stm32::usart::RingBufferedUartRx<'static>,
|
|
}
|
|
|
|
#[init]
|
|
fn init(cx: init::Context) -> (Shared, Local) {
|
|
static DMA_BUF: static_cell::ConstStaticCell<[u8; TC_DMA_BUF_LEN]> =
|
|
static_cell::ConstStaticCell::new([0; TC_DMA_BUF_LEN]);
|
|
|
|
let p = embassy_stm32::init(Default::default());
|
|
|
|
let (req_sender, req_receiver) = make_channel!(RequestWithTcId, 16);
|
|
// Initialize the systick interrupt & obtain the token to prove that we did
|
|
Mono::start(cx.core.SYST, 8_000_000);
|
|
|
|
defmt::info!("sat-rs demo application for the STM32F3-Discovery with RTICv2");
|
|
let led_pin_set = LedPinSet {
|
|
pin_n: p.PE8,
|
|
pin_ne: p.PE9,
|
|
pin_e: p.PE10,
|
|
pin_se: p.PE11,
|
|
pin_s: p.PE12,
|
|
pin_sw: p.PE13,
|
|
pin_w: p.PE14,
|
|
pin_nw: p.PE15,
|
|
};
|
|
let leds = satrs_stm32f3_disco_rtic::Leds::new(led_pin_set);
|
|
|
|
let mut config = embassy_stm32::usart::Config::default();
|
|
config.baudrate = UART_BAUD;
|
|
let uart = embassy_stm32::usart::Uart::new(
|
|
p.USART2, p.PA3, p.PA2, Irqs, p.DMA1_CH7, p.DMA1_CH6, config,
|
|
)
|
|
.unwrap();
|
|
|
|
let (tx, rx) = uart.split();
|
|
defmt::info!("Spawning tasks");
|
|
blinky::spawn().unwrap();
|
|
serial_tx_handler::spawn().unwrap();
|
|
serial_rx_handler::spawn(req_sender).unwrap();
|
|
req_handler::spawn(req_receiver).unwrap();
|
|
|
|
(
|
|
Shared {
|
|
blink_freq: Duration::from_millis(DEFAULT_BLINK_FREQ_MS as u64),
|
|
},
|
|
Local {
|
|
leds,
|
|
tx,
|
|
seq_count: u14::new(0),
|
|
rx: rx.into_ring_buffered(DMA_BUF.take()),
|
|
current_dir: satrs_stm32f3_disco_rtic::Direction::North,
|
|
},
|
|
)
|
|
}
|
|
|
|
#[task(local = [leds, current_dir], shared=[blink_freq])]
|
|
async fn blinky(mut cx: blinky::Context) {
|
|
loop {
|
|
cx.local.leds.blink_next(cx.local.current_dir);
|
|
let current_blink_freq = cx.shared.blink_freq.lock(|current| *current);
|
|
Mono::delay(MillisDurationU32::from_ticks(
|
|
current_blink_freq.as_millis() as u32,
|
|
))
|
|
.await;
|
|
}
|
|
}
|
|
|
|
#[task(
|
|
local = [
|
|
tx,
|
|
encoded_buf: [u8; TM_BUF_LEN] = [0; TM_BUF_LEN]
|
|
],
|
|
shared = [],
|
|
)]
|
|
async fn serial_tx_handler(cx: serial_tx_handler::Context) {
|
|
loop {
|
|
while let Some(vec) = TM_QUEUE.dequeue() {
|
|
let encoded_len =
|
|
cobs::encode_including_sentinels(&vec[0..vec.len()], cx.local.encoded_buf);
|
|
defmt::debug!("sending {} bytes over UART", encoded_len);
|
|
cx.local
|
|
.tx
|
|
.write(&cx.local.encoded_buf[0..encoded_len])
|
|
.await
|
|
.unwrap();
|
|
continue;
|
|
}
|
|
Mono::delay(TX_HANDLER_FREQ_MS.millis()).await;
|
|
}
|
|
}
|
|
|
|
#[task(
|
|
local = [
|
|
rx,
|
|
read_buf: [u8; 128] = [0; 128],
|
|
decode_buf: [u8; MAX_TC_LEN] = [0; MAX_TC_LEN],
|
|
],
|
|
shared = [blink_freq]
|
|
)]
|
|
async fn serial_rx_handler(
|
|
cx: serial_rx_handler::Context,
|
|
mut sender: Sender<'static, RequestWithTcId, 16>,
|
|
) {
|
|
let mut decoder = cobs::CobsDecoder::new(cx.local.decode_buf);
|
|
loop {
|
|
match cx.local.rx.read(cx.local.read_buf).await {
|
|
Ok(bytes) => {
|
|
defmt::debug!("received {} bytes over UART", bytes);
|
|
for byte in cx.local.read_buf[0..bytes].iter() {
|
|
match decoder.feed(*byte) {
|
|
Ok(None) => (),
|
|
Ok(Some(packet_size)) => {
|
|
match CcsdsPacketReader::new_with_checksum(
|
|
&decoder.dest()[0..packet_size],
|
|
) {
|
|
Ok(packet) => {
|
|
let packet_id = packet.packet_id();
|
|
let psc = packet.psc();
|
|
let tc_packet_id = CcsdsPacketId { packet_id, psc };
|
|
if let Ok(request) =
|
|
postcard::from_bytes::<Request>(packet.packet_data())
|
|
{
|
|
sender
|
|
.send(RequestWithTcId {
|
|
request,
|
|
tc_id: tc_packet_id,
|
|
})
|
|
.await
|
|
.unwrap();
|
|
}
|
|
}
|
|
Err(e) => {
|
|
defmt::error!("error unpacking ccsds packet: {}", e);
|
|
}
|
|
}
|
|
}
|
|
Err(e) => {
|
|
defmt::error!("cobs decoding error: {}", e);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
Err(e) => {
|
|
defmt::error!("uart read error: {}", e);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
#[task(shared = [blink_freq], local = [seq_count])]
|
|
async fn req_handler(
|
|
mut cx: req_handler::Context,
|
|
mut receiver: Receiver<'static, RequestWithTcId, 16>,
|
|
) {
|
|
loop {
|
|
match receiver.recv().await {
|
|
Ok(request_with_tc_id) => {
|
|
let tm_send_result = match request_with_tc_id.request {
|
|
Request::Ping => handle_ping_request(&mut cx, request_with_tc_id.tc_id),
|
|
Request::ChangeBlinkFrequency(duration) => {
|
|
handle_change_blink_frequency_request(
|
|
&mut cx,
|
|
request_with_tc_id.tc_id,
|
|
duration,
|
|
)
|
|
}
|
|
};
|
|
if let Err(e) = tm_send_result {
|
|
defmt::error!("error sending TM response: {}", e);
|
|
}
|
|
}
|
|
Err(_e) => defmt::error!("request receive error"),
|
|
}
|
|
}
|
|
}
|
|
|
|
fn handle_ping_request(
|
|
cx: &mut req_handler::Context,
|
|
tc_packet_id: CcsdsPacketId,
|
|
) -> Result<(), TmSendError> {
|
|
defmt::info!("Received PUS ping telecommand, sending ping reply");
|
|
send_tm(tc_packet_id, Response::CommandDone, *cx.local.seq_count)?;
|
|
*cx.local.seq_count = cx.local.seq_count.wrapping_add(u14::new(1));
|
|
Ok(())
|
|
}
|
|
|
|
fn handle_change_blink_frequency_request(
|
|
cx: &mut req_handler::Context,
|
|
tc_packet_id: CcsdsPacketId,
|
|
duration: Duration,
|
|
) -> Result<(), TmSendError> {
|
|
defmt::info!(
|
|
"Received ChangeBlinkFrequency request, new frequency: {} ms",
|
|
duration.as_millis()
|
|
);
|
|
cx.shared
|
|
.blink_freq
|
|
.lock(|blink_freq| *blink_freq = duration);
|
|
send_tm(tc_packet_id, Response::CommandDone, *cx.local.seq_count)?;
|
|
*cx.local.seq_count = cx.local.seq_count.wrapping_add(u14::new(1));
|
|
Ok(())
|
|
}
|
|
}
|
|
|
|
fn send_tm(
|
|
tc_packet_id: CcsdsPacketId,
|
|
response: Response,
|
|
current_seq_count: u14,
|
|
) -> Result<(), TmSendError> {
|
|
let sp_header = SpHeader::new_for_unseg_tc(PUS_APID, current_seq_count, 0);
|
|
let tm_header = satrs_stm32f3_disco_rtic::TmHeader {
|
|
tc_packet_id: Some(tc_packet_id),
|
|
uptime_millis: Mono::now().duration_since_epoch().to_millis(),
|
|
};
|
|
let mut tm_packet = TmPacket::new();
|
|
let tm_size = tm_size(&tm_header, &response);
|
|
tm_packet.resize(tm_size, 0).expect("vec resize failed");
|
|
create_tm_packet(&mut tm_packet, sp_header, tm_header, response)?;
|
|
if TM_QUEUE.enqueue(tm_packet).is_err() {
|
|
defmt::warn!("TC queue full");
|
|
return Err(TmSendError::Queue);
|
|
}
|
|
Ok(())
|
|
}
|
|
|
|
// same panicking *behavior* as `panic-probe` but doesn't print a panic message
|
|
// this prevents the panic message being printed *twice* when `defmt::panic` is invoked
|
|
#[defmt::panic_handler]
|
|
fn panic() -> ! {
|
|
cortex_m::asm::udf()
|
|
}
|
|
|
|
/// Terminates the application and makes a semihosting-capable debug tool exit
|
|
/// with status code 0.
|
|
pub fn exit() -> ! {
|
|
loop {
|
|
debug::exit(EXIT_SUCCESS);
|
|
}
|
|
}
|
|
|
|
/// Hardfault handler.
|
|
///
|
|
/// Terminates the application and makes a semihosting-capable debug tool exit
|
|
/// with an error. This seems better than the default, which is to spin in a
|
|
/// loop.
|
|
#[cortex_m_rt::exception]
|
|
unsafe fn HardFault(_frame: &cortex_m_rt::ExceptionFrame) -> ! {
|
|
loop {
|
|
debug::exit(EXIT_FAILURE);
|
|
}
|
|
}
|
|
|
|
// defmt-test 0.3.0 has the limitation that this `#[tests]` attribute can only be used
|
|
// once within a crate. the module can be in any file but there can only be at most
|
|
// one `#[tests]` module in this library crate
|
|
#[cfg(test)]
|
|
#[defmt_test::tests]
|
|
mod unit_tests {
|
|
use defmt::assert;
|
|
|
|
#[test]
|
|
fn it_works() {
|
|
assert!(true)
|
|
}
|
|
}
|