boards/kakute-f7-mini-v3: defer work to single-owner tasks
This commit is contained in:
Onur Özkan committed
1 parent
7c2bf333ac
commit
a31a1beb71
1 file changed
+252
-326
@@ -1,367 +1,293 @@
|
||||
#![no_std]
|
||||
#![no_main]
|
||||
|
||||
use core::sync::atomic::{AtomicBool, AtomicU8, Ordering};
|
||||
|
||||
use cortex_m_rt as _;
|
||||
use kaptan_runtime::LatestImuMailbox;
|
||||
use panic_halt as _;
|
||||
|
||||
#[rtic::app(device = stm32f7::stm32f722, peripherals = true)]
|
||||
mod app {
|
||||
use core::fmt::{self, Write as _};
|
||||
mod board;
|
||||
mod routes;
|
||||
|
||||
#[derive(Clone, Copy, Debug)]
|
||||
pub struct RawImuSample {
|
||||
pub acceleration: [i16; 3],
|
||||
pub temperature: i16,
|
||||
pub angular_rate: [i16; 3],
|
||||
}
|
||||
const IMU_EVENT_DRDY: u8 = 1 << 0;
|
||||
const IMU_EVENT_TRANSFER_COMPLETE: u8 = 1 << 1;
|
||||
const IMU_EVENT_TRANSFER_FAULT: u8 = 1 << 2;
|
||||
const IMU_EVENT_TIMER: u8 = 1 << 3;
|
||||
const TELEMETRY_EVENT_SAMPLE: u8 = 1 << 0;
|
||||
const TELEMETRY_EVENT_HEARTBEAT: u8 = 1 << 1;
|
||||
const TELEMETRY_EVENT_DMA: u8 = 1 << 2;
|
||||
|
||||
static IMU_EVENTS: AtomicU8 = AtomicU8::new(0);
|
||||
static TELEMETRY_EVENTS: AtomicU8 = AtomicU8::new(0);
|
||||
static IMU_QUEUED: AtomicBool = AtomicBool::new(false);
|
||||
static TELEMETRY_QUEUED: AtomicBool = AtomicBool::new(false);
|
||||
static SAMPLES: LatestImuMailbox = LatestImuMailbox::new();
|
||||
static UART_COMPLETION: AtomicU8 = AtomicU8::new(0);
|
||||
|
||||
#[rtic::app(device = stm32f7::stm32f722, peripherals = true, dispatchers = [TIM3, TIM5])]
|
||||
mod app {
|
||||
use super::{
|
||||
IMU_EVENT_DRDY, IMU_EVENT_TIMER, IMU_EVENT_TRANSFER_COMPLETE, IMU_EVENT_TRANSFER_FAULT,
|
||||
IMU_EVENTS, IMU_QUEUED, Ordering, SAMPLES, TELEMETRY_EVENT_DMA, TELEMETRY_EVENT_HEARTBEAT,
|
||||
TELEMETRY_EVENT_SAMPLE, TELEMETRY_EVENTS, TELEMETRY_QUEUED, UART_COMPLETION,
|
||||
};
|
||||
use crate::board::{
|
||||
CORE_CLOCK_MHZ, HEARTBEAT_TIMER_CONFIG, MAVLINK_COMPONENT_ID, MAVLINK_SYSTEM_ID,
|
||||
MPU6000_CONFIG, USART1_BAUD, USART1_CLOCK_HZ,
|
||||
};
|
||||
use crate::routes::KakuteMpu6000SpiRoute;
|
||||
use kaptan_dma::TxCompletion;
|
||||
use kaptan_drivers::mpu6000::Mpu6000;
|
||||
use kaptan_runtime::{ImuEvent, ImuService, TelemetryService};
|
||||
use kaptan_spi::SpiCompletion;
|
||||
use kaptan_stm32f7::stm32f722::{
|
||||
ActiveLowLedPd2, DcacheDisabledDmaMemory, DwtMicrosecondClock, Spi1, Tim2Periodic,
|
||||
Usart1Tx, Usart1TxDma, configure_exti4_pin_pa4, configure_spi1_pins_pa5_pa6_pa7,
|
||||
configure_usart1_tx_pin_pa9, select_hsi16_profile,
|
||||
};
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
#[shared]
|
||||
struct Shared {
|
||||
imu_data_ready_count: u32,
|
||||
heartbeat_count: u32,
|
||||
latest_imu_sample: RawImuSample,
|
||||
}
|
||||
struct Shared {}
|
||||
|
||||
#[local]
|
||||
struct Local {
|
||||
led: stm32f7::stm32f722::GPIOD,
|
||||
led_output_high: bool,
|
||||
heartbeat_timer: stm32f7::stm32f722::TIM2,
|
||||
last_data_ready_count: u32,
|
||||
imu_spi: stm32f7::stm32f722::SPI1,
|
||||
imu_chip_select: stm32f7::stm32f722::GPIOB,
|
||||
data_ready_interrupt: stm32f7::stm32f722::EXTI,
|
||||
telemetry: Uart1,
|
||||
imu: ImuService<DwtMicrosecondClock, KakuteMpu6000SpiRoute>,
|
||||
telemetry: TelemetryService<DcacheDisabledDmaMemory, Usart1TxDma, 4>,
|
||||
led: ActiveLowLedPd2,
|
||||
heartbeat_timer: Tim2Periodic,
|
||||
heartbeat_divider: u8,
|
||||
observed_sample: u32,
|
||||
}
|
||||
|
||||
struct Uart1(stm32f7::stm32f722::USART1);
|
||||
|
||||
#[init]
|
||||
fn init(context: init::Context) -> (Shared, Local) {
|
||||
fn init(mut context: init::Context) -> (Shared, Local) {
|
||||
let device = context.device;
|
||||
if select_hsi16_profile(&device.RCC).is_err() {
|
||||
safe_idle();
|
||||
}
|
||||
context.core.SCB.disable_dcache(&mut context.core.CPUID);
|
||||
let clock = match DwtMicrosecondClock::start::<CORE_CLOCK_MHZ>(
|
||||
&mut context.core.DCB,
|
||||
&mut context.core.DWT,
|
||||
) {
|
||||
Ok(clock) => clock,
|
||||
Err(_) => safe_idle(),
|
||||
};
|
||||
|
||||
device
|
||||
.RCC
|
||||
.ahb1enr()
|
||||
.modify(|_, write| write.gpioden().set_bit());
|
||||
device
|
||||
.GPIOD
|
||||
.moder()
|
||||
.modify(|_, write| write.moder2().output());
|
||||
|
||||
device
|
||||
.RCC
|
||||
.ahb1enr()
|
||||
.modify(|_, write| write.gpioaen().set_bit());
|
||||
device.GPIOA.moder().modify(|_, write| {
|
||||
write
|
||||
.moder5()
|
||||
.alternate()
|
||||
.moder6()
|
||||
.alternate()
|
||||
.moder7()
|
||||
.alternate()
|
||||
.moder4()
|
||||
.input()
|
||||
});
|
||||
device.GPIOA.afrl().modify(|_, write| {
|
||||
// SAFETY: AF5 selects SPI1 for PA5, PA6, and PA7.
|
||||
unsafe {
|
||||
write
|
||||
.afrl5()
|
||||
.bits(0b0101)
|
||||
.afrl6()
|
||||
.bits(0b0101)
|
||||
.afrl7()
|
||||
.bits(0b0101)
|
||||
}
|
||||
});
|
||||
device
|
||||
.GPIOA
|
||||
.moder()
|
||||
.modify(|_, write| write.moder9().alternate());
|
||||
device.GPIOA.afrh().modify(|_, write| {
|
||||
// SAFETY: AF7 selects USART1 transmit for PA9, routed to T1.
|
||||
unsafe { write.afrh9().bits(0b0111) }
|
||||
});
|
||||
device
|
||||
.RCC
|
||||
.ahb1enr()
|
||||
.modify(|_, write| write.gpioben().set_bit());
|
||||
device
|
||||
.GPIOB
|
||||
.moder()
|
||||
.modify(|_, write| write.moder2().output());
|
||||
device.GPIOB.bsrr().write(|write| write.bs2().set_bit());
|
||||
|
||||
device
|
||||
.RCC
|
||||
.apb2enr()
|
||||
.modify(|_, write| write.spi1en().set_bit());
|
||||
device.SPI1.cr1().write(|write| {
|
||||
write
|
||||
.mstr()
|
||||
.master()
|
||||
.br()
|
||||
.div64()
|
||||
.ssm()
|
||||
.set_bit()
|
||||
.ssi()
|
||||
.set_bit()
|
||||
});
|
||||
device.SPI1.cr2().modify(|_, write| write.frxth().set_bit());
|
||||
device.SPI1.cr1().modify(|_, write| write.spe().set_bit());
|
||||
|
||||
device
|
||||
.RCC
|
||||
.apb2enr()
|
||||
.modify(|_, write| write.usart1en().set_bit());
|
||||
device.USART1.brr().write(|write| {
|
||||
// SAFETY: 139 configures approximately 115200 baud from the 16 MHz reset clock.
|
||||
unsafe { write.bits(139) }
|
||||
});
|
||||
device
|
||||
.USART1
|
||||
.cr1()
|
||||
.write(|write| write.te().set_bit().ue().set_bit());
|
||||
|
||||
let mpu6000_detected = is_mpu6000(&device.SPI1, &device.GPIOB);
|
||||
if mpu6000_detected {
|
||||
write_mpu_register(&device.SPI1, &device.GPIOB, 0x6B, 0x01);
|
||||
write_mpu_register(&device.SPI1, &device.GPIOB, 0x1A, 0x03);
|
||||
write_mpu_register(&device.SPI1, &device.GPIOB, 0x19, 0x09);
|
||||
write_mpu_register(&device.SPI1, &device.GPIOB, 0x37, 0);
|
||||
write_mpu_register(&device.SPI1, &device.GPIOB, 0x38, 0x01);
|
||||
|
||||
device
|
||||
.RCC
|
||||
.apb2enr()
|
||||
.modify(|_, write| write.syscfgen().set_bit());
|
||||
device.SYSCFG.exticr2().modify(|_, write| {
|
||||
// SAFETY: EXTI4 source 0 selects GPIOA.
|
||||
unsafe { write.exti4().bits(0) }
|
||||
});
|
||||
device.EXTI.imr().modify(|_, write| write.im4().clear_bit());
|
||||
device.EXTI.rtsr().modify(|_, write| write.tr4().set_bit());
|
||||
device
|
||||
.EXTI
|
||||
.ftsr()
|
||||
.modify(|_, write| write.tr4().clear_bit());
|
||||
device.EXTI.pr().write(|write| write.pr4().clear());
|
||||
device.EXTI.imr().modify(|_, write| write.im4().set_bit());
|
||||
configure_spi1_pins_pa5_pa6_pa7(&device.RCC, &device.GPIOA);
|
||||
configure_exti4_pin_pa4(&device.GPIOA);
|
||||
configure_usart1_tx_pin_pa9(&device.GPIOA);
|
||||
let spi = Spi1::new(&device.RCC, device.SPI1, device.DMA2);
|
||||
let route = KakuteMpu6000SpiRoute::new(&device.RCC, spi, device.GPIOB);
|
||||
let mut imu = ImuService::new(clock, Mpu6000::new(route, MPU6000_CONFIG));
|
||||
if imu.begin().is_err() {
|
||||
safe_idle();
|
||||
}
|
||||
|
||||
device
|
||||
.RCC
|
||||
.apb1enr()
|
||||
.modify(|_, write| write.tim2en().set_bit());
|
||||
device.TIM2.cr1().modify(|_, write| write.cen().clear_bit());
|
||||
device.TIM2.psc().write(|write| {
|
||||
// SAFETY: 15_999 fits in TIM2's 16-bit prescaler field.
|
||||
unsafe { write.psc().bits(15_999) }
|
||||
});
|
||||
device.TIM2.arr().write(|write| {
|
||||
// SAFETY: 999 fits in TIM2's 32-bit auto-reload field.
|
||||
unsafe { write.arr().bits(999) }
|
||||
});
|
||||
device.TIM2.egr().write(|write| write.ug().set_bit());
|
||||
device.TIM2.sr().modify(|_, write| write.uif().clear_bit());
|
||||
if mpu6000_detected {
|
||||
device.TIM2.dier().modify(|_, write| write.uie().set_bit());
|
||||
device.TIM2.cr1().modify(|_, write| write.cen().set_bit());
|
||||
}
|
||||
let uart = match Usart1Tx::new(&device.RCC, device.USART1, USART1_CLOCK_HZ, USART1_BAUD) {
|
||||
Ok(uart) => uart,
|
||||
Err(_) => safe_idle(),
|
||||
};
|
||||
// SAFETY: D-cache is disabled above and `memory.x` excludes DTCM.
|
||||
let memory = unsafe { DcacheDisabledDmaMemory::new() };
|
||||
let telemetry = TelemetryService::new(
|
||||
MAVLINK_SYSTEM_ID,
|
||||
MAVLINK_COMPONENT_ID,
|
||||
memory,
|
||||
Usart1TxDma::new(&device.RCC, uart),
|
||||
);
|
||||
|
||||
configure_exti4(&device.RCC, device.SYSCFG, device.EXTI);
|
||||
(
|
||||
Shared {
|
||||
imu_data_ready_count: 0,
|
||||
heartbeat_count: 0,
|
||||
latest_imu_sample: RawImuSample {
|
||||
acceleration: [0; 3],
|
||||
temperature: 0,
|
||||
angular_rate: [0; 3],
|
||||
},
|
||||
},
|
||||
Shared {},
|
||||
Local {
|
||||
led: device.GPIOD,
|
||||
led_output_high: false,
|
||||
heartbeat_timer: device.TIM2,
|
||||
last_data_ready_count: 0,
|
||||
imu_spi: device.SPI1,
|
||||
imu_chip_select: device.GPIOB,
|
||||
data_ready_interrupt: device.EXTI,
|
||||
telemetry: Uart1(device.USART1),
|
||||
imu,
|
||||
telemetry,
|
||||
led: ActiveLowLedPd2::new(&device.RCC, device.GPIOD),
|
||||
heartbeat_timer: Tim2Periodic::new(
|
||||
&device.RCC,
|
||||
device.TIM2,
|
||||
HEARTBEAT_TIMER_CONFIG,
|
||||
),
|
||||
heartbeat_divider: 0,
|
||||
observed_sample: 0,
|
||||
},
|
||||
)
|
||||
}
|
||||
|
||||
#[idle(
|
||||
shared = [heartbeat_count, latest_imu_sample],
|
||||
local = [telemetry]
|
||||
)]
|
||||
fn idle(mut context: idle::Context) -> ! {
|
||||
let mut last_heartbeat_count = 0;
|
||||
|
||||
#[idle]
|
||||
fn idle(_: idle::Context) -> ! {
|
||||
loop {
|
||||
let heartbeat_count = context.shared.heartbeat_count.lock(|count| *count);
|
||||
if heartbeat_count == last_heartbeat_count {
|
||||
cortex_m::asm::wfi();
|
||||
continue;
|
||||
cortex_m::asm::wfi();
|
||||
}
|
||||
}
|
||||
|
||||
#[task(binds = EXTI4, priority = 15)]
|
||||
fn exti4(_: exti4::Context) {
|
||||
// SAFETY: this handler exclusively acknowledges the board's EXTI4 bit.
|
||||
unsafe {
|
||||
(&*stm32f722::EXTI::ptr())
|
||||
.pr()
|
||||
.write(|write| write.pr4().clear())
|
||||
};
|
||||
schedule_imu(IMU_EVENT_DRDY);
|
||||
}
|
||||
|
||||
#[task(binds = DMA2_STREAM0, priority = 14)]
|
||||
fn spi_rx_dma(_: spi_rx_dma::Context) {
|
||||
match Spi1::acknowledge_rx_interrupt() {
|
||||
SpiCompletion::Complete => schedule_imu(IMU_EVENT_TRANSFER_COMPLETE),
|
||||
SpiCompletion::TransferFault => schedule_imu(IMU_EVENT_TRANSFER_FAULT),
|
||||
SpiCompletion::NotPending => {}
|
||||
}
|
||||
}
|
||||
|
||||
#[task(binds = DMA2_STREAM3, priority = 14)]
|
||||
fn spi_tx_dma(_: spi_tx_dma::Context) {
|
||||
if Spi1::acknowledge_tx_interrupt() == SpiCompletion::TransferFault {
|
||||
schedule_imu(IMU_EVENT_TRANSFER_FAULT);
|
||||
}
|
||||
}
|
||||
|
||||
#[task(binds = TIM2, priority = 4, local = [heartbeat_timer])]
|
||||
fn tim2(context: tim2::Context) {
|
||||
context.local.heartbeat_timer.acknowledge_update();
|
||||
schedule_imu(IMU_EVENT_TIMER);
|
||||
schedule_telemetry(TELEMETRY_EVENT_HEARTBEAT);
|
||||
}
|
||||
|
||||
#[task(binds = DMA2_STREAM7, priority = 10)]
|
||||
fn uart_dma(_: uart_dma::Context) {
|
||||
match Usart1TxDma::acknowledge_interrupt() {
|
||||
TxCompletion::Complete => {
|
||||
UART_COMPLETION.store(1, Ordering::Release);
|
||||
schedule_telemetry(TELEMETRY_EVENT_DMA);
|
||||
}
|
||||
|
||||
last_heartbeat_count = heartbeat_count;
|
||||
let imu_sample = context.shared.latest_imu_sample.lock(|sample| *sample);
|
||||
let _ = core::writeln!(
|
||||
context.local.telemetry,
|
||||
"accel={:?} temp={} gyro={:?}",
|
||||
imu_sample.acceleration,
|
||||
imu_sample.temperature,
|
||||
imu_sample.angular_rate,
|
||||
);
|
||||
TxCompletion::TransferFault | TxCompletion::Halted => {
|
||||
UART_COMPLETION.store(2, Ordering::Release);
|
||||
schedule_telemetry(TELEMETRY_EVENT_DMA);
|
||||
}
|
||||
TxCompletion::NotPending => {}
|
||||
}
|
||||
}
|
||||
|
||||
#[task(
|
||||
binds = EXTI4,
|
||||
priority = 15,
|
||||
shared = [imu_data_ready_count, latest_imu_sample],
|
||||
local = [imu_spi, imu_chip_select, data_ready_interrupt]
|
||||
)]
|
||||
fn imu_data_ready(mut context: imu_data_ready::Context) {
|
||||
context
|
||||
.local
|
||||
.data_ready_interrupt
|
||||
.pr()
|
||||
.write(|write| write.pr4().clear());
|
||||
let imu_sample = read_mpu6000_sample(context.local.imu_spi, context.local.imu_chip_select);
|
||||
context.shared.latest_imu_sample.lock(|sample| {
|
||||
*sample = imu_sample;
|
||||
});
|
||||
context.shared.imu_data_ready_count.lock(|count| {
|
||||
*count = count.wrapping_add(1);
|
||||
});
|
||||
}
|
||||
|
||||
#[task(
|
||||
binds = TIM2,
|
||||
priority = 1,
|
||||
shared = [imu_data_ready_count, heartbeat_count],
|
||||
local = [led, led_output_high, heartbeat_timer, last_data_ready_count]
|
||||
)]
|
||||
fn led_heartbeat(mut context: led_heartbeat::Context) {
|
||||
context
|
||||
.local
|
||||
.heartbeat_timer
|
||||
.sr()
|
||||
.modify(|_, write| write.uif().clear_bit());
|
||||
context.shared.heartbeat_count.lock(|count| {
|
||||
*count = count.wrapping_add(1);
|
||||
});
|
||||
let imu_data_ready_count = context.shared.imu_data_ready_count.lock(|count| *count);
|
||||
if imu_data_ready_count == *context.local.last_data_ready_count {
|
||||
*context.local.led_output_high = false;
|
||||
} else {
|
||||
*context.local.led_output_high = !*context.local.led_output_high;
|
||||
*context.local.last_data_ready_count = imu_data_ready_count;
|
||||
#[task(priority = 13, local = [imu])]
|
||||
async fn imu_worker(context: imu_worker::Context) {
|
||||
let events = IMU_EVENTS.swap(0, Ordering::AcqRel);
|
||||
if events & IMU_EVENT_TRANSFER_FAULT != 0 {
|
||||
if !matches!(
|
||||
context
|
||||
.local
|
||||
.imu
|
||||
.on_transfer_complete(SpiCompletion::TransferFault),
|
||||
ImuEvent::Fault(_)
|
||||
) {
|
||||
panic!("SPI fault without an active MPU transaction");
|
||||
}
|
||||
panic!("SPI DMA transfer fault");
|
||||
}
|
||||
|
||||
if *context.local.led_output_high {
|
||||
context
|
||||
if events & IMU_EVENT_TRANSFER_COMPLETE != 0 {
|
||||
match context
|
||||
.local
|
||||
.led
|
||||
.bsrr()
|
||||
.write(|write| write.br2().set_bit());
|
||||
} else {
|
||||
context
|
||||
.local
|
||||
.led
|
||||
.bsrr()
|
||||
.write(|write| write.bs2().set_bit());
|
||||
}
|
||||
}
|
||||
|
||||
fn is_mpu6000(spi: &stm32f7::stm32f722::SPI1, chip_select: &stm32f7::stm32f722::GPIOB) -> bool {
|
||||
chip_select.bsrr().write(|write| write.br2().set_bit());
|
||||
let _ = spi_transfer(spi, 0xF5);
|
||||
let identity = spi_transfer(spi, 0);
|
||||
while spi.sr().read().bsy().is_busy() {}
|
||||
chip_select.bsrr().write(|write| write.bs2().set_bit());
|
||||
|
||||
identity == 0x68
|
||||
}
|
||||
|
||||
fn spi_transfer(spi: &stm32f7::stm32f722::SPI1, byte: u8) -> u8 {
|
||||
while spi.sr().read().txe().is_not_empty() {}
|
||||
spi.dr8().write(|write| {
|
||||
// SAFETY: `byte` is an 8-bit SPI frame and fits DR8 exactly.
|
||||
unsafe { write.dr().bits(byte) }
|
||||
});
|
||||
while spi.sr().read().rxne().is_empty() {}
|
||||
|
||||
spi.dr8().read().dr().bits()
|
||||
}
|
||||
|
||||
fn write_mpu_register(
|
||||
spi: &stm32f7::stm32f722::SPI1,
|
||||
chip_select: &stm32f7::stm32f722::GPIOB,
|
||||
register: u8,
|
||||
value: u8,
|
||||
) {
|
||||
chip_select.bsrr().write(|write| write.br2().set_bit());
|
||||
let _ = spi_transfer(spi, register);
|
||||
let _ = spi_transfer(spi, value);
|
||||
while spi.sr().read().bsy().is_busy() {}
|
||||
chip_select.bsrr().write(|write| write.bs2().set_bit());
|
||||
}
|
||||
|
||||
fn read_mpu6000_sample(
|
||||
spi: &stm32f7::stm32f722::SPI1,
|
||||
chip_select: &stm32f7::stm32f722::GPIOB,
|
||||
) -> RawImuSample {
|
||||
let mut bytes = [0; 14];
|
||||
|
||||
chip_select.bsrr().write(|write| write.br2().set_bit());
|
||||
let _ = spi_transfer(spi, 0xBB);
|
||||
for byte in &mut bytes {
|
||||
*byte = spi_transfer(spi, 0);
|
||||
}
|
||||
while spi.sr().read().bsy().is_busy() {}
|
||||
chip_select.bsrr().write(|write| write.bs2().set_bit());
|
||||
|
||||
RawImuSample {
|
||||
acceleration: [
|
||||
i16::from_be_bytes([bytes[0], bytes[1]]),
|
||||
i16::from_be_bytes([bytes[2], bytes[3]]),
|
||||
i16::from_be_bytes([bytes[4], bytes[5]]),
|
||||
],
|
||||
temperature: i16::from_be_bytes([bytes[6], bytes[7]]),
|
||||
angular_rate: [
|
||||
i16::from_be_bytes([bytes[8], bytes[9]]),
|
||||
i16::from_be_bytes([bytes[10], bytes[11]]),
|
||||
i16::from_be_bytes([bytes[12], bytes[13]]),
|
||||
],
|
||||
}
|
||||
}
|
||||
|
||||
impl fmt::Write for Uart1 {
|
||||
fn write_str(&mut self, string: &str) -> fmt::Result {
|
||||
for byte in string.bytes() {
|
||||
if byte == b'\n' {
|
||||
self.write_byte(b'\r');
|
||||
.imu
|
||||
.on_transfer_complete(SpiCompletion::Complete)
|
||||
{
|
||||
ImuEvent::Sample(sample) => {
|
||||
SAMPLES.publish(sample);
|
||||
schedule_telemetry(TELEMETRY_EVENT_SAMPLE);
|
||||
}
|
||||
self.write_byte(byte);
|
||||
ImuEvent::Fault(_) => panic!("MPU transfer failed"),
|
||||
ImuEvent::Initialized | ImuEvent::Idle => {}
|
||||
}
|
||||
}
|
||||
if events & IMU_EVENT_TIMER != 0
|
||||
&& matches!(context.local.imu.on_periodic_tick(), ImuEvent::Fault(_))
|
||||
{
|
||||
panic!("MPU initialization failed");
|
||||
}
|
||||
if events & IMU_EVENT_DRDY != 0
|
||||
&& matches!(context.local.imu.on_data_ready(), ImuEvent::Fault(_))
|
||||
{
|
||||
panic!("MPU sample submission failed");
|
||||
}
|
||||
IMU_QUEUED.store(false, Ordering::Release);
|
||||
if IMU_EVENTS.load(Ordering::Acquire) != 0 && !IMU_QUEUED.swap(true, Ordering::AcqRel) {
|
||||
if imu_worker::spawn().is_err() {
|
||||
panic!("IMU worker reschedule failed");
|
||||
}
|
||||
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
impl Uart1 {
|
||||
fn write_byte(&mut self, byte: u8) {
|
||||
while self.0.isr().read().txe().bit_is_clear() {}
|
||||
self.0.tdr().write(|write| {
|
||||
// SAFETY: `byte` fits in USART1's 9-bit data field.
|
||||
unsafe { write.tdr().bits(byte.into()) }
|
||||
});
|
||||
#[task(priority = 9, local = [telemetry, led, heartbeat_divider, observed_sample])]
|
||||
async fn telemetry_worker(context: telemetry_worker::Context) {
|
||||
let events = TELEMETRY_EVENTS.swap(0, Ordering::AcqRel);
|
||||
if events & TELEMETRY_EVENT_DMA != 0 {
|
||||
let completion = match UART_COMPLETION.swap(0, Ordering::AcqRel) {
|
||||
1 => TxCompletion::Complete,
|
||||
2 => TxCompletion::TransferFault,
|
||||
_ => TxCompletion::NotPending,
|
||||
};
|
||||
if context.local.telemetry.on_dma_complete(completion).is_err() {
|
||||
panic!("telemetry DMA failed");
|
||||
}
|
||||
}
|
||||
if events & TELEMETRY_EVENT_SAMPLE != 0 {
|
||||
if let Some(sample) = SAMPLES.take_newer(context.local.observed_sample) {
|
||||
if context.local.telemetry.publish_sample(&sample).is_err() {
|
||||
panic!("RAW_IMU publication failed");
|
||||
}
|
||||
}
|
||||
}
|
||||
if events & TELEMETRY_EVENT_HEARTBEAT != 0 {
|
||||
*context.local.heartbeat_divider = context.local.heartbeat_divider.wrapping_add(1);
|
||||
if *context.local.heartbeat_divider == 10 {
|
||||
*context.local.heartbeat_divider = 0;
|
||||
if context.local.telemetry.publish_heartbeat().is_err() {
|
||||
panic!("heartbeat publication failed");
|
||||
}
|
||||
context.local.led.toggle();
|
||||
}
|
||||
}
|
||||
TELEMETRY_QUEUED.store(false, Ordering::Release);
|
||||
if TELEMETRY_EVENTS.load(Ordering::Acquire) != 0
|
||||
&& !TELEMETRY_QUEUED.swap(true, Ordering::AcqRel)
|
||||
{
|
||||
if telemetry_worker::spawn().is_err() {
|
||||
panic!("telemetry worker reschedule failed");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn schedule_imu(event: u8) {
|
||||
IMU_EVENTS.fetch_or(event, Ordering::Release);
|
||||
if !IMU_QUEUED.swap(true, Ordering::AcqRel) && imu_worker::spawn().is_err() {
|
||||
panic!("IMU worker schedule failed");
|
||||
}
|
||||
}
|
||||
|
||||
fn schedule_telemetry(event: u8) {
|
||||
TELEMETRY_EVENTS.fetch_or(event, Ordering::Release);
|
||||
if !TELEMETRY_QUEUED.swap(true, Ordering::AcqRel) && telemetry_worker::spawn().is_err() {
|
||||
panic!("telemetry worker schedule failed");
|
||||
}
|
||||
}
|
||||
|
||||
fn configure_exti4(rcc: &stm32f722::RCC, syscfg: stm32f722::SYSCFG, exti: stm32f722::EXTI) {
|
||||
rcc.apb2enr().modify(|_, write| write.syscfgen().set_bit());
|
||||
syscfg
|
||||
.exticr2()
|
||||
.modify(|_, write| unsafe { write.exti4().bits(0) });
|
||||
exti.imr().modify(|_, write| write.im4().set_bit());
|
||||
exti.rtsr().modify(|_, write| write.tr4().set_bit());
|
||||
exti.ftsr().modify(|_, write| write.tr4().clear_bit());
|
||||
exti.pr().write(|write| write.pr4().clear());
|
||||
}
|
||||
|
||||
fn safe_idle() -> ! {
|
||||
loop {
|
||||
cortex_m::asm::wfi();
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user