311 lines
13 KiB
Rust
311 lines
13 KiB
Rust
//! The one platform-owned RTIC task graph for all supported F722 profiles.
|
|
|
|
/// Defines the F722 application in the platform crate. All interrupt bindings
|
|
/// are the finite platform-owned superset. Profile activation determines which
|
|
/// acknowledged route has initialized hardware.
|
|
#[doc(hidden)]
|
|
#[macro_export]
|
|
macro_rules! __f722_flight_application {
|
|
($board:ty) => {
|
|
#[allow(unused_imports)]
|
|
use $crate::__platform_internal::cmrt as _;
|
|
#[allow(unused_imports)]
|
|
use $crate::__platform_internal::panic as _;
|
|
|
|
// RTIC 2.3 expands hardware-vector shims that call unsafe PAC token
|
|
// constructors inside generated unsafe functions. Those shims are
|
|
// generated by RTIC, not hand-written platform accesses; all platform
|
|
// unsafe register operations remain documented at their source.
|
|
#[allow(unsafe_op_in_unsafe_fn)]
|
|
#[$crate::__platform_internal::rtic_app::app(
|
|
device = $crate::__platform_internal::pac::stm32f722,
|
|
peripherals = true,
|
|
dispatchers = [TIM6_DAC, TIM7]
|
|
)]
|
|
mod kaptan_f722_platform_app {
|
|
use core::sync::atomic::{AtomicBool, AtomicU8, Ordering};
|
|
|
|
use $crate::f722::{
|
|
F722Imu, F722ImuTransferCompletion, F722InitResources, F722LidarState,
|
|
F722MavlinkState, F722Mtf01, F722Mtf01Uart, F722ServiceTimer,
|
|
acknowledge_exti3, acknowledge_exti4,
|
|
acknowledge_imu_rx_dma1, acknowledge_imu_rx_dma2, acknowledge_imu_spi1_error,
|
|
acknowledge_imu_spi3_error, acknowledge_imu_tx_dma1, acknowledge_imu_tx_dma2,
|
|
acknowledge_mavlink_dma1, acknowledge_mavlink_dma2, acknowledge_mavlink_rx_usart1,
|
|
acknowledge_mavlink_rx_usart2, initialize, on_uart4_interrupt, process_imu,
|
|
record_mavlink_completion,
|
|
};
|
|
#[allow(unused_imports)]
|
|
use $crate::__platform_internal::rtic_app::Mutex;
|
|
|
|
type BoardUserConfig = <$board as $crate::F7Board>::UserConfig;
|
|
type BoardImmutableConfig = <$board as $crate::F7Board>::ImmutableConfig;
|
|
const CONFIG_PAYLOAD_BYTES: usize = $crate::config_snapshot_length::<
|
|
BoardUserConfig,
|
|
>();
|
|
type BoardInitResources = F722InitResources<
|
|
BoardUserConfig,
|
|
BoardImmutableConfig,
|
|
CONFIG_PAYLOAD_BYTES,
|
|
>;
|
|
type BoardMavlinkState = F722MavlinkState<
|
|
BoardUserConfig,
|
|
BoardImmutableConfig,
|
|
CONFIG_PAYLOAD_BYTES,
|
|
>;
|
|
|
|
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 IMU_EVENT_START: u8 = 1 << 4;
|
|
const MAVLINK_EVENT_TIMER: u8 = 1 << 0;
|
|
const MAVLINK_EVENT_RECEIVED: u8 = 1 << 1;
|
|
const MAVLINK_EVENT_DMA: u8 = 1 << 2;
|
|
|
|
// Hardware IRQs and both software workers coalesce event bits.
|
|
// Release/Acquire pairs make each terminal acknowledgement visible
|
|
// to the worker that consumes it; the queued bit prevents unbounded
|
|
// software-task backlog while preserving the latest pending work.
|
|
static IMU_EVENTS: AtomicU8 = AtomicU8::new(0);
|
|
static MAVLINK_EVENTS: AtomicU8 = AtomicU8::new(0);
|
|
static IMU_QUEUED: AtomicBool = AtomicBool::new(false);
|
|
static MAVLINK_QUEUED: AtomicBool = AtomicBool::new(false);
|
|
|
|
#[shared]
|
|
struct Shared {
|
|
mavlink: BoardMavlinkState,
|
|
lidar: F722LidarState,
|
|
}
|
|
|
|
#[local]
|
|
struct Local {
|
|
imu: F722Imu,
|
|
mtf01: Option<F722Mtf01>,
|
|
_mtf01_uart: Option<F722Mtf01Uart>,
|
|
timer: F722ServiceTimer,
|
|
}
|
|
|
|
#[init]
|
|
fn init(context: init::Context) -> (Shared, Local) {
|
|
let BoardInitResources {
|
|
imu,
|
|
mtf01,
|
|
mtf01_uart,
|
|
timer,
|
|
lidar,
|
|
mavlink,
|
|
} = initialize::<BoardUserConfig, BoardImmutableConfig, CONFIG_PAYLOAD_BYTES>(
|
|
context.core,
|
|
context.device,
|
|
<$board as $crate::F7Board>::FACTORY_USER_CONFIG,
|
|
<$board as $crate::F7Board>::IMMUTABLE_CONFIG,
|
|
)
|
|
.unwrap_or_else(|_| safe_idle());
|
|
$crate::__platform_internal::logging::info!("flight controller initialized");
|
|
schedule_imu(IMU_EVENT_START);
|
|
(
|
|
Shared {
|
|
mavlink,
|
|
lidar,
|
|
},
|
|
Local {
|
|
imu,
|
|
mtf01,
|
|
_mtf01_uart: mtf01_uart,
|
|
timer,
|
|
},
|
|
)
|
|
}
|
|
|
|
#[idle]
|
|
fn idle(_: idle::Context) -> ! {
|
|
loop {
|
|
$crate::__platform_internal::cm::asm::wfi();
|
|
}
|
|
}
|
|
|
|
#[task(binds = EXTI4, priority = 15)]
|
|
fn exti4(_: exti4::Context) {
|
|
if acknowledge_exti4() {
|
|
schedule_imu(IMU_EVENT_DRDY);
|
|
}
|
|
}
|
|
|
|
#[task(binds = EXTI3, priority = 15)]
|
|
fn exti3(_: exti3::Context) {
|
|
if acknowledge_exti3() {
|
|
schedule_imu(IMU_EVENT_DRDY);
|
|
}
|
|
}
|
|
|
|
#[task(binds = DMA2_STREAM0, priority = 14)]
|
|
fn imu_rx_dma2(_: imu_rx_dma2::Context) {
|
|
match acknowledge_imu_rx_dma2() {
|
|
F722ImuTransferCompletion::Complete => schedule_imu(IMU_EVENT_TRANSFER_COMPLETE),
|
|
F722ImuTransferCompletion::TransferFault => schedule_imu(IMU_EVENT_TRANSFER_FAULT),
|
|
F722ImuTransferCompletion::NotPending => {}
|
|
}
|
|
}
|
|
|
|
#[task(binds = DMA2_STREAM3, priority = 14)]
|
|
fn imu_tx_dma2(_: imu_tx_dma2::Context) {
|
|
if acknowledge_imu_tx_dma2() == F722ImuTransferCompletion::TransferFault {
|
|
schedule_imu(IMU_EVENT_TRANSFER_FAULT);
|
|
}
|
|
}
|
|
|
|
#[task(binds = SPI1, priority = 14)]
|
|
fn imu_spi1_error(_: imu_spi1_error::Context) {
|
|
if acknowledge_imu_spi1_error() {
|
|
schedule_imu(IMU_EVENT_TRANSFER_FAULT);
|
|
}
|
|
}
|
|
|
|
#[task(binds = DMA1_STREAM0, priority = 14)]
|
|
fn imu_rx_dma1(_: imu_rx_dma1::Context) {
|
|
match acknowledge_imu_rx_dma1() {
|
|
F722ImuTransferCompletion::Complete => schedule_imu(IMU_EVENT_TRANSFER_COMPLETE),
|
|
F722ImuTransferCompletion::TransferFault => schedule_imu(IMU_EVENT_TRANSFER_FAULT),
|
|
F722ImuTransferCompletion::NotPending => {}
|
|
}
|
|
}
|
|
|
|
#[task(binds = DMA1_STREAM5, priority = 14)]
|
|
fn imu_tx_dma1(_: imu_tx_dma1::Context) {
|
|
if acknowledge_imu_tx_dma1() == F722ImuTransferCompletion::TransferFault {
|
|
schedule_imu(IMU_EVENT_TRANSFER_FAULT);
|
|
}
|
|
}
|
|
|
|
#[task(binds = SPI3, priority = 14)]
|
|
fn imu_spi3_error(_: imu_spi3_error::Context) {
|
|
if acknowledge_imu_spi3_error() {
|
|
schedule_imu(IMU_EVENT_TRANSFER_FAULT);
|
|
}
|
|
}
|
|
|
|
#[task(binds = UART4, priority = 14, local = [mtf01])]
|
|
fn uart4(context: uart4::Context) {
|
|
on_uart4_interrupt(context.local.mtf01);
|
|
}
|
|
|
|
#[task(binds = I2C1_EV, priority = 11, shared = [lidar])]
|
|
fn i2c1_event(mut context: i2c1_event::Context) {
|
|
context
|
|
.shared
|
|
.lidar
|
|
.lock(|lidar| lidar.on_i2c_event(false));
|
|
}
|
|
|
|
#[task(binds = I2C1_ER, priority = 11, shared = [lidar])]
|
|
fn i2c1_error(mut context: i2c1_error::Context) {
|
|
context
|
|
.shared
|
|
.lidar
|
|
.lock(|lidar| lidar.on_i2c_event(true));
|
|
}
|
|
|
|
#[task(binds = DMA2_STREAM7, priority = 10)]
|
|
fn mavlink_dma2(_: mavlink_dma2::Context) {
|
|
let completion = acknowledge_mavlink_dma2();
|
|
if completion != $crate::__platform_internal::dma::TxCompletion::NotPending {
|
|
record_mavlink_completion(completion);
|
|
schedule_mavlink(MAVLINK_EVENT_DMA);
|
|
}
|
|
}
|
|
|
|
#[task(binds = DMA1_STREAM6, priority = 10)]
|
|
fn mavlink_dma1(_: mavlink_dma1::Context) {
|
|
let completion = acknowledge_mavlink_dma1();
|
|
if completion != $crate::__platform_internal::dma::TxCompletion::NotPending {
|
|
record_mavlink_completion(completion);
|
|
schedule_mavlink(MAVLINK_EVENT_DMA);
|
|
}
|
|
}
|
|
|
|
#[task(binds = USART1, priority = 8, shared = [mavlink])]
|
|
fn mavlink_rx_usart1(mut context: mavlink_rx_usart1::Context) {
|
|
let event = acknowledge_mavlink_rx_usart1();
|
|
if context
|
|
.shared
|
|
.mavlink
|
|
.lock(|mavlink| mavlink.receive_mavlink_interrupt(event))
|
|
{
|
|
schedule_mavlink(MAVLINK_EVENT_RECEIVED);
|
|
}
|
|
}
|
|
|
|
#[task(binds = USART2, priority = 8, shared = [mavlink])]
|
|
fn mavlink_rx_usart2(mut context: mavlink_rx_usart2::Context) {
|
|
let event = acknowledge_mavlink_rx_usart2();
|
|
if context
|
|
.shared
|
|
.mavlink
|
|
.lock(|mavlink| mavlink.receive_mavlink_interrupt(event))
|
|
{
|
|
schedule_mavlink(MAVLINK_EVENT_RECEIVED);
|
|
}
|
|
}
|
|
|
|
#[task(binds = TIM2, priority = 4, local = [timer], shared = [lidar])]
|
|
fn tim2(mut context: tim2::Context) {
|
|
context.local.timer.acknowledge_update();
|
|
let _ = context.shared.lidar.lock(F722LidarState::on_timer);
|
|
schedule_imu(IMU_EVENT_TIMER);
|
|
schedule_mavlink(MAVLINK_EVENT_TIMER);
|
|
}
|
|
|
|
#[task(priority = 13, local = [imu])]
|
|
async fn imu_worker(context: imu_worker::Context) {
|
|
let events = IMU_EVENTS.swap(0, Ordering::AcqRel);
|
|
process_imu(context.local.imu, events);
|
|
IMU_QUEUED.store(false, Ordering::Release);
|
|
if IMU_EVENTS.load(Ordering::Acquire) != 0
|
|
&& !IMU_QUEUED.swap(true, Ordering::AcqRel)
|
|
&& imu_worker::spawn().is_err()
|
|
{
|
|
IMU_QUEUED.store(false, Ordering::Release);
|
|
}
|
|
}
|
|
|
|
#[task(priority = 9, shared = [mavlink])]
|
|
async fn mavlink_worker(mut context: mavlink_worker::Context) {
|
|
let events = MAVLINK_EVENTS.swap(0, Ordering::AcqRel);
|
|
context
|
|
.shared
|
|
.mavlink
|
|
.lock(|mavlink| mavlink.process_worker_events(events));
|
|
MAVLINK_QUEUED.store(false, Ordering::Release);
|
|
if MAVLINK_EVENTS.load(Ordering::Acquire) != 0
|
|
&& !MAVLINK_QUEUED.swap(true, Ordering::AcqRel)
|
|
&& mavlink_worker::spawn().is_err()
|
|
{
|
|
MAVLINK_QUEUED.store(false, Ordering::Release);
|
|
}
|
|
}
|
|
|
|
fn schedule_imu(event: u8) {
|
|
IMU_EVENTS.fetch_or(event, Ordering::Release);
|
|
if !IMU_QUEUED.swap(true, Ordering::AcqRel) && imu_worker::spawn().is_err() {
|
|
IMU_QUEUED.store(false, Ordering::Release);
|
|
}
|
|
}
|
|
|
|
fn schedule_mavlink(event: u8) {
|
|
MAVLINK_EVENTS.fetch_or(event, Ordering::Release);
|
|
if !MAVLINK_QUEUED.swap(true, Ordering::AcqRel) && mavlink_worker::spawn().is_err()
|
|
{
|
|
MAVLINK_QUEUED.store(false, Ordering::Release);
|
|
}
|
|
}
|
|
|
|
fn safe_idle() -> ! {
|
|
loop {
|
|
$crate::__platform_internal::cm::asm::wfi();
|
|
}
|
|
}
|
|
}
|
|
};
|
|
}
|