272 lines
8.2 KiB
Rust
272 lines
8.2 KiB
Rust
#![no_std]
|
|
#![no_main]
|
|
|
|
use embedded_alloc::Heap;
|
|
use panic_halt as _;
|
|
|
|
#[global_allocator]
|
|
static HEAP: Heap = Heap::empty();
|
|
|
|
use bxcan::Frame;
|
|
use core::cmp::Ordering;
|
|
use heapless::binary_heap::{BinaryHeap, Max};
|
|
|
|
use stm32f1xx_hal::pac::Interrupt;
|
|
extern crate alloc;
|
|
|
|
#[derive(Debug)]
|
|
pub struct PriorityFrame(Frame);
|
|
|
|
/// Ordering is based on the Identifier and frame type (data vs. remote) and can be used to sort
|
|
/// frames by priority.
|
|
impl Ord for PriorityFrame {
|
|
fn cmp(&self, other: &Self) -> Ordering {
|
|
self.0.priority().cmp(&other.0.priority())
|
|
}
|
|
}
|
|
|
|
impl PartialOrd for PriorityFrame {
|
|
fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
|
|
Some(self.cmp(other))
|
|
}
|
|
}
|
|
|
|
impl PartialEq for PriorityFrame {
|
|
fn eq(&self, other: &Self) -> bool {
|
|
self.cmp(other) == Ordering::Equal
|
|
}
|
|
}
|
|
|
|
impl Eq for PriorityFrame {}
|
|
|
|
fn enqueue_frame(queue: &mut BinaryHeap<PriorityFrame, Max, 16>, frame: Frame) {
|
|
queue.push(PriorityFrame(frame)).unwrap();
|
|
rtic::pend(Interrupt::USB_HP_CAN_TX);
|
|
}
|
|
|
|
#[rtic::app(device = stm32f1xx_hal::pac)]
|
|
mod app {
|
|
use cortex_m_semihosting::hprintln;
|
|
use heapless::binary_heap::{BinaryHeap, Max};
|
|
|
|
use super::{enqueue_frame, PriorityFrame};
|
|
use bxcan::{filter::Mask32, Fifo, Frame, Interrupts, Rx0, StandardId, Tx};
|
|
use stm32f1xx_hal::{
|
|
can::Can,
|
|
device::TIM2,
|
|
gpio::{Output, Pin},
|
|
pac::CAN1,
|
|
prelude::*,
|
|
timer::Delay,
|
|
};
|
|
|
|
#[shared]
|
|
struct Shared {
|
|
can_tx_queue: BinaryHeap<PriorityFrame, Max, 16>,
|
|
tx_count: usize,
|
|
}
|
|
|
|
#[local]
|
|
struct Local {
|
|
can_tx: Tx<Can<CAN1>>,
|
|
can_rx: Rx0<Can<CAN1>>,
|
|
led: Pin<'C', 13, Output>,
|
|
delay: Delay<TIM2, 1000000>,
|
|
counter: usize,
|
|
}
|
|
|
|
#[init]
|
|
fn init(ctx: init::Context) -> (Shared, Local, init::Monotonics) {
|
|
let mut flash = ctx.device.FLASH.constrain();
|
|
let rcc = ctx.device.RCC.constrain();
|
|
|
|
// let clocks = rcc.cfgr.freeze(&mut flash.acr);
|
|
|
|
let clocks = rcc
|
|
.cfgr
|
|
.use_hse(8.MHz())
|
|
.sysclk(64.MHz())
|
|
.hclk(64.MHz())
|
|
.pclk1(16.MHz())
|
|
.pclk2(64.MHz())
|
|
.freeze(&mut flash.acr);
|
|
|
|
let can = Can::new(ctx.device.CAN1, ctx.device.USB);
|
|
let mut gpioa = ctx.device.GPIOA.split();
|
|
let can_rx_pin = gpioa.pa11.into_floating_input(&mut gpioa.crh);
|
|
let can_tx_pin = gpioa.pa12.into_alternate_push_pull(&mut gpioa.crh);
|
|
let mut afio = ctx.device.AFIO.constrain();
|
|
can.assign_pins((can_tx_pin, can_rx_pin), &mut afio.mapr);
|
|
|
|
// APB1 (PCLK1): 16MHz, Bit rate: 1000kBit/s, Sample Point 87.5%
|
|
// Value was calculated with http://www.bittiming.can-wiki.info/
|
|
let mut can = bxcan::Can::builder(can)
|
|
.set_bit_timing(0x001c_0000)
|
|
.leave_disabled();
|
|
|
|
can.modify_filters()
|
|
.enable_bank(0, Fifo::Fifo0, Mask32::accept_all());
|
|
|
|
// Sync to the bus and start normal operation.
|
|
can.enable_interrupts(
|
|
Interrupts::TRANSMIT_MAILBOX_EMPTY | Interrupts::FIFO0_MESSAGE_PENDING,
|
|
);
|
|
|
|
nb::block!(can.enable_non_blocking()).unwrap();
|
|
|
|
let (can_tx, can_rx, _) = can.split();
|
|
|
|
let can_tx_queue = BinaryHeap::new();
|
|
|
|
let delay = ctx.device.TIM2.delay_us(&clocks);
|
|
|
|
let mut gpioc = ctx.device.GPIOC.split();
|
|
let led = gpioc.pc13.into_push_pull_output(&mut gpioc.crh);
|
|
|
|
(
|
|
Shared {
|
|
can_tx_queue,
|
|
tx_count: 0,
|
|
},
|
|
Local {
|
|
can_tx,
|
|
can_rx,
|
|
led,
|
|
delay,
|
|
counter: 0,
|
|
},
|
|
init::Monotonics(),
|
|
)
|
|
}
|
|
|
|
fn speed2array(speed: i16) -> [u8; 8] {
|
|
let b = speed.to_be_bytes();
|
|
[b[0], b[1], b[0], b[1], b[0], b[1], b[0], b[1]]
|
|
}
|
|
|
|
#[idle(shared = [can_tx_queue, tx_count], local = [delay])]
|
|
fn idle(mut cx: idle::Context) -> ! {
|
|
let mut tx_queue = cx.shared.can_tx_queue;
|
|
|
|
loop {
|
|
for speed in 0..=30 {
|
|
tx_queue.lock(|mut tx_queue| {
|
|
enqueue_frame(
|
|
&mut tx_queue,
|
|
Frame::new_data(StandardId::new(0x1FF).unwrap(), speed2array(speed * 1000)),
|
|
);
|
|
});
|
|
|
|
cx.local.delay.delay(100.millis());
|
|
}
|
|
|
|
for _ in 0..10 {
|
|
tx_queue.lock(|mut tx_queue| {
|
|
enqueue_frame(
|
|
&mut tx_queue,
|
|
Frame::new_data(StandardId::new(0x1FF).unwrap(), speed2array(30000)),
|
|
);
|
|
});
|
|
|
|
cx.local.delay.delay(100.millis());
|
|
}
|
|
|
|
for speed in 0..=30 {
|
|
tx_queue.lock(|mut tx_queue| {
|
|
enqueue_frame(
|
|
&mut tx_queue,
|
|
Frame::new_data(
|
|
StandardId::new(0x1FF).unwrap(),
|
|
speed2array(30000 - (speed * 1000)),
|
|
),
|
|
);
|
|
});
|
|
|
|
cx.local.delay.delay(100.millis());
|
|
}
|
|
|
|
for speed in 0..=30 {
|
|
tx_queue.lock(|mut tx_queue| {
|
|
enqueue_frame(
|
|
&mut tx_queue,
|
|
Frame::new_data(StandardId::new(0x1FF).unwrap(), speed2array(speed * 1000 * -1)),
|
|
);
|
|
});
|
|
|
|
cx.local.delay.delay(100.millis());
|
|
}
|
|
|
|
for _ in 0..10 {
|
|
tx_queue.lock(|mut tx_queue| {
|
|
enqueue_frame(
|
|
&mut tx_queue,
|
|
Frame::new_data(StandardId::new(0x1FF).unwrap(), speed2array(30000 * -1)),
|
|
);
|
|
});
|
|
|
|
cx.local.delay.delay(100.millis());
|
|
}
|
|
|
|
for speed in 0..=30 {
|
|
tx_queue.lock(|mut tx_queue| {
|
|
enqueue_frame(
|
|
&mut tx_queue,
|
|
Frame::new_data(
|
|
StandardId::new(0x1FF).unwrap(),
|
|
speed2array(( 30000 - (speed * 1000) ) * -1),
|
|
),
|
|
);
|
|
});
|
|
|
|
cx.local.delay.delay(100.millis());
|
|
}
|
|
}
|
|
}
|
|
|
|
// This ISR is triggered by each finished frame transmission.
|
|
#[task(binds = USB_HP_CAN_TX, local = [can_tx, led], shared = [can_tx_queue, tx_count])]
|
|
fn can_tx(cx: can_tx::Context) {
|
|
let tx = cx.local.can_tx;
|
|
let mut tx_queue = cx.shared.can_tx_queue;
|
|
let mut tx_count = cx.shared.tx_count;
|
|
|
|
tx.clear_interrupt_flags();
|
|
|
|
// There is now a free mailbox. Try to transmit pending frames until either
|
|
// the queue is empty or transmission would block the execution of this ISR.
|
|
(&mut tx_queue, &mut tx_count).lock(|tx_queue, tx_count| {
|
|
while let Some(frame) = tx_queue.peek() {
|
|
match tx.transmit(&frame.0) {
|
|
Ok(status) => match status.dequeued_frame() {
|
|
None => {
|
|
tx_queue.pop();
|
|
*tx_count += 1;
|
|
cx.local.led.toggle();
|
|
}
|
|
Some(pending_frame) => {
|
|
tx_queue.pop();
|
|
enqueue_frame(tx_queue, pending_frame.clone());
|
|
}
|
|
},
|
|
Err(nb::Error::WouldBlock) => break,
|
|
Err(_) => unreachable!(),
|
|
}
|
|
}
|
|
});
|
|
}
|
|
|
|
#[task(binds = USB_LP_CAN_RX0, local = [can_rx], shared = [can_tx_queue])]
|
|
fn can_rx0(mut cx: can_rx0::Context) {
|
|
// Echo back received packages with correct priority ordering.
|
|
loop {
|
|
match cx.local.can_rx.receive() {
|
|
Ok(frame) => {
|
|
// let data = frame.data().unwrap();
|
|
}
|
|
Err(nb::Error::WouldBlock) => break,
|
|
Err(nb::Error::Other(_)) => {} // Ignore overrun errors.
|
|
}
|
|
}
|
|
}
|
|
}
|