platform: inhibit motor pins and remove output routes
This commit is contained in:
1 parent
5cb4b7df68
commit
b0d22b61d8
19 files changed
+426
-1126
No files matched your search
@@ -9,11 +9,16 @@ version.workspace = true
|
|||||||
[lints]
|
[lints]
|
||||||
workspace = true
|
workspace = true
|
||||||
|
|
||||||
|
[lib]
|
||||||
|
# This is a bare-metal `no_std` platform library with its own panic handler.
|
||||||
|
# Host-verifiable invariants live in `tests/platform_safety_boundary.rs`; a
|
||||||
|
# generated lib-test harness would link `std` and a second panic handler.
|
||||||
|
test = false
|
||||||
|
|
||||||
[features]
|
[features]
|
||||||
default = []
|
default = []
|
||||||
|
|
||||||
[dependencies]
|
[dependencies]
|
||||||
kaptan-actuator = { path = "../../libs/kaptan-actuator" }
|
|
||||||
kaptan-board = { path = "../../libs/kaptan-board" }
|
kaptan-board = { path = "../../libs/kaptan-board" }
|
||||||
kaptan-data-channel = { path = "../../libs/kaptan-data-channel" }
|
kaptan-data-channel = { path = "../../libs/kaptan-data-channel" }
|
||||||
kaptan-dma = { path = "../../libs/kaptan-dma" }
|
kaptan-dma = { path = "../../libs/kaptan-dma" }
|
||||||
|
|||||||
@@ -1,133 +0,0 @@
|
|||||||
mod tim3_dshot;
|
|
||||||
mod tim3_pwm;
|
|
||||||
|
|
||||||
use kaptan_actuator::{MotorOutput, MotorProtocol};
|
|
||||||
use stm32f7::stm32f722;
|
|
||||||
|
|
||||||
use self::{tim3_dshot::Tim3Dshot, tim3_pwm::Tim3Pwm};
|
|
||||||
use crate::f722::MotorRoute;
|
|
||||||
|
|
||||||
/// STM32F722 TIM3/TIM5 protocol-neutral M1–M4 output route.
|
|
||||||
///
|
|
||||||
/// TIM3 channels 1–4 use PB4, PB5, PB0, and PB1 respectively. Kakute's
|
|
||||||
/// M1–M4 connector order is PB0, PB1, PB4, PB5, while GEPRC's is PB4, PB5,
|
|
||||||
/// PB0, PB1. The route descriptor therefore supplies the logical-to-channel
|
|
||||||
/// map; no board inherits another board's motor numbering.
|
|
||||||
/// Opaque owner of the F722 motor peripherals used by the RTIC application.
|
|
||||||
///
|
|
||||||
/// The type is public only because RTIC exposes resource metadata in generated
|
|
||||||
/// public items. Its fields and construction remain platform-private.
|
|
||||||
pub struct F722MotorOutput {
|
|
||||||
transport: Transport,
|
|
||||||
}
|
|
||||||
|
|
||||||
enum Transport {
|
|
||||||
Pwm(Tim3Pwm),
|
|
||||||
Dshot(Tim3Dshot),
|
|
||||||
}
|
|
||||||
|
|
||||||
impl F722MotorOutput {
|
|
||||||
pub(crate) fn new(
|
|
||||||
rcc: &stm32f722::RCC,
|
|
||||||
tim3: stm32f722::TIM3,
|
|
||||||
tim5: stm32f722::TIM5,
|
|
||||||
protocol: MotorProtocol,
|
|
||||||
route: MotorRoute,
|
|
||||||
) -> Self {
|
|
||||||
let logical_to_tim3_channel = logical_to_tim3_channel(route);
|
|
||||||
let transport = match protocol {
|
|
||||||
MotorProtocol::Pwm(config) => {
|
|
||||||
let _ = tim5;
|
|
||||||
Transport::Pwm(Tim3Pwm::new(rcc, tim3, config))
|
|
||||||
}
|
|
||||||
MotorProtocol::Dshot(config) => Transport::Dshot(Tim3Dshot::new(
|
|
||||||
rcc,
|
|
||||||
tim3,
|
|
||||||
tim5,
|
|
||||||
config,
|
|
||||||
logical_to_tim3_channel,
|
|
||||||
)),
|
|
||||||
};
|
|
||||||
Self { transport }
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Services TIM3 while RTIC holds this motor owner's resource lock.
|
|
||||||
pub(crate) fn on_tim3_interrupt(&mut self) {
|
|
||||||
if matches!(self.transport, Transport::Pwm(_)) {
|
|
||||||
Tim3Pwm::on_tim3_update();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Services TIM5 while RTIC holds this motor owner's resource lock.
|
|
||||||
pub(crate) fn on_tim5_interrupt(&mut self) {
|
|
||||||
if matches!(self.transport, Transport::Dshot(_)) {
|
|
||||||
Tim3Dshot::on_tim5_update();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Begins the configured arming sequence.
|
|
||||||
pub(crate) fn arm(&mut self) {
|
|
||||||
<Self as MotorOutput>::arm(self);
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Immediately returns the outputs to their disarmed state.
|
|
||||||
pub(crate) fn disarm(&mut self) {
|
|
||||||
<Self as MotorOutput>::disarm(self);
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Returns the arming state of this uniquely owned output route.
|
|
||||||
pub(crate) fn is_armed(&self) -> bool {
|
|
||||||
<Self as MotorOutput>::is_armed(self)
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Maps logical M1–M4 indices to zero-based TIM3 channel indices.
|
|
||||||
fn logical_to_tim3_channel(route: MotorRoute) -> [usize; 4] {
|
|
||||||
match route {
|
|
||||||
// Kakute: M1=PB0/CH3, M2=PB1/CH4, M3=PB4/CH1, M4=PB5/CH2.
|
|
||||||
MotorRoute::KakuteTim3Tim5 => [2, 3, 0, 1],
|
|
||||||
// GEPRC: M1=PB4/CH1, M2=PB5/CH2, M3=PB0/CH3, M4=PB1/CH4.
|
|
||||||
MotorRoute::GeprcTim3Tim5 => [0, 1, 2, 3],
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl MotorOutput for F722MotorOutput {
|
|
||||||
fn arm(&mut self) {
|
|
||||||
match &mut self.transport {
|
|
||||||
Transport::Pwm(output) => output.arm(),
|
|
||||||
Transport::Dshot(output) => output.arm(),
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
fn disarm(&mut self) {
|
|
||||||
match &mut self.transport {
|
|
||||||
Transport::Pwm(output) => output.disarm(),
|
|
||||||
Transport::Dshot(output) => output.disarm(),
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
fn is_armed(&self) -> bool {
|
|
||||||
match &self.transport {
|
|
||||||
Transport::Pwm(output) => output.is_armed(),
|
|
||||||
Transport::Dshot(output) => output.is_armed(),
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#[cfg(test)]
|
|
||||||
mod tests {
|
|
||||||
use super::logical_to_tim3_channel;
|
|
||||||
use crate::f722::MotorRoute;
|
|
||||||
|
|
||||||
#[test]
|
|
||||||
fn keeps_each_supported_board_motor_order_explicit() {
|
|
||||||
assert_eq!(
|
|
||||||
logical_to_tim3_channel(MotorRoute::KakuteTim3Tim5),
|
|
||||||
[2, 3, 0, 1]
|
|
||||||
);
|
|
||||||
assert_eq!(
|
|
||||||
logical_to_tim3_channel(MotorRoute::GeprcTim3Tim5),
|
|
||||||
[0, 1, 2, 3]
|
|
||||||
);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -1,466 +0,0 @@
|
|||||||
use core::sync::atomic::{AtomicBool, Ordering};
|
|
||||||
|
|
||||||
use kaptan_actuator::dshot::{
|
|
||||||
ArmSequence, Config, FRAME_SYMBOLS, FrameValue, Throttle, Timing, symbols,
|
|
||||||
};
|
|
||||||
use stm32f7::stm32f722;
|
|
||||||
|
|
||||||
const APB1_TIMER_CLOCK_HZ: u32 = 108_000_000;
|
|
||||||
const DSHOT_CHANNELS: usize = 4;
|
|
||||||
const DSHOT_BUFFER_LENGTH: usize = FRAME_SYMBOLS * DSHOT_CHANNELS;
|
|
||||||
const TIM3_CHANNEL_ENABLE: u32 = 0x1111;
|
|
||||||
const PWM_MODE_1_WITH_PRELOAD: u32 = 0x6868;
|
|
||||||
const TIM3_DMAR_CCR1_OFFSET: u8 = 13;
|
|
||||||
const DEFAULT_ARMED_IDLE_THROTTLE: Throttle = match Throttle::new(48) {
|
|
||||||
Some(throttle) => throttle,
|
|
||||||
None => panic!("valid DShot throttle"),
|
|
||||||
};
|
|
||||||
|
|
||||||
// These static buffers live in DMA-visible SRAM. They are initialized before
|
|
||||||
// DMA is enabled and are subsequently read-only to the DMA controller.
|
|
||||||
static mut DSHOT_ZERO_FRAME: [u32; DSHOT_BUFFER_LENGTH] = [0; DSHOT_BUFFER_LENGTH];
|
|
||||||
static mut DSHOT_ARMED_IDLE_FRAME: [u32; DSHOT_BUFFER_LENGTH] = [0; DSHOT_BUFFER_LENGTH];
|
|
||||||
static ARM_SEQUENCE: ArmSequence = ArmSequence::new(DEFAULT_ARMED_IDLE_THROTTLE, 0);
|
|
||||||
static DMA_RUNNING: AtomicBool = AtomicBool::new(false);
|
|
||||||
|
|
||||||
pub(super) struct Tim3Dshot {
|
|
||||||
timer: stm32f722::TIM3,
|
|
||||||
dma_available: bool,
|
|
||||||
}
|
|
||||||
|
|
||||||
impl Tim3Dshot {
|
|
||||||
pub(super) fn new(
|
|
||||||
rcc: &stm32f722::RCC,
|
|
||||||
timer: stm32f722::TIM3,
|
|
||||||
scheduler: stm32f722::TIM5,
|
|
||||||
config: Config,
|
|
||||||
logical_to_tim3_channel: [usize; DSHOT_CHANNELS],
|
|
||||||
) -> Self {
|
|
||||||
let rate = config.rate();
|
|
||||||
let dshot_timer_hz = rate.timer_base_hz();
|
|
||||||
let timing = match Timing::from_timer_clock(dshot_timer_hz, rate) {
|
|
||||||
Some(timing) => timing,
|
|
||||||
None => unreachable!(),
|
|
||||||
};
|
|
||||||
let timer_prescaler = APB1_TIMER_CLOCK_HZ / dshot_timer_hz;
|
|
||||||
assert!(APB1_TIMER_CLOCK_HZ.is_multiple_of(dshot_timer_hz));
|
|
||||||
ARM_SEQUENCE
|
|
||||||
.configure_zero_frames(rate.frames_for_millis(config.arming_duration().milliseconds()));
|
|
||||||
ARM_SEQUENCE.configure_armed_throttle(config.armed_idle_throttle());
|
|
||||||
|
|
||||||
rcc.ahb1enr().modify(|_, write| write.gpioben().set_bit());
|
|
||||||
rcc.ahb1enr().modify(|_, write| write.dma1en().set_bit());
|
|
||||||
rcc.apb1rstr().modify(|_, write| write.tim3rst().set_bit());
|
|
||||||
rcc.apb1rstr()
|
|
||||||
.modify(|_, write| write.tim3rst().clear_bit());
|
|
||||||
rcc.apb1enr().modify(|_, write| write.tim3en().set_bit());
|
|
||||||
|
|
||||||
timer
|
|
||||||
.cr1()
|
|
||||||
.modify(|_, write| write.cen().clear_bit().arpe().enabled());
|
|
||||||
timer.dier().modify(|_, write| write.ude().disabled());
|
|
||||||
timer.ccer().write(|write| {
|
|
||||||
// SAFETY: zero is the documented all-channels-disabled CCER encoding while TIM3 is stopped.
|
|
||||||
unsafe { write.bits(0) }
|
|
||||||
});
|
|
||||||
timer.psc().write(|write| {
|
|
||||||
// SAFETY: `timer_prescaler` divides the fixed APB1 timer clock and fits the PSC field.
|
|
||||||
unsafe { write.psc().bits((timer_prescaler - 1) as u16) }
|
|
||||||
});
|
|
||||||
timer.arr().write(|write| {
|
|
||||||
// SAFETY: `Timing` validates a non-zero ARR value representable by TIM3.
|
|
||||||
unsafe { write.bits(u32::from(timing.bit_ticks() - 1)) }
|
|
||||||
});
|
|
||||||
timer.ccmr1_output().write(|write| {
|
|
||||||
// SAFETY: this constant selects documented PWM mode 1 with preload for channels 1 and 2.
|
|
||||||
unsafe { write.bits(PWM_MODE_1_WITH_PRELOAD) }
|
|
||||||
});
|
|
||||||
timer.ccmr2_output().write(|write| {
|
|
||||||
// SAFETY: this constant selects documented PWM mode 1 with preload for channels 3 and 4.
|
|
||||||
unsafe { write.bits(PWM_MODE_1_WITH_PRELOAD) }
|
|
||||||
});
|
|
||||||
timer.dcr().write(|write| {
|
|
||||||
// SAFETY: the DMA burst starts at CCR1 and covers exactly four contiguous compare registers.
|
|
||||||
unsafe {
|
|
||||||
write
|
|
||||||
.dba()
|
|
||||||
.bits(TIM3_DMAR_CCR1_OFFSET)
|
|
||||||
.dbl()
|
|
||||||
.bits((DSHOT_CHANNELS - 1) as u8)
|
|
||||||
}
|
|
||||||
});
|
|
||||||
timer.egr().write(|write| write.ug().set_bit());
|
|
||||||
let dma_available = disable_dma_stream();
|
|
||||||
initialize_dma_frames(
|
|
||||||
timing,
|
|
||||||
config.armed_idle_throttle(),
|
|
||||||
logical_to_tim3_channel,
|
|
||||||
);
|
|
||||||
|
|
||||||
rcc.apb1rstr().modify(|_, write| write.tim5rst().set_bit());
|
|
||||||
rcc.apb1rstr()
|
|
||||||
.modify(|_, write| write.tim5rst().clear_bit());
|
|
||||||
rcc.apb1enr().modify(|_, write| write.tim5en().set_bit());
|
|
||||||
scheduler.psc().write(|write| {
|
|
||||||
// SAFETY: zero is a valid TIM5 prescaler and keeps the scheduler at the APB1 timer clock.
|
|
||||||
unsafe { write.psc().bits(0) }
|
|
||||||
});
|
|
||||||
scheduler.arr().write(|write| {
|
|
||||||
// SAFETY: each supported DShot rate divides APB1_TIMER_CLOCK_HZ, yielding a valid ARR value.
|
|
||||||
unsafe { write.bits(APB1_TIMER_CLOCK_HZ / rate.output_rate_hz() - 1) }
|
|
||||||
});
|
|
||||||
scheduler.egr().write(|write| write.ug().set_bit());
|
|
||||||
scheduler.sr().modify(|_, write| write.uif().clear());
|
|
||||||
scheduler.dier().write(|write| write.uie().disabled());
|
|
||||||
scheduler.cr1().write(|write| write.cen().clear_bit());
|
|
||||||
configure_gpio_low();
|
|
||||||
|
|
||||||
Self {
|
|
||||||
timer,
|
|
||||||
dma_available,
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
pub(super) fn arm(&mut self) {
|
|
||||||
if ARM_SEQUENCE.is_armed() {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
if !self.dma_available {
|
|
||||||
self.dma_available = disable_dma_stream();
|
|
||||||
if !self.dma_available {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
ARM_SEQUENCE.arm();
|
|
||||||
if !self.start_stream(zero_frame()) {
|
|
||||||
ARM_SEQUENCE.disarm();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
// SAFETY: TIM5 was uniquely taken during initialization and remains owned by this
|
|
||||||
// platform resource; RTIC serializes this method with `on_tim5_update`.
|
|
||||||
let scheduler = unsafe { &*stm32f722::TIM5::ptr() };
|
|
||||||
scheduler.sr().modify(|_, write| write.uif().clear());
|
|
||||||
scheduler.dier().modify(|_, write| write.uie().enabled());
|
|
||||||
scheduler.cr1().modify(|_, write| write.cen().set_bit());
|
|
||||||
}
|
|
||||||
|
|
||||||
pub(super) fn disarm(&mut self) {
|
|
||||||
ARM_SEQUENCE.disarm();
|
|
||||||
// SAFETY: TIM5 is owned by this motor resource and cannot be accessed concurrently
|
|
||||||
// because RTIC locks that resource for both the service IRQ and this method.
|
|
||||||
let scheduler = unsafe { &*stm32f722::TIM5::ptr() };
|
|
||||||
scheduler.cr1().modify(|_, write| write.cen().clear_bit());
|
|
||||||
scheduler.dier().modify(|_, write| write.uie().disabled());
|
|
||||||
self.stop_stream();
|
|
||||||
self.timer.ccer().write(|write| {
|
|
||||||
// SAFETY: zero disables all TIM3 channels after its stream and counter have stopped.
|
|
||||||
unsafe { write.bits(0) }
|
|
||||||
});
|
|
||||||
configure_gpio_low();
|
|
||||||
}
|
|
||||||
|
|
||||||
pub(super) fn is_armed(&self) -> bool {
|
|
||||||
ARM_SEQUENCE.is_armed()
|
|
||||||
}
|
|
||||||
|
|
||||||
fn start_stream(&mut self, frame: *const u32) -> bool {
|
|
||||||
self.timer.cr1().modify(|_, write| write.cen().clear_bit());
|
|
||||||
DMA_RUNNING.store(false, Ordering::Release);
|
|
||||||
if !configure_dma_stream(frame) {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
self.timer.ccer().write(|write| {
|
|
||||||
// SAFETY: this enables exactly TIM3 channels 1 through 4 in output mode.
|
|
||||||
unsafe { write.bits(TIM3_CHANNEL_ENABLE) }
|
|
||||||
});
|
|
||||||
configure_gpio_alternate_function();
|
|
||||||
self.timer.egr().write(|write| write.ug().set_bit());
|
|
||||||
DMA_RUNNING.store(true, Ordering::Release);
|
|
||||||
dma_stream().cr().modify(|_, write| write.en().enabled());
|
|
||||||
self.timer.dier().modify(|_, write| write.ude().enabled());
|
|
||||||
self.timer.cr1().modify(|_, write| write.cen().set_bit());
|
|
||||||
true
|
|
||||||
}
|
|
||||||
|
|
||||||
fn stop_stream(&mut self) {
|
|
||||||
self.timer.cr1().modify(|_, write| write.cen().clear_bit());
|
|
||||||
DMA_RUNNING.store(false, Ordering::Release);
|
|
||||||
self.timer.dier().modify(|_, write| write.ude().disabled());
|
|
||||||
let _ = disable_dma_stream();
|
|
||||||
}
|
|
||||||
|
|
||||||
pub(super) fn on_tim5_update() {
|
|
||||||
// SAFETY: TIM5 is owned by the motor resource; its RTIC lock excludes arm/disarm while
|
|
||||||
// the update handler clears its flag.
|
|
||||||
let scheduler = unsafe { &*stm32f722::TIM5::ptr() };
|
|
||||||
scheduler.sr().modify(|_, write| write.uif().clear());
|
|
||||||
// SAFETY: TIM3 is uniquely owned by the same platform resource and not otherwise stolen.
|
|
||||||
let timer = unsafe { &*stm32f722::TIM3::ptr() };
|
|
||||||
timer.dier().modify(|_, write| write.ude().disabled());
|
|
||||||
if !disable_dma_stream() {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
if !DMA_RUNNING.load(Ordering::Acquire) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
let frame = match ARM_SEQUENCE.next_frame() {
|
|
||||||
Some(FrameValue::Stop) => zero_frame(),
|
|
||||||
Some(FrameValue::Throttle(_)) => armed_idle_frame(),
|
|
||||||
None => return,
|
|
||||||
};
|
|
||||||
dma_stream().ndtr().write(|write| {
|
|
||||||
// SAFETY: the fixed frame length is the exact initialized DMA buffer length.
|
|
||||||
unsafe { write.bits(DSHOT_BUFFER_LENGTH as u32) }
|
|
||||||
});
|
|
||||||
dma_stream().m0ar().write(|write| {
|
|
||||||
// SAFETY: `frame` is one of the static, aligned DMA-visible frame buffers.
|
|
||||||
unsafe { write.bits(frame as u32) }
|
|
||||||
});
|
|
||||||
dma_stream().cr().modify(|_, write| write.en().enabled());
|
|
||||||
timer.dier().modify(|_, write| write.ude().enabled());
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
fn initialize_dma_frames(
|
|
||||||
timing: Timing,
|
|
||||||
armed_idle: Throttle,
|
|
||||||
logical_to_tim3_channel: [usize; DSHOT_CHANNELS],
|
|
||||||
) {
|
|
||||||
write_dma_frame(
|
|
||||||
core::ptr::addr_of_mut!(DSHOT_ZERO_FRAME).cast(),
|
|
||||||
FrameValue::Stop,
|
|
||||||
timing,
|
|
||||||
logical_to_tim3_channel,
|
|
||||||
);
|
|
||||||
write_dma_frame(
|
|
||||||
core::ptr::addr_of_mut!(DSHOT_ARMED_IDLE_FRAME).cast(),
|
|
||||||
FrameValue::Throttle(armed_idle),
|
|
||||||
timing,
|
|
||||||
logical_to_tim3_channel,
|
|
||||||
);
|
|
||||||
}
|
|
||||||
|
|
||||||
fn write_dma_frame(
|
|
||||||
destination: *mut u32,
|
|
||||||
value: FrameValue,
|
|
||||||
timing: Timing,
|
|
||||||
logical_to_tim3_channel: [usize; DSHOT_CHANNELS],
|
|
||||||
) {
|
|
||||||
let symbols = symbols(
|
|
||||||
value.packet(),
|
|
||||||
timing.zero_high_ticks(),
|
|
||||||
timing.one_high_ticks(),
|
|
||||||
);
|
|
||||||
let mut bit = 0;
|
|
||||||
while bit < FRAME_SYMBOLS {
|
|
||||||
let mut logical_motor = 0;
|
|
||||||
while logical_motor < DSHOT_CHANNELS {
|
|
||||||
let channel = logical_to_tim3_channel[logical_motor];
|
|
||||||
// SAFETY: `destination` identifies one of the exclusive static frame buffers during
|
|
||||||
// initialization. The selected platform route is a permutation of
|
|
||||||
// the four TIM3 channels, so the bounded indices stay within
|
|
||||||
// DSHOT_BUFFER_LENGTH and each DMA word is initialized once.
|
|
||||||
unsafe {
|
|
||||||
destination
|
|
||||||
.add(bit * DSHOT_CHANNELS + channel)
|
|
||||||
.write(symbols[bit] as u32)
|
|
||||||
};
|
|
||||||
logical_motor += 1;
|
|
||||||
}
|
|
||||||
bit += 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
fn zero_frame() -> *const u32 {
|
|
||||||
core::ptr::addr_of!(DSHOT_ZERO_FRAME).cast()
|
|
||||||
}
|
|
||||||
fn armed_idle_frame() -> *const u32 {
|
|
||||||
core::ptr::addr_of!(DSHOT_ARMED_IDLE_FRAME).cast()
|
|
||||||
}
|
|
||||||
|
|
||||||
fn dma() -> &'static stm32f722::dma2::RegisterBlock {
|
|
||||||
// SAFETY: DMA1 stream 2 is exclusively claimed by Tim3Dshot; all access to this register
|
|
||||||
// block is serialized through the platform's RTIC-owned motor state.
|
|
||||||
unsafe { &*stm32f722::DMA1::ptr() }
|
|
||||||
}
|
|
||||||
fn dma_stream() -> &'static stm32f722::dma2::ST {
|
|
||||||
dma().st(2)
|
|
||||||
}
|
|
||||||
|
|
||||||
fn disable_dma_stream() -> bool {
|
|
||||||
dma_stream().cr().modify(|_, write| write.en().disabled());
|
|
||||||
let mut spins = 1_024;
|
|
||||||
while dma_stream().cr().read().en().is_enabled() && spins != 0 {
|
|
||||||
spins -= 1;
|
|
||||||
}
|
|
||||||
if dma_stream().cr().read().en().is_enabled() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
dma().lifcr().write(|write| {
|
|
||||||
write
|
|
||||||
.cfeif2()
|
|
||||||
.clear()
|
|
||||||
.cdmeif2()
|
|
||||||
.clear()
|
|
||||||
.cteif2()
|
|
||||||
.clear()
|
|
||||||
.chtif2()
|
|
||||||
.clear()
|
|
||||||
.ctcif2()
|
|
||||||
.clear()
|
|
||||||
});
|
|
||||||
true
|
|
||||||
}
|
|
||||||
|
|
||||||
fn configure_dma_stream(frame: *const u32) -> bool {
|
|
||||||
if !disable_dma_stream() {
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
dma_stream().cr().write(|write| {
|
|
||||||
// SAFETY: stream 2 is disabled above; channel 5 and the width/direction settings are
|
|
||||||
// the documented DMA1/TIM3_UP route for this fixed u32 frame buffer.
|
|
||||||
unsafe {
|
|
||||||
write
|
|
||||||
.chsel()
|
|
||||||
.bits(5)
|
|
||||||
.dir()
|
|
||||||
.memory_to_peripheral()
|
|
||||||
.pl()
|
|
||||||
.very_high()
|
|
||||||
.minc()
|
|
||||||
.incremented()
|
|
||||||
.pinc()
|
|
||||||
.fixed()
|
|
||||||
.msize()
|
|
||||||
.bits32()
|
|
||||||
.psize()
|
|
||||||
.bits32()
|
|
||||||
.circ()
|
|
||||||
.disabled()
|
|
||||||
.tcie()
|
|
||||||
.disabled()
|
|
||||||
.teie()
|
|
||||||
.disabled()
|
|
||||||
.dmeie()
|
|
||||||
.disabled()
|
|
||||||
}
|
|
||||||
});
|
|
||||||
dma_stream().ndtr().write(|write| {
|
|
||||||
// SAFETY: transfer count equals the fixed initialized DMA frame length.
|
|
||||||
unsafe { write.bits(DSHOT_BUFFER_LENGTH as u32) }
|
|
||||||
});
|
|
||||||
dma_stream().par().write(|write| {
|
|
||||||
// SAFETY: the address is TIM3's DMA-burst register, the fixed peripheral endpoint.
|
|
||||||
unsafe { write.bits(tim3_dmar_address()) }
|
|
||||||
});
|
|
||||||
dma_stream().m0ar().write(|write| {
|
|
||||||
// SAFETY: `frame` is a static aligned buffer retained for the entire DMA transfer.
|
|
||||||
unsafe { write.bits(frame as u32) }
|
|
||||||
});
|
|
||||||
dma_stream().fcr().write(|write| {
|
|
||||||
// SAFETY: FIFO threshold 3 and direct-mode disable are documented legal DMA settings.
|
|
||||||
unsafe { write.fth().bits(3).dmdis().set_bit() }
|
|
||||||
});
|
|
||||||
true
|
|
||||||
}
|
|
||||||
|
|
||||||
fn tim3_dmar_address() -> u32 {
|
|
||||||
// SAFETY: TIM3 is uniquely owned by Tim3Dshot; this only obtains the stable MMIO address of
|
|
||||||
// its DMAR register and does not create mutable Rust access to the peripheral.
|
|
||||||
unsafe { (&*stm32f722::TIM3::ptr()).dmar().as_ptr() as u32 }
|
|
||||||
}
|
|
||||||
|
|
||||||
fn port() -> &'static stm32f722::gpiob::RegisterBlock {
|
|
||||||
// SAFETY: PB0/PB1/PB4/PB5 are exclusively configured as TIM3 motor pins by this platform
|
|
||||||
// resource; no board code receives GPIOB or this raw register block.
|
|
||||||
unsafe { &*stm32f722::GPIOB::ptr() }
|
|
||||||
}
|
|
||||||
|
|
||||||
fn configure_gpio_low() {
|
|
||||||
let port = port();
|
|
||||||
port.moder().modify(|_, write| {
|
|
||||||
write
|
|
||||||
.moder0()
|
|
||||||
.output()
|
|
||||||
.moder1()
|
|
||||||
.output()
|
|
||||||
.moder4()
|
|
||||||
.output()
|
|
||||||
.moder5()
|
|
||||||
.output()
|
|
||||||
});
|
|
||||||
port.otyper().modify(|_, write| {
|
|
||||||
write
|
|
||||||
.ot0()
|
|
||||||
.push_pull()
|
|
||||||
.ot1()
|
|
||||||
.push_pull()
|
|
||||||
.ot4()
|
|
||||||
.push_pull()
|
|
||||||
.ot5()
|
|
||||||
.push_pull()
|
|
||||||
});
|
|
||||||
port.pupdr().modify(|_, write| {
|
|
||||||
write
|
|
||||||
.pupdr0()
|
|
||||||
.floating()
|
|
||||||
.pupdr1()
|
|
||||||
.floating()
|
|
||||||
.pupdr4()
|
|
||||||
.floating()
|
|
||||||
.pupdr5()
|
|
||||||
.floating()
|
|
||||||
});
|
|
||||||
port.bsrr().write(|write| {
|
|
||||||
write
|
|
||||||
.br0()
|
|
||||||
.set_bit()
|
|
||||||
.br1()
|
|
||||||
.set_bit()
|
|
||||||
.br4()
|
|
||||||
.set_bit()
|
|
||||||
.br5()
|
|
||||||
.set_bit()
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
fn configure_gpio_alternate_function() {
|
|
||||||
let port = port();
|
|
||||||
port.afrl().modify(|_, write| {
|
|
||||||
// SAFETY: AF2 is TIM3 on PB0/PB1/PB4/PB5; these fields are disjoint from board-owned
|
|
||||||
// pins and have been exclusively reserved for motor output by the route profile.
|
|
||||||
unsafe {
|
|
||||||
write
|
|
||||||
.afrl0()
|
|
||||||
.bits(2)
|
|
||||||
.afrl1()
|
|
||||||
.bits(2)
|
|
||||||
.afrl4()
|
|
||||||
.bits(2)
|
|
||||||
.afrl5()
|
|
||||||
.bits(2)
|
|
||||||
}
|
|
||||||
});
|
|
||||||
port.moder().modify(|_, write| {
|
|
||||||
write
|
|
||||||
.moder0()
|
|
||||||
.alternate()
|
|
||||||
.moder1()
|
|
||||||
.alternate()
|
|
||||||
.moder4()
|
|
||||||
.alternate()
|
|
||||||
.moder5()
|
|
||||||
.alternate()
|
|
||||||
});
|
|
||||||
port.ospeedr().modify(|_, write| {
|
|
||||||
write
|
|
||||||
.ospeedr0()
|
|
||||||
.high_speed()
|
|
||||||
.ospeedr1()
|
|
||||||
.high_speed()
|
|
||||||
.ospeedr4()
|
|
||||||
.high_speed()
|
|
||||||
.ospeedr5()
|
|
||||||
.high_speed()
|
|
||||||
});
|
|
||||||
}
|
|
||||||
@@ -1,206 +0,0 @@
|
|||||||
use core::sync::atomic::{AtomicU16, Ordering};
|
|
||||||
|
|
||||||
use kaptan_actuator::pwm::{ArmSequence, Config, FrameValue, PulseWidth};
|
|
||||||
use stm32f7::stm32f722;
|
|
||||||
|
|
||||||
const APB1_TIMER_CLOCK_HZ: u32 = 108_000_000;
|
|
||||||
const TIMER_TICK_HZ: u32 = 1_000_000;
|
|
||||||
const PWM_CHANNEL_ENABLE: u32 = 0x1111;
|
|
||||||
const PWM_MODE_1_WITH_PRELOAD: u32 = 0x6868;
|
|
||||||
const DEFAULT_MINIMUM_PULSE: PulseWidth = match PulseWidth::new(1_000) {
|
|
||||||
Some(pulse) => pulse,
|
|
||||||
None => panic!("valid PWM pulse"),
|
|
||||||
};
|
|
||||||
const DEFAULT_ARMED_IDLE_PULSE: PulseWidth = match PulseWidth::new(1_000) {
|
|
||||||
Some(pulse) => pulse,
|
|
||||||
None => panic!("valid PWM pulse"),
|
|
||||||
};
|
|
||||||
|
|
||||||
static ARM_SEQUENCE: ArmSequence = ArmSequence::new(DEFAULT_ARMED_IDLE_PULSE, 0);
|
|
||||||
static MINIMUM_PULSE: AtomicU16 = AtomicU16::new(DEFAULT_MINIMUM_PULSE.microseconds());
|
|
||||||
|
|
||||||
pub(super) struct Tim3Pwm {
|
|
||||||
timer: stm32f722::TIM3,
|
|
||||||
minimum_pulse: PulseWidth,
|
|
||||||
}
|
|
||||||
|
|
||||||
impl Tim3Pwm {
|
|
||||||
pub(super) fn new(rcc: &stm32f722::RCC, timer: stm32f722::TIM3, config: Config) -> Self {
|
|
||||||
let period_us = match config.frame_rate().period_microseconds() {
|
|
||||||
Some(period) => period,
|
|
||||||
None => unreachable!(),
|
|
||||||
};
|
|
||||||
ARM_SEQUENCE.configure_minimum_frames(
|
|
||||||
config
|
|
||||||
.frame_rate()
|
|
||||||
.frames_for_millis(config.arming_duration().milliseconds()),
|
|
||||||
);
|
|
||||||
ARM_SEQUENCE.configure_armed_idle(config.armed_idle_pulse());
|
|
||||||
MINIMUM_PULSE.store(config.minimum_pulse().microseconds(), Ordering::Release);
|
|
||||||
|
|
||||||
rcc.ahb1enr().modify(|_, write| write.gpioben().set_bit());
|
|
||||||
rcc.apb1rstr().modify(|_, write| write.tim3rst().set_bit());
|
|
||||||
rcc.apb1rstr()
|
|
||||||
.modify(|_, write| write.tim3rst().clear_bit());
|
|
||||||
rcc.apb1enr().modify(|_, write| write.tim3en().set_bit());
|
|
||||||
|
|
||||||
configure_gpio_alternate_function();
|
|
||||||
timer
|
|
||||||
.cr1()
|
|
||||||
.write(|write| write.cen().clear_bit().arpe().enabled());
|
|
||||||
timer.psc().write(|write| {
|
|
||||||
// SAFETY: the fixed APB1 clock divides exactly to the documented 1 MHz timer tick.
|
|
||||||
unsafe {
|
|
||||||
write
|
|
||||||
.psc()
|
|
||||||
.bits((APB1_TIMER_CLOCK_HZ / TIMER_TICK_HZ - 1) as u16)
|
|
||||||
}
|
|
||||||
});
|
|
||||||
timer.arr().write(|write| {
|
|
||||||
// SAFETY: `frame_rate` guarantees a non-zero period fitting TIM3's ARR register.
|
|
||||||
unsafe { write.bits(u32::from(period_us - 1)) }
|
|
||||||
});
|
|
||||||
timer.ccmr1_output().write(|write| {
|
|
||||||
// SAFETY: this is the documented PWM mode 1 plus preload encoding for channels 1 and 2.
|
|
||||||
unsafe { write.bits(PWM_MODE_1_WITH_PRELOAD) }
|
|
||||||
});
|
|
||||||
timer.ccmr2_output().write(|write| {
|
|
||||||
// SAFETY: this is the documented PWM mode 1 plus preload encoding for channels 3 and 4.
|
|
||||||
unsafe { write.bits(PWM_MODE_1_WITH_PRELOAD) }
|
|
||||||
});
|
|
||||||
write_pulse(&timer, config.minimum_pulse());
|
|
||||||
timer.ccer().write(|write| {
|
|
||||||
// SAFETY: this enables only the four TIM3 output channels configured above.
|
|
||||||
unsafe { write.bits(PWM_CHANNEL_ENABLE) }
|
|
||||||
});
|
|
||||||
timer.egr().write(|write| write.ug().set_bit());
|
|
||||||
timer.sr().modify(|_, write| write.uif().clear());
|
|
||||||
timer.dier().write(|write| write.uie().enabled());
|
|
||||||
timer.cr1().modify(|_, write| write.cen().set_bit());
|
|
||||||
|
|
||||||
Self {
|
|
||||||
timer,
|
|
||||||
minimum_pulse: config.minimum_pulse(),
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
pub(super) fn arm(&mut self) {
|
|
||||||
ARM_SEQUENCE.arm();
|
|
||||||
}
|
|
||||||
|
|
||||||
pub(super) fn disarm(&mut self) {
|
|
||||||
ARM_SEQUENCE.disarm();
|
|
||||||
write_pulse(&self.timer, self.minimum_pulse);
|
|
||||||
}
|
|
||||||
|
|
||||||
pub(super) fn is_armed(&self) -> bool {
|
|
||||||
ARM_SEQUENCE.is_armed()
|
|
||||||
}
|
|
||||||
|
|
||||||
pub(super) fn on_tim3_update() {
|
|
||||||
// SAFETY: TIM3 is exclusively owned by the platform motor resource; RTIC locks that
|
|
||||||
// resource across the service task and this update handler.
|
|
||||||
let timer = unsafe { &*stm32f722::TIM3::ptr() };
|
|
||||||
timer.sr().modify(|_, write| write.uif().clear());
|
|
||||||
let pulse = match ARM_SEQUENCE.next_frame() {
|
|
||||||
Some(FrameValue::ArmedIdle(pulse)) => pulse,
|
|
||||||
Some(FrameValue::Minimum) | None => configured_minimum_pulse(),
|
|
||||||
};
|
|
||||||
write_pulse(timer, pulse);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
fn configured_minimum_pulse() -> PulseWidth {
|
|
||||||
match PulseWidth::new(MINIMUM_PULSE.load(Ordering::Acquire)) {
|
|
||||||
Some(pulse) => pulse,
|
|
||||||
None => unreachable!(),
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
fn write_pulse(timer: &stm32f722::tim3::RegisterBlock, pulse: PulseWidth) {
|
|
||||||
let ticks = u32::from(pulse.microseconds());
|
|
||||||
timer.ccr1().write(|write| {
|
|
||||||
// SAFETY: `PulseWidth` bounds ticks to the configured PWM frame interval.
|
|
||||||
unsafe { write.bits(ticks) }
|
|
||||||
});
|
|
||||||
timer.ccr2().write(|write| {
|
|
||||||
// SAFETY: `PulseWidth` bounds ticks to the configured PWM frame interval.
|
|
||||||
unsafe { write.bits(ticks) }
|
|
||||||
});
|
|
||||||
timer.ccr3().write(|write| {
|
|
||||||
// SAFETY: `PulseWidth` bounds ticks to the configured PWM frame interval.
|
|
||||||
unsafe { write.bits(ticks) }
|
|
||||||
});
|
|
||||||
timer.ccr4().write(|write| {
|
|
||||||
// SAFETY: `PulseWidth` bounds ticks to the configured PWM frame interval.
|
|
||||||
unsafe { write.bits(ticks) }
|
|
||||||
});
|
|
||||||
}
|
|
||||||
|
|
||||||
fn port() -> &'static stm32f722::gpiob::RegisterBlock {
|
|
||||||
// SAFETY: PB0/PB1/PB4/PB5 are exclusively claimed by the selected TIM3 motor route; board
|
|
||||||
// code never receives GPIOB and RTIC serializes platform writes to these registers.
|
|
||||||
unsafe { &*stm32f722::GPIOB::ptr() }
|
|
||||||
}
|
|
||||||
|
|
||||||
fn configure_gpio_alternate_function() {
|
|
||||||
let port = port();
|
|
||||||
port.afrl().modify(|_, write| {
|
|
||||||
// SAFETY: AF2 selects TIM3 on these four reserved GPIOB fields, leaving all other pins intact.
|
|
||||||
unsafe {
|
|
||||||
write
|
|
||||||
.afrl0()
|
|
||||||
.bits(2)
|
|
||||||
.afrl1()
|
|
||||||
.bits(2)
|
|
||||||
.afrl4()
|
|
||||||
.bits(2)
|
|
||||||
.afrl5()
|
|
||||||
.bits(2)
|
|
||||||
}
|
|
||||||
});
|
|
||||||
port.moder().modify(|_, write| {
|
|
||||||
write
|
|
||||||
.moder0()
|
|
||||||
.alternate()
|
|
||||||
.moder1()
|
|
||||||
.alternate()
|
|
||||||
.moder4()
|
|
||||||
.alternate()
|
|
||||||
.moder5()
|
|
||||||
.alternate()
|
|
||||||
});
|
|
||||||
port.otyper().modify(|_, write| {
|
|
||||||
write
|
|
||||||
.ot0()
|
|
||||||
.push_pull()
|
|
||||||
.ot1()
|
|
||||||
.push_pull()
|
|
||||||
.ot4()
|
|
||||||
.push_pull()
|
|
||||||
.ot5()
|
|
||||||
.push_pull()
|
|
||||||
});
|
|
||||||
port.ospeedr().modify(|_, write| {
|
|
||||||
write
|
|
||||||
.ospeedr0()
|
|
||||||
.high_speed()
|
|
||||||
.ospeedr1()
|
|
||||||
.high_speed()
|
|
||||||
.ospeedr4()
|
|
||||||
.high_speed()
|
|
||||||
.ospeedr5()
|
|
||||||
.high_speed()
|
|
||||||
});
|
|
||||||
port.pupdr().modify(|_, write| {
|
|
||||||
write
|
|
||||||
.pupdr0()
|
|
||||||
.floating()
|
|
||||||
.pupdr1()
|
|
||||||
.floating()
|
|
||||||
.pupdr4()
|
|
||||||
.floating()
|
|
||||||
.pupdr5()
|
|
||||||
.floating()
|
|
||||||
});
|
|
||||||
}
|
|
||||||
@@ -1,6 +1,6 @@
|
|||||||
//! Profile-independent STM32F722 flight state and ownership graph.
|
//! Physical-sensor telemetry STM32F722 board-I/O state and ownership graph.
|
||||||
|
|
||||||
use core::sync::atomic::{AtomicBool, AtomicU8, Ordering};
|
use core::sync::atomic::{AtomicU8, Ordering};
|
||||||
|
|
||||||
use super::peripherals::{
|
use super::peripherals::{
|
||||||
DwtMicrosecondClock, I2c1, I2c1Config, ReceiveInterrupt, Tim2Periodic, Uart4,
|
DwtMicrosecondClock, I2c1, I2c1Config, ReceiveInterrupt, Tim2Periodic, Uart4,
|
||||||
@@ -9,7 +9,6 @@ use super::peripherals::{
|
|||||||
configure_uart4_pins_pa0_pa1, configure_usart1_rx_pin_pa10, configure_usart1_tx_pin_pa9,
|
configure_uart4_pins_pa0_pa1, configure_usart1_rx_pin_pa10, configure_usart1_tx_pin_pa9,
|
||||||
configure_usart2_pins_pa2_pa3,
|
configure_usart2_pins_pa2_pa3,
|
||||||
};
|
};
|
||||||
use kaptan_actuator::{ArmingDuration, MotorProtocol};
|
|
||||||
use kaptan_board::{BoardConfig, VerticalSensingKind};
|
use kaptan_board::{BoardConfig, VerticalSensingKind};
|
||||||
use kaptan_data_channel::{
|
use kaptan_data_channel::{
|
||||||
DataChannel, DataSources, LatestImuMailbox, LatestOpticalFlowMailbox, LatestRangeMailbox,
|
DataChannel, DataSources, LatestImuMailbox, LatestOpticalFlowMailbox, LatestRangeMailbox,
|
||||||
@@ -18,17 +17,13 @@ use kaptan_drivers::{
|
|||||||
lidar_lite_v3::{AcquisitionMode, LidarLiteV3, LidarLiteV3Service, LidarLiteV3ServiceEvent},
|
lidar_lite_v3::{AcquisitionMode, LidarLiteV3, LidarLiteV3Service, LidarLiteV3ServiceEvent},
|
||||||
mtf01::Mtf01Service,
|
mtf01::Mtf01Service,
|
||||||
};
|
};
|
||||||
use kaptan_mavlink::{
|
|
||||||
commands::arm::{ArmRequest, decode_arm_request},
|
|
||||||
receiver::MavlinkReceiver,
|
|
||||||
};
|
|
||||||
use kaptan_spi::SpiCompletion;
|
use kaptan_spi::SpiCompletion;
|
||||||
use stm32f7::stm32f722;
|
use stm32f7::stm32f722;
|
||||||
|
|
||||||
use crate::{BoardError, F722BoardRoutes, F722Profile};
|
use crate::{BoardError, F722BoardRoutes, F722Profile};
|
||||||
|
|
||||||
use super::{
|
use super::{
|
||||||
MotorRoute, VerticalRoute, clock,
|
VerticalRoute, clock,
|
||||||
data_channel::{
|
data_channel::{
|
||||||
F722MavlinkTransmitter, F722StatusLed, GeprcStatusLed, SERVICE_HZ, SERVICE_TIMER_CONFIG,
|
F722MavlinkTransmitter, F722StatusLed, GeprcStatusLed, SERVICE_HZ, SERVICE_TIMER_CONFIG,
|
||||||
vertical_data_config,
|
vertical_data_config,
|
||||||
@@ -42,24 +37,6 @@ const MTF01_BAUD: u32 = 115_200;
|
|||||||
const I2C1_CONFIG: I2c1Config = I2c1Config::from_timing_register(0x2040_4768);
|
const I2C1_CONFIG: I2c1Config = I2c1Config::from_timing_register(0x2040_4768);
|
||||||
const LIDAR_MODE: AcquisitionMode = AcquisitionMode::WithReceiverBiasCorrection;
|
const LIDAR_MODE: AcquisitionMode = AcquisitionMode::WithReceiverBiasCorrection;
|
||||||
const LIDAR_STARTUP_TICKS: u8 = 3;
|
const LIDAR_STARTUP_TICKS: u8 = 3;
|
||||||
const MOTOR_PROTOCOL: MotorProtocol = MotorProtocol::Dshot(
|
|
||||||
match kaptan_actuator::dshot::Config::new(
|
|
||||||
kaptan_actuator::dshot::Rate::Dshot600,
|
|
||||||
match kaptan_actuator::dshot::Throttle::new(300) {
|
|
||||||
Some(throttle) => throttle,
|
|
||||||
None => panic!("armed idle throttle must be valid"),
|
|
||||||
},
|
|
||||||
match ArmingDuration::from_millis(2_000) {
|
|
||||||
Some(duration) => duration,
|
|
||||||
None => panic!("arming duration must be valid"),
|
|
||||||
},
|
|
||||||
) {
|
|
||||||
Some(config) => config,
|
|
||||||
None => panic!("DShot motor protocol must be valid"),
|
|
||||||
},
|
|
||||||
);
|
|
||||||
|
|
||||||
static IMU_HEALTHY: AtomicBool = AtomicBool::new(false);
|
|
||||||
static UART_COMPLETION: AtomicU8 = AtomicU8::new(0);
|
static UART_COMPLETION: AtomicU8 = AtomicU8::new(0);
|
||||||
static SAMPLES: LatestImuMailbox = LatestImuMailbox::new();
|
static SAMPLES: LatestImuMailbox = LatestImuMailbox::new();
|
||||||
static RANGES: LatestRangeMailbox = LatestRangeMailbox::new();
|
static RANGES: LatestRangeMailbox = LatestRangeMailbox::new();
|
||||||
@@ -75,13 +52,11 @@ pub type F722ServiceTimer = Tim2Periodic;
|
|||||||
|
|
||||||
/// RTIC-shared MAVLink state.
|
/// RTIC-shared MAVLink state.
|
||||||
///
|
///
|
||||||
/// Only USART receive and the MAVLink worker access this state, so its
|
/// Only the MAVLink worker accesses this state, so its priority ceiling is 9
|
||||||
/// priority ceiling is 9 rather than the IMU or vertical-sensor IRQ ceiling.
|
/// rather than the IMU or vertical-sensor IRQ ceiling.
|
||||||
pub struct F722MavlinkState {
|
pub struct F722MavlinkState {
|
||||||
receiver: MavlinkReceiver<4>,
|
|
||||||
data_channel: DataChannel<'static, F722MavlinkTransmitter, SERVICE_HZ>,
|
data_channel: DataChannel<'static, F722MavlinkTransmitter, SERVICE_HZ>,
|
||||||
led: F722StatusLed,
|
led: F722StatusLed,
|
||||||
config: BoardConfig,
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/// RTIC-shared LIDAR state.
|
/// RTIC-shared LIDAR state.
|
||||||
@@ -104,7 +79,6 @@ pub struct F722InitResources {
|
|||||||
pub(crate) timer: F722ServiceTimer,
|
pub(crate) timer: F722ServiceTimer,
|
||||||
pub(crate) lidar: F722LidarState,
|
pub(crate) lidar: F722LidarState,
|
||||||
pub(crate) mavlink: F722MavlinkState,
|
pub(crate) mavlink: F722MavlinkState,
|
||||||
pub(crate) motors: crate::actuator::F722MotorOutput,
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Validates the relationship between portable metadata and selected sensor
|
/// Validates the relationship between portable metadata and selected sensor
|
||||||
@@ -113,7 +87,8 @@ pub(crate) fn validate_vertical_route(
|
|||||||
config: BoardConfig,
|
config: BoardConfig,
|
||||||
routes: F722BoardRoutes,
|
routes: F722BoardRoutes,
|
||||||
) -> Result<(), BoardError> {
|
) -> Result<(), BoardError> {
|
||||||
if config.data_channel.service_hz != SERVICE_HZ {
|
let telemetry = config.telemetry().ok_or(BoardError::Initialization)?;
|
||||||
|
if telemetry.data_channel.service_hz != SERVICE_HZ {
|
||||||
return Err(BoardError::UnsupportedServiceFrequency);
|
return Err(BoardError::UnsupportedServiceFrequency);
|
||||||
}
|
}
|
||||||
let expected = match routes.vertical {
|
let expected = match routes.vertical {
|
||||||
@@ -121,7 +96,7 @@ pub(crate) fn validate_vertical_route(
|
|||||||
VerticalRoute::Mtf01 => VerticalSensingKind::RangeAndOpticalFlow,
|
VerticalRoute::Mtf01 => VerticalSensingKind::RangeAndOpticalFlow,
|
||||||
VerticalRoute::LidarLiteV3 => VerticalSensingKind::Range,
|
VerticalRoute::LidarLiteV3 => VerticalSensingKind::Range,
|
||||||
};
|
};
|
||||||
if config.vertical_sensing != expected {
|
if telemetry.vertical_sensing != expected {
|
||||||
return Err(BoardError::VerticalSensorCapabilityMismatch);
|
return Err(BoardError::VerticalSensorCapabilityMismatch);
|
||||||
}
|
}
|
||||||
Ok(())
|
Ok(())
|
||||||
@@ -136,16 +111,15 @@ pub(crate) fn initialize(
|
|||||||
let clock = clock::initialize(&mut core, &device).map_err(|_| BoardError::Initialization)?;
|
let clock = clock::initialize(&mut core, &device).map_err(|_| BoardError::Initialization)?;
|
||||||
|
|
||||||
// The profile is written once before any interrupt is enabled. ISR helpers
|
// The profile is written once before any interrupt is enabled. ISR helpers
|
||||||
// read it with Acquire ordering and only access a route initialized by the
|
// read it with Acquire ordering and only access the initialized route.
|
||||||
// matching branch below.
|
|
||||||
interrupts::activate_profile(routes.profile());
|
interrupts::activate_profile(routes.profile());
|
||||||
|
|
||||||
match routes.profile() {
|
match routes.profile() {
|
||||||
F722Profile::Mpu6000Spi1Usart1 => {
|
F722Profile::Mpu6000Spi1Usart1 => {
|
||||||
initialize_mpu6000(device, clock, config, routes.motors, routes.vertical)
|
initialize_mpu6000(device, clock, config, routes.vertical)
|
||||||
}
|
}
|
||||||
F722Profile::Icm42688Spi3Usart2 => {
|
F722Profile::Icm42688Spi3Usart2 => {
|
||||||
initialize_icm42688(device, clock, config, routes.motors, routes.vertical)
|
initialize_icm42688(device, clock, config, routes.vertical)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -154,7 +128,6 @@ fn initialize_mpu6000(
|
|||||||
device: stm32f722::Peripherals,
|
device: stm32f722::Peripherals,
|
||||||
clock: DwtMicrosecondClock,
|
clock: DwtMicrosecondClock,
|
||||||
config: BoardConfig,
|
config: BoardConfig,
|
||||||
motors: MotorRoute,
|
|
||||||
vertical: VerticalRoute,
|
vertical: VerticalRoute,
|
||||||
) -> Result<F722InitResources, BoardError> {
|
) -> Result<F722InitResources, BoardError> {
|
||||||
configure_spi1_pins_pa5_pa6_pa7(&device.RCC, &device.GPIOA);
|
configure_spi1_pins_pa5_pa6_pa7(&device.RCC, &device.GPIOA);
|
||||||
@@ -172,7 +145,13 @@ fn initialize_mpu6000(
|
|||||||
clock.fork(),
|
clock.fork(),
|
||||||
vertical,
|
vertical,
|
||||||
)?;
|
)?;
|
||||||
let imu = F722Imu::mpu6000(&device.RCC, device.SPI1, device.DMA2, device.GPIOB, clock);
|
let imu = F722Imu::mpu6000(
|
||||||
|
&device.RCC,
|
||||||
|
device.SPI1,
|
||||||
|
device.DMA2,
|
||||||
|
device.GPIOB,
|
||||||
|
clock.fork(),
|
||||||
|
);
|
||||||
let transmitter = F722MavlinkTransmitter::usart1(&device.RCC, device.USART1)
|
let transmitter = F722MavlinkTransmitter::usart1(&device.RCC, device.USART1)
|
||||||
.map_err(|_| BoardError::Initialization)?;
|
.map_err(|_| BoardError::Initialization)?;
|
||||||
let led = F722StatusLed::Kakute(super::peripherals::ActiveLowLedPd2::new(
|
let led = F722StatusLed::Kakute(super::peripherals::ActiveLowLedPd2::new(
|
||||||
@@ -182,10 +161,7 @@ fn initialize_mpu6000(
|
|||||||
finish(
|
finish(
|
||||||
&device.RCC,
|
&device.RCC,
|
||||||
device.TIM2,
|
device.TIM2,
|
||||||
device.TIM3,
|
|
||||||
device.TIM5,
|
|
||||||
config,
|
config,
|
||||||
motors,
|
|
||||||
vertical,
|
vertical,
|
||||||
imu,
|
imu,
|
||||||
range,
|
range,
|
||||||
@@ -200,7 +176,6 @@ fn initialize_icm42688(
|
|||||||
device: stm32f722::Peripherals,
|
device: stm32f722::Peripherals,
|
||||||
clock: DwtMicrosecondClock,
|
clock: DwtMicrosecondClock,
|
||||||
config: BoardConfig,
|
config: BoardConfig,
|
||||||
motors: MotorRoute,
|
|
||||||
vertical: VerticalRoute,
|
vertical: VerticalRoute,
|
||||||
) -> Result<F722InitResources, BoardError> {
|
) -> Result<F722InitResources, BoardError> {
|
||||||
configure_spi3_pins_pc10_pc11_pc12(&device.RCC, &device.GPIOC);
|
configure_spi3_pins_pc10_pc11_pc12(&device.RCC, &device.GPIOC);
|
||||||
@@ -224,17 +199,20 @@ fn initialize_icm42688(
|
|||||||
clock.fork(),
|
clock.fork(),
|
||||||
vertical,
|
vertical,
|
||||||
)?;
|
)?;
|
||||||
let imu = F722Imu::icm42688(&device.RCC, device.SPI3, device.DMA1, &device.GPIOA, clock);
|
let imu = F722Imu::icm42688(
|
||||||
|
&device.RCC,
|
||||||
|
device.SPI3,
|
||||||
|
device.DMA1,
|
||||||
|
&device.GPIOA,
|
||||||
|
clock.fork(),
|
||||||
|
);
|
||||||
let transmitter = F722MavlinkTransmitter::usart2(&device.RCC, device.USART2)
|
let transmitter = F722MavlinkTransmitter::usart2(&device.RCC, device.USART2)
|
||||||
.map_err(|_| BoardError::Initialization)?;
|
.map_err(|_| BoardError::Initialization)?;
|
||||||
let led = F722StatusLed::Geprc(GeprcStatusLed::new(&device.RCC, device.GPIOA));
|
let led = F722StatusLed::Geprc(GeprcStatusLed::new(&device.RCC, device.GPIOA));
|
||||||
finish(
|
finish(
|
||||||
&device.RCC,
|
&device.RCC,
|
||||||
device.TIM2,
|
device.TIM2,
|
||||||
device.TIM3,
|
|
||||||
device.TIM5,
|
|
||||||
config,
|
config,
|
||||||
motors,
|
|
||||||
vertical,
|
vertical,
|
||||||
imu,
|
imu,
|
||||||
range,
|
range,
|
||||||
@@ -249,10 +227,7 @@ fn initialize_icm42688(
|
|||||||
fn finish(
|
fn finish(
|
||||||
rcc: &stm32f722::RCC,
|
rcc: &stm32f722::RCC,
|
||||||
tim2: stm32f722::TIM2,
|
tim2: stm32f722::TIM2,
|
||||||
tim3: stm32f722::TIM3,
|
|
||||||
tim5: stm32f722::TIM5,
|
|
||||||
config: BoardConfig,
|
config: BoardConfig,
|
||||||
motors: MotorRoute,
|
|
||||||
vertical: VerticalRoute,
|
vertical: VerticalRoute,
|
||||||
imu: F722Imu,
|
imu: F722Imu,
|
||||||
range: Option<F722Range>,
|
range: Option<F722Range>,
|
||||||
@@ -281,13 +256,7 @@ fn finish(
|
|||||||
range,
|
range,
|
||||||
startup_ticks: 0,
|
startup_ticks: 0,
|
||||||
},
|
},
|
||||||
mavlink: F722MavlinkState {
|
mavlink: F722MavlinkState { data_channel, led },
|
||||||
receiver: MavlinkReceiver::new(),
|
|
||||||
data_channel,
|
|
||||||
led,
|
|
||||||
config,
|
|
||||||
},
|
|
||||||
motors: crate::actuator::F722MotorOutput::new(rcc, tim3, tim5, MOTOR_PROTOCOL, motors),
|
|
||||||
})
|
})
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -439,42 +408,21 @@ pub(crate) fn process_imu(imu: &mut F722Imu, events: u8) {
|
|||||||
fn accept_imu_event(event: ImuEvent) {
|
fn accept_imu_event(event: ImuEvent) {
|
||||||
match event {
|
match event {
|
||||||
ImuEvent::Sample(sample) => {
|
ImuEvent::Sample(sample) => {
|
||||||
IMU_HEALTHY.store(true, Ordering::Release);
|
|
||||||
SAMPLES.publish(sample);
|
SAMPLES.publish(sample);
|
||||||
}
|
}
|
||||||
ImuEvent::Fault => IMU_HEALTHY.store(false, Ordering::Release),
|
ImuEvent::Fault => {}
|
||||||
ImuEvent::Idle | ImuEvent::Initialized => {}
|
ImuEvent::Idle | ImuEvent::Initialized => {}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
pub(crate) fn imu_is_healthy() -> bool {
|
|
||||||
IMU_HEALTHY.load(Ordering::Acquire)
|
|
||||||
}
|
|
||||||
|
|
||||||
impl F722MavlinkState {
|
impl F722MavlinkState {
|
||||||
pub(crate) fn on_mavlink_byte(&mut self, receive: ReceiveInterrupt) -> bool {
|
/// Handles bounded DMA and timer work for the telemetry image.
|
||||||
match receive {
|
pub(crate) fn process_worker_events(&mut self, events: u8) {
|
||||||
ReceiveInterrupt::Byte(byte) => self.receiver.receive_byte(byte),
|
|
||||||
ReceiveInterrupt::Fault => {
|
|
||||||
self.receiver.reset();
|
|
||||||
false
|
|
||||||
}
|
|
||||||
ReceiveInterrupt::NotPending => false,
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Handles the bounded DMA and timer work that does not need the motor
|
|
||||||
/// resource. `motors_armed` is sampled under its separate RTIC lock.
|
|
||||||
pub(crate) fn process_worker_events(&mut self, events: u8, motors_armed: bool) {
|
|
||||||
const TIMER: u8 = 1 << 0;
|
const TIMER: u8 = 1 << 0;
|
||||||
const DMA: u8 = 1 << 2;
|
const DMA: u8 = 1 << 2;
|
||||||
|
|
||||||
if events & DMA != 0 {
|
if events & DMA != 0 {
|
||||||
let completion = match UART_COMPLETION.swap(0, Ordering::AcqRel) {
|
let completion = take_mavlink_completion();
|
||||||
1 => kaptan_dma::TxCompletion::Complete,
|
|
||||||
2 => kaptan_dma::TxCompletion::TransferFault,
|
|
||||||
_ => kaptan_dma::TxCompletion::NotPending,
|
|
||||||
};
|
|
||||||
self.data_channel.on_dma_complete(completion);
|
self.data_channel.on_dma_complete(completion);
|
||||||
if completion == kaptan_dma::TxCompletion::Complete {
|
if completion == kaptan_dma::TxCompletion::Complete {
|
||||||
self.led.toggle();
|
self.led.toggle();
|
||||||
@@ -482,31 +430,21 @@ impl F722MavlinkState {
|
|||||||
}
|
}
|
||||||
|
|
||||||
if events & TIMER != 0 {
|
if events & TIMER != 0 {
|
||||||
|
// The board is initialized and transmitting physical telemetry,
|
||||||
|
// but has no flight runtime or output authority. `STANDBY` with
|
||||||
|
// an unarmed base mode communicates that accurately; `UNINIT`
|
||||||
|
// would incorrectly claim initialization never completed.
|
||||||
self.data_channel
|
self.data_channel
|
||||||
.on_service_tick(IMU_HEALTHY.load(Ordering::Acquire), motors_armed);
|
.on_service_tick(kaptan_mavlink::message::MavState::MAV_STATE_STANDBY, false);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
/// Removes already-decoded non-arm frames and returns one arm command at a
|
pub(crate) fn take_mavlink_completion() -> kaptan_dma::TxCompletion {
|
||||||
/// time. The worker releases this resource before taking the motor lock.
|
match UART_COMPLETION.swap(0, Ordering::AcqRel) {
|
||||||
pub(crate) fn next_arm_request(&mut self) -> Option<ArmRequest> {
|
1 => kaptan_dma::TxCompletion::Complete,
|
||||||
while let Some(frame) = self.receiver.take_received() {
|
2 => kaptan_dma::TxCompletion::TransferFault,
|
||||||
if let Some(request) = decode_arm_request(
|
_ => kaptan_dma::TxCompletion::NotPending,
|
||||||
&frame,
|
|
||||||
self.config.mavlink.system_id,
|
|
||||||
self.config.mavlink.component_id,
|
|
||||||
) {
|
|
||||||
return Some(request);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
None
|
|
||||||
}
|
|
||||||
|
|
||||||
pub(crate) fn acknowledge_arm_request(&mut self, result: kaptan_mavlink::message::MavResult) {
|
|
||||||
self.data_channel.acknowledge_command(
|
|
||||||
kaptan_mavlink::message::MavCmd::MAV_CMD_COMPONENT_ARM_DISARM,
|
|
||||||
result,
|
|
||||||
);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -530,14 +468,16 @@ fn publish_lidar_event<E>(event: LidarLiteV3ServiceEvent<E>) {
|
|||||||
#[cfg(test)]
|
#[cfg(test)]
|
||||||
mod tests {
|
mod tests {
|
||||||
use super::validate_vertical_route;
|
use super::validate_vertical_route;
|
||||||
use crate::{F7BoardBuilder, Mavlink, MotorOutputs, Mpu6000, Pa4, VerticalSensor};
|
use crate::{F7BoardBuilder, Mavlink, Mpu6000, Pa4, VerticalSensor};
|
||||||
use kaptan_board::{BoardConfig, DataChannelRates, MavlinkEndpoint, VerticalSensingKind};
|
use kaptan_board::{
|
||||||
|
BoardConfig, BoardProfile, DataChannelRates, MavlinkEndpoint, TelemetryProfile,
|
||||||
|
VerticalSensingKind,
|
||||||
|
};
|
||||||
|
|
||||||
#[test]
|
#[test]
|
||||||
fn rejects_metadata_that_claims_flow_for_a_lidar_route() {
|
fn rejects_metadata_that_claims_flow_for_a_lidar_route() {
|
||||||
let routes = F7BoardBuilder::new()
|
let routes = F7BoardBuilder::new()
|
||||||
.imu(Mpu6000::on_spi1().with_drdy(Pa4))
|
.imu(Mpu6000::on_spi1().with_drdy(Pa4))
|
||||||
.motors(MotorOutputs::kakute_tim3_tim5().dshot600())
|
|
||||||
.mavlink(Mavlink::on_usart1())
|
.mavlink(Mavlink::on_usart1())
|
||||||
.vertical_sensor(VerticalSensor::lidar_lite_v3_on_i2c1())
|
.vertical_sensor(VerticalSensor::lidar_lite_v3_on_i2c1())
|
||||||
.build()
|
.build()
|
||||||
@@ -548,14 +488,16 @@ mod tests {
|
|||||||
system_id: 1,
|
system_id: 1,
|
||||||
component_id: 1,
|
component_id: 1,
|
||||||
},
|
},
|
||||||
vertical_sensing: VerticalSensingKind::RangeAndOpticalFlow,
|
profile: BoardProfile::TelemetryOnly(TelemetryProfile {
|
||||||
data_channel: DataChannelRates {
|
vertical_sensing: VerticalSensingKind::RangeAndOpticalFlow,
|
||||||
service_hz: 100,
|
data_channel: DataChannelRates {
|
||||||
raw_imu_hz: 20,
|
service_hz: 100,
|
||||||
range_hz: Some(20),
|
raw_imu_hz: 20,
|
||||||
optical_flow_hz: Some(20),
|
range_hz: Some(20),
|
||||||
heartbeat_hz: 2,
|
optical_flow_hz: Some(20),
|
||||||
},
|
heartbeat_hz: 2,
|
||||||
|
},
|
||||||
|
}),
|
||||||
};
|
};
|
||||||
assert_eq!(
|
assert_eq!(
|
||||||
validate_vertical_route(config, routes),
|
validate_vertical_route(config, routes),
|
||||||
|
|||||||
@@ -9,6 +9,11 @@ use kaptan_mavlink::{
|
|||||||
transmitter::{FrameClass, MavlinkTransmitter},
|
transmitter::{FrameClass, MavlinkTransmitter},
|
||||||
};
|
};
|
||||||
|
|
||||||
|
// TODO(configuration): transport baud rate, sensor orientation/range, and
|
||||||
|
// freshness are currently development profile constants. A ground-station
|
||||||
|
// configuration flow must stage them with sensor calibration and link
|
||||||
|
// authority, apply them only with output inhibited, then reset estimator,
|
||||||
|
// controller, allocator, and safety prearm state as one revision.
|
||||||
pub(crate) const UART1_CLOCK_HZ: u32 = 108_000_000;
|
pub(crate) const UART1_CLOCK_HZ: u32 = 108_000_000;
|
||||||
pub(crate) const UART2_CLOCK_HZ: u32 = 54_000_000;
|
pub(crate) const UART2_CLOCK_HZ: u32 = 54_000_000;
|
||||||
pub(crate) const MAVLINK_BAUD: u32 = 115_200;
|
pub(crate) const MAVLINK_BAUD: u32 = 115_200;
|
||||||
@@ -21,7 +26,10 @@ pub(crate) const SERVICE_TIMER_CONFIG: peripherals::Tim2PeriodicConfig =
|
|||||||
auto_reload: 99,
|
auto_reload: 99,
|
||||||
};
|
};
|
||||||
|
|
||||||
/// Maps a selected vertical electrical route to complete data-channel metadata.
|
/// Maps a selected vertical electrical route to development data-channel metadata.
|
||||||
|
///
|
||||||
|
/// The values are telemetry descriptions only; they neither calibrate a flight
|
||||||
|
/// estimator nor authorize an `ExternalControlOnly` profile.
|
||||||
pub(crate) fn vertical_data_config(route: VerticalRoute) -> VerticalDataConfig {
|
pub(crate) fn vertical_data_config(route: VerticalRoute) -> VerticalDataConfig {
|
||||||
let distance =
|
let distance =
|
||||||
|minimum_distance_centimetres, maximum_distance_centimetres| DistanceSensorConfig {
|
|minimum_distance_centimetres, maximum_distance_centimetres| DistanceSensorConfig {
|
||||||
@@ -104,8 +112,12 @@ impl FrameSink for F722MavlinkTransmitter {
|
|||||||
|
|
||||||
fn on_dma_complete(&mut self, completion: TxCompletion) {
|
fn on_dma_complete(&mut self, completion: TxCompletion) {
|
||||||
match self {
|
match self {
|
||||||
Self::Usart1(transmitter) => transmitter.on_dma_complete(completion),
|
Self::Usart1(transmitter) => {
|
||||||
Self::Usart2(transmitter) => transmitter.on_dma_complete(completion),
|
let _ = transmitter.on_dma_complete(completion);
|
||||||
|
}
|
||||||
|
Self::Usart2(transmitter) => {
|
||||||
|
let _ = transmitter.on_dma_complete(completion);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -24,16 +24,15 @@ macro_rules! f722_flight_application {
|
|||||||
use core::sync::atomic::{AtomicBool, AtomicU8, Ordering};
|
use core::sync::atomic::{AtomicBool, AtomicU8, Ordering};
|
||||||
|
|
||||||
use $crate::f722::{
|
use $crate::f722::{
|
||||||
F722Imu, F722InitResources, F722LidarState, F722MavlinkState, F722Mtf01,
|
F722Imu, F722ImuTransferCompletion, F722InitResources, F722LidarState,
|
||||||
F722Mtf01Uart, F722ServiceTimer, acknowledge_exti3, acknowledge_exti4,
|
F722MavlinkState, F722Mtf01, F722Mtf01Uart, F722ServiceTimer,
|
||||||
|
acknowledge_exti3, acknowledge_exti4,
|
||||||
acknowledge_imu_rx_dma1, acknowledge_imu_rx_dma2, acknowledge_imu_spi1_error,
|
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_imu_spi3_error, acknowledge_imu_tx_dma1, acknowledge_imu_tx_dma2,
|
||||||
acknowledge_mavlink_dma1, acknowledge_mavlink_dma2, acknowledge_mavlink_rx_usart1,
|
acknowledge_mavlink_dma1, acknowledge_mavlink_dma2, acknowledge_mavlink_rx_usart1,
|
||||||
acknowledge_mavlink_rx_usart2, imu_is_healthy, initialize, on_uart4_interrupt,
|
acknowledge_mavlink_rx_usart2, initialize, on_uart4_interrupt, process_imu,
|
||||||
process_imu, record_mavlink_completion,
|
record_mavlink_completion,
|
||||||
};
|
};
|
||||||
use $crate::actuator::F722MotorOutput;
|
|
||||||
use $crate::__platform_internal::spi::SpiCompletion;
|
|
||||||
#[allow(unused_imports)]
|
#[allow(unused_imports)]
|
||||||
use $crate::__platform_internal::rtic_app::Mutex;
|
use $crate::__platform_internal::rtic_app::Mutex;
|
||||||
|
|
||||||
@@ -44,7 +43,6 @@ macro_rules! f722_flight_application {
|
|||||||
const IMU_EVENT_START: u8 = 1 << 4;
|
const IMU_EVENT_START: u8 = 1 << 4;
|
||||||
const MAVLINK_EVENT_TIMER: u8 = 1 << 0;
|
const MAVLINK_EVENT_TIMER: u8 = 1 << 0;
|
||||||
const MAVLINK_EVENT_DMA: u8 = 1 << 2;
|
const MAVLINK_EVENT_DMA: u8 = 1 << 2;
|
||||||
const MAVLINK_EVENT_RECEIVED: u8 = 1 << 3;
|
|
||||||
|
|
||||||
// Hardware IRQs and both software workers coalesce event bits.
|
// Hardware IRQs and both software workers coalesce event bits.
|
||||||
// Release/Acquire pairs make each terminal acknowledgement visible
|
// Release/Acquire pairs make each terminal acknowledgement visible
|
||||||
@@ -58,7 +56,6 @@ macro_rules! f722_flight_application {
|
|||||||
#[shared]
|
#[shared]
|
||||||
struct Shared {
|
struct Shared {
|
||||||
mavlink: F722MavlinkState,
|
mavlink: F722MavlinkState,
|
||||||
motors: F722MotorOutput,
|
|
||||||
lidar: F722LidarState,
|
lidar: F722LidarState,
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -80,14 +77,12 @@ macro_rules! f722_flight_application {
|
|||||||
timer,
|
timer,
|
||||||
lidar,
|
lidar,
|
||||||
mavlink,
|
mavlink,
|
||||||
motors,
|
|
||||||
} = initialize(context.core, context.device, $config(), routes)
|
} = initialize(context.core, context.device, $config(), routes)
|
||||||
.unwrap_or_else(|_| safe_idle());
|
.unwrap_or_else(|_| safe_idle());
|
||||||
schedule_imu(IMU_EVENT_START);
|
schedule_imu(IMU_EVENT_START);
|
||||||
(
|
(
|
||||||
Shared {
|
Shared {
|
||||||
mavlink,
|
mavlink,
|
||||||
motors,
|
|
||||||
lidar,
|
lidar,
|
||||||
},
|
},
|
||||||
Local {
|
Local {
|
||||||
@@ -123,15 +118,15 @@ macro_rules! f722_flight_application {
|
|||||||
#[task(binds = DMA2_STREAM0, priority = 14)]
|
#[task(binds = DMA2_STREAM0, priority = 14)]
|
||||||
fn imu_rx_dma2(_: imu_rx_dma2::Context) {
|
fn imu_rx_dma2(_: imu_rx_dma2::Context) {
|
||||||
match acknowledge_imu_rx_dma2() {
|
match acknowledge_imu_rx_dma2() {
|
||||||
SpiCompletion::Complete => schedule_imu(IMU_EVENT_TRANSFER_COMPLETE),
|
F722ImuTransferCompletion::Complete => schedule_imu(IMU_EVENT_TRANSFER_COMPLETE),
|
||||||
SpiCompletion::TransferFault => schedule_imu(IMU_EVENT_TRANSFER_FAULT),
|
F722ImuTransferCompletion::TransferFault => schedule_imu(IMU_EVENT_TRANSFER_FAULT),
|
||||||
SpiCompletion::NotPending => {}
|
F722ImuTransferCompletion::NotPending => {}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#[task(binds = DMA2_STREAM3, priority = 14)]
|
#[task(binds = DMA2_STREAM3, priority = 14)]
|
||||||
fn imu_tx_dma2(_: imu_tx_dma2::Context) {
|
fn imu_tx_dma2(_: imu_tx_dma2::Context) {
|
||||||
if acknowledge_imu_tx_dma2() == SpiCompletion::TransferFault {
|
if acknowledge_imu_tx_dma2() == F722ImuTransferCompletion::TransferFault {
|
||||||
schedule_imu(IMU_EVENT_TRANSFER_FAULT);
|
schedule_imu(IMU_EVENT_TRANSFER_FAULT);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -146,15 +141,15 @@ macro_rules! f722_flight_application {
|
|||||||
#[task(binds = DMA1_STREAM0, priority = 14)]
|
#[task(binds = DMA1_STREAM0, priority = 14)]
|
||||||
fn imu_rx_dma1(_: imu_rx_dma1::Context) {
|
fn imu_rx_dma1(_: imu_rx_dma1::Context) {
|
||||||
match acknowledge_imu_rx_dma1() {
|
match acknowledge_imu_rx_dma1() {
|
||||||
SpiCompletion::Complete => schedule_imu(IMU_EVENT_TRANSFER_COMPLETE),
|
F722ImuTransferCompletion::Complete => schedule_imu(IMU_EVENT_TRANSFER_COMPLETE),
|
||||||
SpiCompletion::TransferFault => schedule_imu(IMU_EVENT_TRANSFER_FAULT),
|
F722ImuTransferCompletion::TransferFault => schedule_imu(IMU_EVENT_TRANSFER_FAULT),
|
||||||
SpiCompletion::NotPending => {}
|
F722ImuTransferCompletion::NotPending => {}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#[task(binds = DMA1_STREAM5, priority = 14)]
|
#[task(binds = DMA1_STREAM5, priority = 14)]
|
||||||
fn imu_tx_dma1(_: imu_tx_dma1::Context) {
|
fn imu_tx_dma1(_: imu_tx_dma1::Context) {
|
||||||
if acknowledge_imu_tx_dma1() == SpiCompletion::TransferFault {
|
if acknowledge_imu_tx_dma1() == F722ImuTransferCompletion::TransferFault {
|
||||||
schedule_imu(IMU_EVENT_TRANSFER_FAULT);
|
schedule_imu(IMU_EVENT_TRANSFER_FAULT);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -171,16 +166,6 @@ macro_rules! f722_flight_application {
|
|||||||
on_uart4_interrupt(context.local.mtf01);
|
on_uart4_interrupt(context.local.mtf01);
|
||||||
}
|
}
|
||||||
|
|
||||||
#[task(binds = TIM3, priority = 12, shared = [motors])]
|
|
||||||
fn motor_update(mut context: motor_update::Context) {
|
|
||||||
context.shared.motors.lock(F722MotorOutput::on_tim3_interrupt);
|
|
||||||
}
|
|
||||||
|
|
||||||
#[task(binds = TIM5, priority = 12, shared = [motors])]
|
|
||||||
fn motor_scheduler(mut context: motor_scheduler::Context) {
|
|
||||||
context.shared.motors.lock(F722MotorOutput::on_tim5_interrupt);
|
|
||||||
}
|
|
||||||
|
|
||||||
#[task(binds = I2C1_EV, priority = 11, shared = [lidar])]
|
#[task(binds = I2C1_EV, priority = 11, shared = [lidar])]
|
||||||
fn i2c1_event(mut context: i2c1_event::Context) {
|
fn i2c1_event(mut context: i2c1_event::Context) {
|
||||||
context
|
context
|
||||||
@@ -215,26 +200,20 @@ macro_rules! f722_flight_application {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#[task(binds = USART1, priority = 8, shared = [mavlink])]
|
#[task(binds = USART1, priority = 8)]
|
||||||
fn mavlink_rx_usart1(mut context: mavlink_rx_usart1::Context) {
|
fn mavlink_rx_usart1(_: mavlink_rx_usart1::Context) {
|
||||||
let admitted = context
|
// `ExternalControlOnly` is rejected and production signing is
|
||||||
.shared
|
// not implemented, so this image has no inbound authority
|
||||||
.mavlink
|
// path. Acknowledge every RX condition to keep the peripheral
|
||||||
.lock(|mavlink| mavlink.on_mavlink_byte(acknowledge_mavlink_rx_usart1()));
|
// quiescent without taking any RTIC-shared flight state.
|
||||||
if admitted {
|
let _ = acknowledge_mavlink_rx_usart1();
|
||||||
schedule_mavlink(MAVLINK_EVENT_RECEIVED);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#[task(binds = USART2, priority = 8, shared = [mavlink])]
|
#[task(binds = USART2, priority = 8)]
|
||||||
fn mavlink_rx_usart2(mut context: mavlink_rx_usart2::Context) {
|
fn mavlink_rx_usart2(_: mavlink_rx_usart2::Context) {
|
||||||
let admitted = context
|
// See USART1: inbound bytes are drained but never decoded or
|
||||||
.shared
|
// dispatched by the telemetry-only image.
|
||||||
.mavlink
|
let _ = acknowledge_mavlink_rx_usart2();
|
||||||
.lock(|mavlink| mavlink.on_mavlink_byte(acknowledge_mavlink_rx_usart2()));
|
|
||||||
if admitted {
|
|
||||||
schedule_mavlink(MAVLINK_EVENT_RECEIVED);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#[task(binds = TIM2, priority = 4, local = [timer], shared = [lidar])]
|
#[task(binds = TIM2, priority = 4, local = [timer], shared = [lidar])]
|
||||||
@@ -258,38 +237,13 @@ macro_rules! f722_flight_application {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#[task(priority = 9, shared = [mavlink, motors])]
|
#[task(priority = 9, shared = [mavlink])]
|
||||||
async fn mavlink_worker(mut context: mavlink_worker::Context) {
|
async fn mavlink_worker(mut context: mavlink_worker::Context) {
|
||||||
let events = MAVLINK_EVENTS.swap(0, Ordering::AcqRel);
|
let events = MAVLINK_EVENTS.swap(0, Ordering::AcqRel);
|
||||||
let motors_armed = context.shared.motors.lock(|motors| motors.is_armed());
|
|
||||||
context
|
context
|
||||||
.shared
|
.shared
|
||||||
.mavlink
|
.mavlink
|
||||||
.lock(|mavlink| mavlink.process_worker_events(events, motors_armed));
|
.lock(|mavlink| mavlink.process_worker_events(events));
|
||||||
if events & MAVLINK_EVENT_RECEIVED != 0 {
|
|
||||||
while let Some(request) =
|
|
||||||
context.shared.mavlink.lock(F722MavlinkState::next_arm_request)
|
|
||||||
{
|
|
||||||
let result = match request {
|
|
||||||
$crate::__platform_internal::mavlink::commands::arm::ArmRequest::Arm if !imu_is_healthy() =>
|
|
||||||
{
|
|
||||||
$crate::__platform_internal::mavlink::message::MavResult::MAV_RESULT_DENIED
|
|
||||||
}
|
|
||||||
$crate::__platform_internal::mavlink::commands::arm::ArmRequest::Arm => {
|
|
||||||
context.shared.motors.lock(F722MotorOutput::arm);
|
|
||||||
$crate::__platform_internal::mavlink::message::MavResult::MAV_RESULT_ACCEPTED
|
|
||||||
}
|
|
||||||
$crate::__platform_internal::mavlink::commands::arm::ArmRequest::Disarm => {
|
|
||||||
context.shared.motors.lock(F722MotorOutput::disarm);
|
|
||||||
$crate::__platform_internal::mavlink::message::MavResult::MAV_RESULT_ACCEPTED
|
|
||||||
}
|
|
||||||
};
|
|
||||||
context
|
|
||||||
.shared
|
|
||||||
.mavlink
|
|
||||||
.lock(|mavlink| mavlink.acknowledge_arm_request(result));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
MAVLINK_QUEUED.store(false, Ordering::Release);
|
MAVLINK_QUEUED.store(false, Ordering::Release);
|
||||||
if MAVLINK_EVENTS.load(Ordering::Acquire) != 0
|
if MAVLINK_EVENTS.load(Ordering::Acquire) != 0
|
||||||
&& !MAVLINK_QUEUED.swap(true, Ordering::AcqRel)
|
&& !MAVLINK_QUEUED.swap(true, Ordering::AcqRel)
|
||||||
|
|||||||
@@ -2,11 +2,12 @@
|
|||||||
|
|
||||||
use core::sync::atomic::{AtomicU8, Ordering};
|
use core::sync::atomic::{AtomicU8, Ordering};
|
||||||
|
|
||||||
use super::peripherals::{
|
use super::peripherals::{Spi1, Spi3, Usart2Tx, Usart2TxDma};
|
||||||
ReceiveInterrupt, Spi1, Spi3, Usart1Tx, Usart1TxDma, Usart2Tx, Usart2TxDma,
|
use super::{
|
||||||
|
F722ImuTransferCompletion,
|
||||||
|
peripherals::{ReceiveInterrupt, Usart1Tx, Usart1TxDma},
|
||||||
};
|
};
|
||||||
use kaptan_dma::TxCompletion;
|
use kaptan_dma::TxCompletion;
|
||||||
use kaptan_spi::SpiCompletion;
|
|
||||||
use stm32f7::stm32f722;
|
use stm32f7::stm32f722;
|
||||||
|
|
||||||
use crate::F722Profile;
|
use crate::F722Profile;
|
||||||
@@ -59,40 +60,36 @@ pub fn acknowledge_exti3() -> bool {
|
|||||||
true
|
true
|
||||||
}
|
}
|
||||||
|
|
||||||
pub fn acknowledge_imu_rx_dma2() -> SpiCompletion {
|
pub fn acknowledge_imu_rx_dma2() -> F722ImuTransferCompletion {
|
||||||
if active_profile() == PROFILE_MPU6000_SPI1_USART1 {
|
if active_profile() == PROFILE_MPU6000_SPI1_USART1 {
|
||||||
Spi1::acknowledge_rx_interrupt()
|
return Spi1::acknowledge_rx_interrupt().into();
|
||||||
} else {
|
|
||||||
SpiCompletion::NotPending
|
|
||||||
}
|
}
|
||||||
|
F722ImuTransferCompletion::NotPending
|
||||||
}
|
}
|
||||||
|
|
||||||
pub fn acknowledge_imu_tx_dma2() -> SpiCompletion {
|
pub fn acknowledge_imu_tx_dma2() -> F722ImuTransferCompletion {
|
||||||
if active_profile() == PROFILE_MPU6000_SPI1_USART1 {
|
if active_profile() == PROFILE_MPU6000_SPI1_USART1 {
|
||||||
Spi1::acknowledge_tx_interrupt()
|
return Spi1::acknowledge_tx_interrupt().into();
|
||||||
} else {
|
|
||||||
SpiCompletion::NotPending
|
|
||||||
}
|
}
|
||||||
|
F722ImuTransferCompletion::NotPending
|
||||||
}
|
}
|
||||||
|
|
||||||
pub fn acknowledge_imu_spi1_error() -> bool {
|
pub fn acknowledge_imu_spi1_error() -> bool {
|
||||||
active_profile() == PROFILE_MPU6000_SPI1_USART1 && Spi1::acknowledge_error_interrupt()
|
active_profile() == PROFILE_MPU6000_SPI1_USART1 && Spi1::acknowledge_error_interrupt()
|
||||||
}
|
}
|
||||||
|
|
||||||
pub fn acknowledge_imu_rx_dma1() -> SpiCompletion {
|
pub fn acknowledge_imu_rx_dma1() -> F722ImuTransferCompletion {
|
||||||
if active_profile() == PROFILE_ICM42688_SPI3_USART2 {
|
if active_profile() == PROFILE_ICM42688_SPI3_USART2 {
|
||||||
Spi3::acknowledge_rx_interrupt()
|
return Spi3::acknowledge_rx_interrupt().into();
|
||||||
} else {
|
|
||||||
SpiCompletion::NotPending
|
|
||||||
}
|
}
|
||||||
|
F722ImuTransferCompletion::NotPending
|
||||||
}
|
}
|
||||||
|
|
||||||
pub fn acknowledge_imu_tx_dma1() -> SpiCompletion {
|
pub fn acknowledge_imu_tx_dma1() -> F722ImuTransferCompletion {
|
||||||
if active_profile() == PROFILE_ICM42688_SPI3_USART2 {
|
if active_profile() == PROFILE_ICM42688_SPI3_USART2 {
|
||||||
Spi3::acknowledge_tx_interrupt()
|
return Spi3::acknowledge_tx_interrupt().into();
|
||||||
} else {
|
|
||||||
SpiCompletion::NotPending
|
|
||||||
}
|
}
|
||||||
|
F722ImuTransferCompletion::NotPending
|
||||||
}
|
}
|
||||||
|
|
||||||
pub fn acknowledge_imu_spi3_error() -> bool {
|
pub fn acknowledge_imu_spi3_error() -> bool {
|
||||||
@@ -109,10 +106,9 @@ pub fn acknowledge_mavlink_dma2() -> TxCompletion {
|
|||||||
|
|
||||||
pub fn acknowledge_mavlink_dma1() -> TxCompletion {
|
pub fn acknowledge_mavlink_dma1() -> TxCompletion {
|
||||||
if active_profile() == PROFILE_ICM42688_SPI3_USART2 {
|
if active_profile() == PROFILE_ICM42688_SPI3_USART2 {
|
||||||
Usart2TxDma::acknowledge_interrupt()
|
return Usart2TxDma::acknowledge_interrupt();
|
||||||
} else {
|
|
||||||
TxCompletion::NotPending
|
|
||||||
}
|
}
|
||||||
|
TxCompletion::NotPending
|
||||||
}
|
}
|
||||||
|
|
||||||
pub fn acknowledge_mavlink_rx_usart1() -> ReceiveInterrupt {
|
pub fn acknowledge_mavlink_rx_usart1() -> ReceiveInterrupt {
|
||||||
@@ -125,8 +121,7 @@ pub fn acknowledge_mavlink_rx_usart1() -> ReceiveInterrupt {
|
|||||||
|
|
||||||
pub fn acknowledge_mavlink_rx_usart2() -> ReceiveInterrupt {
|
pub fn acknowledge_mavlink_rx_usart2() -> ReceiveInterrupt {
|
||||||
if active_profile() == PROFILE_ICM42688_SPI3_USART2 {
|
if active_profile() == PROFILE_ICM42688_SPI3_USART2 {
|
||||||
Usart2Tx::acknowledge_receive_interrupt()
|
return Usart2Tx::acknowledge_receive_interrupt();
|
||||||
} else {
|
|
||||||
ReceiveInterrupt::NotPending
|
|
||||||
}
|
}
|
||||||
|
ReceiveInterrupt::NotPending
|
||||||
}
|
}
|
||||||
@@ -10,6 +10,7 @@ mod clock;
|
|||||||
mod data_channel;
|
mod data_channel;
|
||||||
mod firmware;
|
mod firmware;
|
||||||
mod interrupts;
|
mod interrupts;
|
||||||
|
mod motor_inhibit;
|
||||||
mod peripherals;
|
mod peripherals;
|
||||||
mod routes;
|
mod routes;
|
||||||
mod sensors;
|
mod sensors;
|
||||||
@@ -17,6 +18,24 @@ mod sensors;
|
|||||||
use crate::{BoardError, F722BoardRoutes};
|
use crate::{BoardError, F722BoardRoutes};
|
||||||
use kaptan_board::BoardConfig;
|
use kaptan_board::BoardConfig;
|
||||||
|
|
||||||
|
/// Completion classification crossing the RTIC interrupt template.
|
||||||
|
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||||
|
pub(crate) enum F722ImuTransferCompletion {
|
||||||
|
Complete,
|
||||||
|
TransferFault,
|
||||||
|
NotPending,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl From<kaptan_spi::SpiCompletion> for F722ImuTransferCompletion {
|
||||||
|
fn from(value: kaptan_spi::SpiCompletion) -> Self {
|
||||||
|
match value {
|
||||||
|
kaptan_spi::SpiCompletion::Complete => Self::Complete,
|
||||||
|
kaptan_spi::SpiCompletion::TransferFault => Self::TransferFault,
|
||||||
|
kaptan_spi::SpiCompletion::NotPending => Self::NotPending,
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||||
pub(crate) enum VerticalRoute {
|
pub(crate) enum VerticalRoute {
|
||||||
None,
|
None,
|
||||||
@@ -24,20 +43,9 @@ pub(crate) enum VerticalRoute {
|
|||||||
LidarLiteV3,
|
LidarLiteV3,
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Logical-M1–M4 ordering for the fixed TIM3 physical channels.
|
|
||||||
///
|
|
||||||
/// Both supported boards use PB4/PB5/PB0/PB1 electrically, but their ESC
|
|
||||||
/// connectors assign those pins to logical motors differently. This remains a
|
|
||||||
/// platform-private route choice rather than a board-side execution detail.
|
|
||||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
|
||||||
pub(crate) enum MotorRoute {
|
|
||||||
KakuteTim3Tim5,
|
|
||||||
GeprcTim3Tim5,
|
|
||||||
}
|
|
||||||
|
|
||||||
pub(crate) use board::{
|
pub(crate) use board::{
|
||||||
F722InitResources, F722LidarState, F722MavlinkState, F722Mtf01, F722Mtf01Uart,
|
F722InitResources, F722LidarState, F722MavlinkState, F722Mtf01, F722Mtf01Uart,
|
||||||
F722ServiceTimer, imu_is_healthy, on_uart4_interrupt, process_imu, record_mavlink_completion,
|
F722ServiceTimer, on_uart4_interrupt, process_imu, record_mavlink_completion,
|
||||||
};
|
};
|
||||||
pub(crate) use interrupts::{
|
pub(crate) use interrupts::{
|
||||||
acknowledge_exti3, acknowledge_exti4, acknowledge_imu_rx_dma1, acknowledge_imu_rx_dma2,
|
acknowledge_exti3, acknowledge_exti4, acknowledge_imu_rx_dma1, acknowledge_imu_rx_dma2,
|
||||||
@@ -56,6 +64,10 @@ pub fn initialize(
|
|||||||
config: BoardConfig,
|
config: BoardConfig,
|
||||||
routes: F722BoardRoutes,
|
routes: F722BoardRoutes,
|
||||||
) -> Result<F722InitResources, BoardError> {
|
) -> Result<F722InitResources, BoardError> {
|
||||||
|
// This is deliberately the first hardware access, before configuration
|
||||||
|
// validation, clocks, sensor routes, interrupts, or UART setup. It keeps
|
||||||
|
// the former four motor pins low even for malformed configuration.
|
||||||
|
motor_inhibit::inhibit_motor_pins(&device.RCC, &device.GPIOB);
|
||||||
config.validate().map_err(BoardError::InvalidConfig)?;
|
config.validate().map_err(BoardError::InvalidConfig)?;
|
||||||
board::validate_vertical_route(config, routes)?;
|
board::validate_vertical_route(config, routes)?;
|
||||||
board::initialize(core, device, config, routes)
|
board::initialize(core, device, config, routes)
|
||||||
|
|||||||
@@ -0,0 +1,67 @@
|
|||||||
|
//! Common former-motor-pin interlock for every F722 image.
|
||||||
|
//!
|
||||||
|
//! This is intentionally independent of profile initialization and is called
|
||||||
|
//! as the first PAC access. The GPIO-low interlock is a separate electrical
|
||||||
|
//! boundary from all telemetry initialization.
|
||||||
|
|
||||||
|
use stm32f7::stm32f722;
|
||||||
|
|
||||||
|
/// Drives PB0/PB1/PB4/PB5 low before selecting push-pull GPIO output mode.
|
||||||
|
pub(crate) fn inhibit_motor_pins(rcc: &stm32f722::RCC, port: &stm32f722::GPIOB) {
|
||||||
|
rcc.ahb1enr().modify(|_, write| write.gpioben().set_bit());
|
||||||
|
port.bsrr().write(|write| {
|
||||||
|
write
|
||||||
|
.br0()
|
||||||
|
.set_bit()
|
||||||
|
.br1()
|
||||||
|
.set_bit()
|
||||||
|
.br4()
|
||||||
|
.set_bit()
|
||||||
|
.br5()
|
||||||
|
.set_bit()
|
||||||
|
});
|
||||||
|
port.otyper().modify(|_, write| {
|
||||||
|
write
|
||||||
|
.ot0()
|
||||||
|
.push_pull()
|
||||||
|
.ot1()
|
||||||
|
.push_pull()
|
||||||
|
.ot4()
|
||||||
|
.push_pull()
|
||||||
|
.ot5()
|
||||||
|
.push_pull()
|
||||||
|
});
|
||||||
|
port.ospeedr().modify(|_, write| {
|
||||||
|
write
|
||||||
|
.ospeedr0()
|
||||||
|
.low_speed()
|
||||||
|
.ospeedr1()
|
||||||
|
.low_speed()
|
||||||
|
.ospeedr4()
|
||||||
|
.low_speed()
|
||||||
|
.ospeedr5()
|
||||||
|
.low_speed()
|
||||||
|
});
|
||||||
|
port.pupdr().modify(|_, write| {
|
||||||
|
write
|
||||||
|
.pupdr0()
|
||||||
|
.floating()
|
||||||
|
.pupdr1()
|
||||||
|
.floating()
|
||||||
|
.pupdr4()
|
||||||
|
.floating()
|
||||||
|
.pupdr5()
|
||||||
|
.floating()
|
||||||
|
});
|
||||||
|
port.moder().modify(|_, write| {
|
||||||
|
write
|
||||||
|
.moder0()
|
||||||
|
.output()
|
||||||
|
.moder1()
|
||||||
|
.output()
|
||||||
|
.moder4()
|
||||||
|
.output()
|
||||||
|
.moder5()
|
||||||
|
.output()
|
||||||
|
});
|
||||||
|
}
|
||||||
@@ -152,7 +152,11 @@ pub fn configure_usart1_rx_pin_pa10(port: &stm32f722::GPIOA) {
|
|||||||
// SAFETY: AF7 selects USART1 RX on PA10 on STM32F722.
|
// SAFETY: AF7 selects USART1 RX on PA10 on STM32F722.
|
||||||
unsafe { write.afrh10().bits(0b0111) }
|
unsafe { write.afrh10().bits(0b0111) }
|
||||||
});
|
});
|
||||||
port.pupdr().modify(|_, write| write.pupdr10().floating());
|
// USART is idle-high. Leaving PA10 floating before its peer drives the
|
||||||
|
// line can manufacture framing and noise errors. The weak internal pull-up
|
||||||
|
// supplies only the electrically correct idle level; it does not mask a
|
||||||
|
// driven low, framing, noise, or overrun fault.
|
||||||
|
port.pupdr().modify(|_, write| write.pupdr10().pull_up());
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Configures PA2/PA3 for USART2 transmit/receive alternate function 7.
|
/// Configures PA2/PA3 for USART2 transmit/receive alternate function 7.
|
||||||
|
|||||||
@@ -114,14 +114,28 @@ impl Spi1 {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// Releases controller-side ownership after a DMA terminal event.
|
/// Releases controller-side ownership after a DMA terminal event.
|
||||||
|
///
|
||||||
|
/// Both streams are explicitly disabled for every outcome, including an
|
||||||
|
/// SPI peripheral fault. Disabling the SPI DMA requests first prevents a
|
||||||
|
/// new request while the stream hardware drains its enable bit. This
|
||||||
|
/// method deliberately does not wait for that bit: the next submission
|
||||||
|
/// observes it and returns `Busy` until the F7 DMA controller completes
|
||||||
|
/// the documented disable sequence.
|
||||||
pub fn complete_transfer(&mut self, _: SpiCompletion) {
|
pub fn complete_transfer(&mut self, _: SpiCompletion) {
|
||||||
// TODO: Abort both DMA streams on peripheral faults.
|
|
||||||
self.peripheral
|
self.peripheral
|
||||||
.cr2()
|
.cr2()
|
||||||
.modify(|_, write| write.rxdmaen().clear_bit().txdmaen().clear_bit());
|
.modify(|_, write| write.rxdmaen().clear_bit().txdmaen().clear_bit());
|
||||||
self.peripheral
|
self.peripheral
|
||||||
.cr1()
|
.cr1()
|
||||||
.modify(|_, write| write.spe().clear_bit());
|
.modify(|_, write| write.spe().clear_bit());
|
||||||
|
self.rx_stream()
|
||||||
|
.cr()
|
||||||
|
.modify(|_, write| write.en().clear_bit());
|
||||||
|
self.tx_stream()
|
||||||
|
.cr()
|
||||||
|
.modify(|_, write| write.en().clear_bit());
|
||||||
|
self.clear_rx_flags();
|
||||||
|
self.clear_tx_flags();
|
||||||
self.active = false;
|
self.active = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -27,10 +27,10 @@ pub struct Spi3 {
|
|||||||
}
|
}
|
||||||
|
|
||||||
impl Spi3 {
|
impl Spi3 {
|
||||||
/// Configures SPI3 mode 0 master operation at APB2/128 and reserves DMA1.
|
/// Configures SPI3 mode 0 master operation at APB1/128 and reserves DMA1.
|
||||||
///
|
///
|
||||||
/// This keeps the ICM-42688-P route below 1 MHz with the board's 108 MHz
|
/// This keeps the ICM-42688-P route below 1 MHz with the board's 54 MHz
|
||||||
/// APB2 clock while retaining substantial margin for board-level wiring.
|
/// APB1 clock while retaining substantial margin for board-level wiring.
|
||||||
pub fn new(
|
pub fn new(
|
||||||
rcc: &stm32f722::RCC,
|
rcc: &stm32f722::RCC,
|
||||||
peripheral: stm32f722::SPI3,
|
peripheral: stm32f722::SPI3,
|
||||||
@@ -114,14 +114,28 @@ impl Spi3 {
|
|||||||
}
|
}
|
||||||
|
|
||||||
/// Releases controller-side ownership after a DMA terminal event.
|
/// Releases controller-side ownership after a DMA terminal event.
|
||||||
|
///
|
||||||
|
/// Both streams are explicitly disabled for every outcome, including an
|
||||||
|
/// SPI peripheral fault. Disabling the SPI DMA requests first prevents a
|
||||||
|
/// new request while the stream hardware drains its enable bit. This
|
||||||
|
/// method deliberately does not wait for that bit: the next submission
|
||||||
|
/// observes it and returns `Busy` until the F7 DMA controller completes
|
||||||
|
/// the documented disable sequence.
|
||||||
pub fn complete_transfer(&mut self, _: SpiCompletion) {
|
pub fn complete_transfer(&mut self, _: SpiCompletion) {
|
||||||
// TODO: Abort both DMA streams on peripheral faults.
|
|
||||||
self.peripheral
|
self.peripheral
|
||||||
.cr2()
|
.cr2()
|
||||||
.modify(|_, write| write.rxdmaen().clear_bit().txdmaen().clear_bit());
|
.modify(|_, write| write.rxdmaen().clear_bit().txdmaen().clear_bit());
|
||||||
self.peripheral
|
self.peripheral
|
||||||
.cr1()
|
.cr1()
|
||||||
.modify(|_, write| write.spe().clear_bit());
|
.modify(|_, write| write.spe().clear_bit());
|
||||||
|
self.rx_stream()
|
||||||
|
.cr()
|
||||||
|
.modify(|_, write| write.en().clear_bit());
|
||||||
|
self.tx_stream()
|
||||||
|
.cr()
|
||||||
|
.modify(|_, write| write.en().clear_bit());
|
||||||
|
self.clear_rx_flags();
|
||||||
|
self.clear_tx_flags();
|
||||||
self.active = false;
|
self.active = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -6,7 +6,7 @@ use kaptan_time::{MonotonicClock, Timestamp};
|
|||||||
/// A DWT cycle counter extended into monotonic microseconds.
|
/// A DWT cycle counter extended into monotonic microseconds.
|
||||||
///
|
///
|
||||||
/// `now` must be called at least once during every 32-bit cycle-counter wrap
|
/// `now` must be called at least once during every 32-bit cycle-counter wrap
|
||||||
/// interval. At the Kakute reset clock this is approximately 268 seconds; the
|
/// interval. At the Kakute reset clock this is approximately 20 seconds; the
|
||||||
/// platform periodic service calls it much more frequently.
|
/// platform periodic service calls it much more frequently.
|
||||||
pub struct DwtMicrosecondClock {
|
pub struct DwtMicrosecondClock {
|
||||||
cycles_per_microsecond: u32,
|
cycles_per_microsecond: u32,
|
||||||
|
|||||||
@@ -104,6 +104,10 @@ impl Usart1Tx {
|
|||||||
) -> Result<Self, BaudRateError> {
|
) -> Result<Self, BaudRateError> {
|
||||||
let brr = calculate_brr(peripheral_clock_hz, baud)?;
|
let brr = calculate_brr(peripheral_clock_hz, baud)?;
|
||||||
|
|
||||||
|
// Do not inherit a bootloader or earlier image's USART1 kernel-clock
|
||||||
|
// selection. The F722 platform passes PCLK2 when calculating BRR, so
|
||||||
|
// select it explicitly before programming the divisor.
|
||||||
|
rcc.dckcfgr2().modify(|_, write| write.usart1sel().apb2());
|
||||||
rcc.apb2rstr()
|
rcc.apb2rstr()
|
||||||
.modify(|_, write| write.usart1rst().set_bit());
|
.modify(|_, write| write.usart1rst().set_bit());
|
||||||
rcc.apb2rstr()
|
rcc.apb2rstr()
|
||||||
|
|||||||
@@ -9,6 +9,11 @@ use stm32f7::stm32f722;
|
|||||||
|
|
||||||
use super::Usart1Tx;
|
use super::Usart1Tx;
|
||||||
|
|
||||||
|
// The F7 DMA controller can expose TCIF a few bus cycles before CR.EN reads
|
||||||
|
// low. This bounded IRQ-side settling observation never spins indefinitely;
|
||||||
|
// a later service turn can retry a start if the stream is not yet available.
|
||||||
|
const DISABLE_POLL_LIMIT: u8 = 32;
|
||||||
|
|
||||||
/// Owns the fixed STM32F722 USART1 TX DMA route.
|
/// Owns the fixed STM32F722 USART1 TX DMA route.
|
||||||
pub struct Usart1TxDma {
|
pub struct Usart1TxDma {
|
||||||
usart: Usart1Tx,
|
usart: Usart1Tx,
|
||||||
@@ -73,12 +78,19 @@ impl Usart1TxDma {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// A normal-mode STM32 DMA stream clears EN at terminal completion.
|
// A normal-mode STM32 DMA stream clears EN at terminal completion.
|
||||||
// Request disable without polling: a later start observes EN and
|
// Request disable and observe that transition for a small fixed bound;
|
||||||
// refuses ownership rather than blocking an interrupt handler.
|
// no unbounded wait is permitted in an IRQ. If the bound expires before
|
||||||
|
// EN becomes visible as low, a later service turn observes `Busy` and
|
||||||
|
// retries without treating this documented hardware latency as a fault.
|
||||||
controller
|
controller
|
||||||
.st(7)
|
.st(7)
|
||||||
.cr()
|
.cr()
|
||||||
.modify(|_, write| write.en().disabled());
|
.modify(|_, write| write.en().disabled());
|
||||||
|
for _ in 0..DISABLE_POLL_LIMIT {
|
||||||
|
if !controller.st(7).cr().read().en().is_enabled() {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
controller.hifcr().write(|write| {
|
controller.hifcr().write(|write| {
|
||||||
write
|
write
|
||||||
.ctcif7()
|
.ctcif7()
|
||||||
@@ -165,8 +177,8 @@ impl Usart1TxDma {
|
|||||||
}
|
}
|
||||||
|
|
||||||
fn controller(&self) -> &stm32f722::dma2::RegisterBlock {
|
fn controller(&self) -> &stm32f722::dma2::RegisterBlock {
|
||||||
// SAFETY: SPI1 owns DMA2 initialization; USART1 TX is permanently
|
// SAFETY: USART1 TX permanently owns this disjoint Stream7 route and
|
||||||
// assigned to the disjoint Stream7 route and accesses no other stream.
|
// accesses no other DMA2 stream.
|
||||||
unsafe { &*stm32f722::DMA2::ptr() }
|
unsafe { &*stm32f722::DMA2::ptr() }
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -14,6 +14,12 @@ use stm32f7::stm32f722;
|
|||||||
|
|
||||||
use super::routes::{Icm42688Spi3Route, Mpu6000Spi1Route};
|
use super::routes::{Icm42688Spi3Route, Mpu6000Spi1Route};
|
||||||
|
|
||||||
|
// TODO(configuration): this is an immutable board-development value, not an
|
||||||
|
// airframe calibration. A ground configuration flow must stage the complete
|
||||||
|
// IMU profile (device settings, mounting transform, bias/scale calibration,
|
||||||
|
// noise model, source/revision) while output is inhibited, then reset the
|
||||||
|
// estimator/controller/allocation state before activation. Never apply one
|
||||||
|
// register/calibration field to a live flight runtime.
|
||||||
const MPU6000_CONFIG: Mpu6000Config = Mpu6000Config {
|
const MPU6000_CONFIG: Mpu6000Config = Mpu6000Config {
|
||||||
accelerometer: AccelerometerRange::PlusMinus4G,
|
accelerometer: AccelerometerRange::PlusMinus4G,
|
||||||
gyroscope: GyroscopeRange::PlusMinus500DegreesPerSecond,
|
gyroscope: GyroscopeRange::PlusMinus500DegreesPerSecond,
|
||||||
@@ -131,8 +137,12 @@ fn icm_event<E>(event: Icm42688ServiceEvent<E>) -> ImuEvent {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Aligns the verified GEP-F722-HD V2 ICM-42688-P yaw-180° mounting to the
|
/// Aligns the development GEP-F722-HD V2 ICM-42688-P yaw-180° mounting to the
|
||||||
/// flight-controller body frame. The MPU6000 profile is already aligned.
|
/// flight-controller body frame. The MPU6000 profile is already aligned.
|
||||||
|
///
|
||||||
|
/// TODO(configuration): replace this build-time transform only as part of the
|
||||||
|
/// same atomic sensor-profile activation described at `MPU6000_CONFIG`; it is
|
||||||
|
/// not a user-adjustable live setting.
|
||||||
fn align_icm42688(mut sample: ImuSnapshot) -> ImuSnapshot {
|
fn align_icm42688(mut sample: ImuSnapshot) -> ImuSnapshot {
|
||||||
sample.acceleration_milligravity[0] = sample.acceleration_milligravity[0].saturating_neg();
|
sample.acceleration_milligravity[0] = sample.acceleration_milligravity[0].saturating_neg();
|
||||||
sample.acceleration_milligravity[1] = sample.acceleration_milligravity[1].saturating_neg();
|
sample.acceleration_milligravity[1] = sample.acceleration_milligravity[1].saturating_neg();
|
||||||
|
|||||||
@@ -14,7 +14,6 @@ use core::marker::PhantomData;
|
|||||||
pub use kaptan_board::BoardConfig;
|
pub use kaptan_board::BoardConfig;
|
||||||
use kaptan_board::BoardConfigError;
|
use kaptan_board::BoardConfigError;
|
||||||
|
|
||||||
mod actuator;
|
|
||||||
mod f722;
|
mod f722;
|
||||||
|
|
||||||
/// Dependencies used only while the platform expands its private RTIC macro.
|
/// Dependencies used only while the platform expands its private RTIC macro.
|
||||||
@@ -23,11 +22,12 @@ pub(crate) mod __platform_internal {
|
|||||||
pub(crate) use cortex_m as cm;
|
pub(crate) use cortex_m as cm;
|
||||||
pub(crate) use cortex_m_rt as cmrt;
|
pub(crate) use cortex_m_rt as cmrt;
|
||||||
pub(crate) use kaptan_dma as dma;
|
pub(crate) use kaptan_dma as dma;
|
||||||
pub(crate) use kaptan_mavlink as mavlink;
|
#[cfg(not(test))]
|
||||||
pub(crate) use kaptan_spi as spi;
|
|
||||||
pub(crate) use panic_halt as panic;
|
pub(crate) use panic_halt as panic;
|
||||||
pub(crate) use rtic as rtic_app;
|
pub(crate) use rtic as rtic_app;
|
||||||
pub(crate) use stm32f7 as pac;
|
pub(crate) use stm32f7 as pac;
|
||||||
|
#[cfg(test)]
|
||||||
|
pub(crate) mod panic {}
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Construction or validation failure for a declarative F722 board.
|
/// Construction or validation failure for a declarative F722 board.
|
||||||
@@ -103,48 +103,6 @@ pub struct Pa4;
|
|||||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||||
pub struct Pc3;
|
pub struct Pc3;
|
||||||
|
|
||||||
/// Motor-output component selector.
|
|
||||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
|
||||||
pub struct MotorOutputs;
|
|
||||||
/// Kakute TIM3/TIM5 motor route before selecting a waveform protocol.
|
|
||||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
|
||||||
pub struct KakuteTim3Tim5Motors;
|
|
||||||
/// GEPRC TIM3/TIM5 motor route before selecting a waveform protocol.
|
|
||||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
|
||||||
pub struct GeprcTim3Tim5Motors;
|
|
||||||
/// Validated Kakute TIM3/TIM5 DShot600 motor route.
|
|
||||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
|
||||||
pub struct KakuteTim3Tim5Dshot600;
|
|
||||||
/// Validated GEPRC TIM3/TIM5 DShot600 motor route.
|
|
||||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
|
||||||
pub struct GeprcTim3Tim5Dshot600;
|
|
||||||
|
|
||||||
impl MotorOutputs {
|
|
||||||
/// Selects Kakute's M1–M4 mapping: PB0, PB1, PB4, PB5.
|
|
||||||
pub const fn kakute_tim3_tim5() -> KakuteTim3Tim5Motors {
|
|
||||||
KakuteTim3Tim5Motors
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Selects GEPRC's M1–M4 mapping: PB4, PB5, PB0, PB1.
|
|
||||||
pub const fn geprc_tim3_tim5() -> GeprcTim3Tim5Motors {
|
|
||||||
GeprcTim3Tim5Motors
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl KakuteTim3Tim5Motors {
|
|
||||||
/// Selects the platform-validated DShot600 waveform configuration.
|
|
||||||
pub const fn dshot600(self) -> KakuteTim3Tim5Dshot600 {
|
|
||||||
KakuteTim3Tim5Dshot600
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl GeprcTim3Tim5Motors {
|
|
||||||
/// Selects the platform-validated DShot600 waveform configuration.
|
|
||||||
pub const fn dshot600(self) -> GeprcTim3Tim5Dshot600 {
|
|
||||||
GeprcTim3Tim5Dshot600
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/// MAVLink component selector.
|
/// MAVLink component selector.
|
||||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||||
pub struct Mavlink;
|
pub struct Mavlink;
|
||||||
@@ -208,13 +166,8 @@ pub struct Missing;
|
|||||||
/// typestate. A final `build()` exists only for complete supported route
|
/// typestate. A final `build()` exists only for complete supported route
|
||||||
/// combinations, so omitted or cross-wired essential routes fail to compile at
|
/// combinations, so omitted or cross-wired essential routes fail to compile at
|
||||||
/// the board declaration.
|
/// the board declaration.
|
||||||
pub struct F7BoardBuilder<
|
pub struct F7BoardBuilder<Imu = Missing, MavlinkRoute = Missing, Vertical = Missing> {
|
||||||
Imu = Missing,
|
_state: PhantomData<(Imu, MavlinkRoute, Vertical)>,
|
||||||
Motors = Missing,
|
|
||||||
MavlinkRoute = Missing,
|
|
||||||
Vertical = Missing,
|
|
||||||
> {
|
|
||||||
_state: PhantomData<(Imu, Motors, MavlinkRoute, Vertical)>,
|
|
||||||
}
|
}
|
||||||
|
|
||||||
impl F7BoardBuilder {
|
impl F7BoardBuilder {
|
||||||
@@ -232,21 +185,14 @@ impl Default for F7BoardBuilder {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
impl<I, M, T, V> F7BoardBuilder<I, M, T, V> {
|
impl<I, T, V> F7BoardBuilder<I, T, V> {
|
||||||
/// Selects an IMU route descriptor.
|
/// Selects an IMU route descriptor.
|
||||||
///
|
///
|
||||||
/// Only a complete, profile-compatible descriptor has a final
|
/// Only a complete, profile-compatible descriptor has a final
|
||||||
/// [`F7BoardBuilder::build`] implementation. Keeping this one selection
|
/// [`F7BoardBuilder::build`] implementation. Keeping this one selection
|
||||||
/// operation uniform avoids teaching a board author platform profile
|
/// operation uniform avoids teaching a board author platform profile
|
||||||
/// mechanics merely because the selected IMU model differs.
|
/// mechanics merely because the selected IMU model differs.
|
||||||
pub fn imu<Selected>(self, _: Selected) -> F7BoardBuilder<Selected, M, T, V> {
|
pub fn imu<Selected>(self, _: Selected) -> F7BoardBuilder<Selected, T, V> {
|
||||||
F7BoardBuilder {
|
|
||||||
_state: PhantomData,
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Selects the TIM3/TIM5 DShot600 output route.
|
|
||||||
pub fn motors<Selected>(self, _: Selected) -> F7BoardBuilder<I, Selected, T, V> {
|
|
||||||
F7BoardBuilder {
|
F7BoardBuilder {
|
||||||
_state: PhantomData,
|
_state: PhantomData,
|
||||||
}
|
}
|
||||||
@@ -254,7 +200,7 @@ impl<I, M, T, V> F7BoardBuilder<I, M, T, V> {
|
|||||||
|
|
||||||
/// Selects a validated MAVLink route. Only profile-compatible route types
|
/// Selects a validated MAVLink route. Only profile-compatible route types
|
||||||
/// have a final [`F7BoardBuilder::build`] implementation.
|
/// have a final [`F7BoardBuilder::build`] implementation.
|
||||||
pub fn mavlink<Selected>(self, _: Selected) -> F7BoardBuilder<I, M, Selected, V> {
|
pub fn mavlink<Selected>(self, _: Selected) -> F7BoardBuilder<I, Selected, V> {
|
||||||
F7BoardBuilder {
|
F7BoardBuilder {
|
||||||
_state: PhantomData,
|
_state: PhantomData,
|
||||||
}
|
}
|
||||||
@@ -262,7 +208,7 @@ impl<I, M, T, V> F7BoardBuilder<I, M, T, V> {
|
|||||||
|
|
||||||
/// Selects a validated vertical-sensor route. Its capability is checked
|
/// Selects a validated vertical-sensor route. Its capability is checked
|
||||||
/// against [`F7Board::CONFIG`] before any peripheral is initialized.
|
/// against [`F7Board::CONFIG`] before any peripheral is initialized.
|
||||||
pub fn vertical_sensor<Selected>(self, _: Selected) -> F7BoardBuilder<I, M, T, Selected> {
|
pub fn vertical_sensor<Selected>(self, _: Selected) -> F7BoardBuilder<I, T, Selected> {
|
||||||
F7BoardBuilder {
|
F7BoardBuilder {
|
||||||
_state: PhantomData,
|
_state: PhantomData,
|
||||||
}
|
}
|
||||||
@@ -285,7 +231,6 @@ pub(crate) enum F722Profile {
|
|||||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||||
pub struct F722BoardRoutes {
|
pub struct F722BoardRoutes {
|
||||||
profile: F722Profile,
|
profile: F722Profile,
|
||||||
motors: f722::MotorRoute,
|
|
||||||
vertical: f722::VerticalRoute,
|
vertical: f722::VerticalRoute,
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -297,12 +242,11 @@ impl F722BoardRoutes {
|
|||||||
|
|
||||||
macro_rules! impl_mpu6000_routes {
|
macro_rules! impl_mpu6000_routes {
|
||||||
($vertical:ty, $route:expr) => {
|
($vertical:ty, $route:expr) => {
|
||||||
impl F7BoardBuilder<Mpu6000Spi1Exti4, KakuteTim3Tim5Dshot600, MavlinkUsart1, $vertical> {
|
impl F7BoardBuilder<Mpu6000Spi1Exti4, MavlinkUsart1, $vertical> {
|
||||||
/// Finishes a complete supported MPU6000 F722 declaration.
|
/// Finishes a complete supported MPU6000 F722 declaration.
|
||||||
pub const fn build(self) -> Result<F722BoardRoutes, BoardError> {
|
pub const fn build(self) -> Result<F722BoardRoutes, BoardError> {
|
||||||
Ok(F722BoardRoutes {
|
Ok(F722BoardRoutes {
|
||||||
profile: F722Profile::Mpu6000Spi1Usart1,
|
profile: F722Profile::Mpu6000Spi1Usart1,
|
||||||
motors: f722::MotorRoute::KakuteTim3Tim5,
|
|
||||||
vertical: $route,
|
vertical: $route,
|
||||||
})
|
})
|
||||||
}
|
}
|
||||||
@@ -316,12 +260,11 @@ impl_mpu6000_routes!(NoVerticalSensor, f722::VerticalRoute::None);
|
|||||||
|
|
||||||
macro_rules! impl_icm42688_routes {
|
macro_rules! impl_icm42688_routes {
|
||||||
($vertical:ty, $route:expr) => {
|
($vertical:ty, $route:expr) => {
|
||||||
impl F7BoardBuilder<Icm42688Spi3Exti3, GeprcTim3Tim5Dshot600, MavlinkUsart2, $vertical> {
|
impl F7BoardBuilder<Icm42688Spi3Exti3, MavlinkUsart2, $vertical> {
|
||||||
/// Finishes a complete supported ICM-42688-P F722 declaration.
|
/// Finishes a complete supported ICM-42688-P F722 declaration.
|
||||||
pub const fn build(self) -> Result<F722BoardRoutes, BoardError> {
|
pub const fn build(self) -> Result<F722BoardRoutes, BoardError> {
|
||||||
Ok(F722BoardRoutes {
|
Ok(F722BoardRoutes {
|
||||||
profile: F722Profile::Icm42688Spi3Usart2,
|
profile: F722Profile::Icm42688Spi3Usart2,
|
||||||
motors: f722::MotorRoute::GeprcTim3Tim5,
|
|
||||||
vertical: $route,
|
vertical: $route,
|
||||||
})
|
})
|
||||||
}
|
}
|
||||||
@@ -333,30 +276,28 @@ impl_icm42688_routes!(NoVerticalSensor, f722::VerticalRoute::None);
|
|||||||
impl_icm42688_routes!(Mtf01Uart4, f722::VerticalRoute::Mtf01);
|
impl_icm42688_routes!(Mtf01Uart4, f722::VerticalRoute::Mtf01);
|
||||||
impl_icm42688_routes!(LidarLiteV3I2c1, f722::VerticalRoute::LidarLiteV3);
|
impl_icm42688_routes!(LidarLiteV3I2c1, f722::VerticalRoute::LidarLiteV3);
|
||||||
|
|
||||||
/// Missing and cross-profile route selections are rejected before firmware is
|
/// Missing and cross-profile route selections are rejected before board I/O is
|
||||||
/// linked, because no matching `build` implementation exists.
|
/// linked, because no matching `build` implementation exists.
|
||||||
///
|
///
|
||||||
/// ```compile_fail
|
/// ```compile_fail
|
||||||
/// use platform_stm32f7::{
|
/// use platform_stm32f7::{
|
||||||
/// F7BoardBuilder, Mavlink, MotorOutputs, Mpu6000, Pa4,
|
/// F7BoardBuilder, Mavlink, Mpu6000, Pa4,
|
||||||
/// };
|
/// };
|
||||||
///
|
///
|
||||||
/// let _ = F7BoardBuilder::new()
|
/// let _ = F7BoardBuilder::new()
|
||||||
/// .imu(Mpu6000::on_spi1().with_drdy(Pa4))
|
/// .imu(Mpu6000::on_spi1().with_drdy(Pa4))
|
||||||
/// .motors(MotorOutputs::kakute_tim3_tim5().dshot600())
|
|
||||||
/// .mavlink(Mavlink::on_usart1())
|
/// .mavlink(Mavlink::on_usart1())
|
||||||
/// .build(); // vertical route is missing
|
/// .build(); // vertical route is missing
|
||||||
/// ```
|
/// ```
|
||||||
///
|
///
|
||||||
/// ```compile_fail
|
/// ```compile_fail
|
||||||
/// use platform_stm32f7::{
|
/// use platform_stm32f7::{
|
||||||
/// F7BoardBuilder, Icm42688, Mavlink, MotorOutputs, Pc3,
|
/// F7BoardBuilder, Icm42688, Mavlink, Pc3,
|
||||||
/// VerticalSensor,
|
/// VerticalSensor,
|
||||||
/// };
|
/// };
|
||||||
///
|
///
|
||||||
/// let _ = F7BoardBuilder::new()
|
/// let _ = F7BoardBuilder::new()
|
||||||
/// .imu(Icm42688::on_spi3().with_drdy(Pc3))
|
/// .imu(Icm42688::on_spi3().with_drdy(Pc3))
|
||||||
/// .motors(MotorOutputs::geprc_tim3_tim5().dshot600())
|
|
||||||
/// .mavlink(Mavlink::on_usart1()) // ICM route requires USART2
|
/// .mavlink(Mavlink::on_usart1()) // ICM route requires USART2
|
||||||
/// .vertical_sensor(VerticalSensor::none())
|
/// .vertical_sensor(VerticalSensor::none())
|
||||||
/// .build();
|
/// .build();
|
||||||
@@ -364,15 +305,14 @@ impl_icm42688_routes!(LidarLiteV3I2c1, f722::VerticalRoute::LidarLiteV3);
|
|||||||
///
|
///
|
||||||
/// ```compile_fail
|
/// ```compile_fail
|
||||||
/// use platform_stm32f7::{
|
/// use platform_stm32f7::{
|
||||||
/// F7BoardBuilder, Icm42688, Mavlink, MotorOutputs, Pc3, VerticalSensor,
|
/// F7BoardBuilder, Icm42688, Mavlink, Pc3, VerticalSensor,
|
||||||
/// };
|
/// };
|
||||||
///
|
///
|
||||||
/// let _ = F7BoardBuilder::new()
|
/// let _ = F7BoardBuilder::new()
|
||||||
/// .imu(Icm42688::on_spi3().with_drdy(Pc3))
|
/// .imu(Icm42688::on_spi3().with_drdy(Pc3))
|
||||||
/// .motors(MotorOutputs::kakute_tim3_tim5().dshot600())
|
|
||||||
/// .mavlink(Mavlink::on_usart2())
|
/// .mavlink(Mavlink::on_usart2())
|
||||||
/// .vertical_sensor(VerticalSensor::none())
|
/// .vertical_sensor(VerticalSensor::none())
|
||||||
/// .build(); // ICM/USART2 uses the GEPRC logical motor order.
|
/// .build(); // MPU6000/USART1 is the only other supported profile.
|
||||||
/// ```
|
/// ```
|
||||||
/// Contract implemented by a declarative STM32F722 board crate.
|
/// Contract implemented by a declarative STM32F722 board crate.
|
||||||
pub trait F7Board {
|
pub trait F7Board {
|
||||||
@@ -394,7 +334,10 @@ macro_rules! enforce_f7_board_contract {
|
|||||||
};
|
};
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Installs the platform-owned RTIC flight firmware for a declarative board.
|
/// Installs the one platform-owned RTIC application for a declarative board.
|
||||||
|
///
|
||||||
|
/// Board crates can only select one of the reviewed physical-sensor telemetry
|
||||||
|
/// route descriptors; RTIC ownership stays platform-private.
|
||||||
///
|
///
|
||||||
/// The application binds the finite superset of interrupt roles needed by the
|
/// The application binds the finite superset of interrupt roles needed by the
|
||||||
/// currently supported F722 profiles. The route declaration selects which
|
/// currently supported F722 profiles. The route declaration selects which
|
||||||
@@ -416,11 +359,11 @@ macro_rules! flight_firmware {
|
|||||||
<$board as $crate::F7Board>::build($crate::F7BoardBuilder::new())
|
<$board as $crate::F7Board>::build($crate::F7BoardBuilder::new())
|
||||||
}
|
}
|
||||||
#[used]
|
#[used]
|
||||||
static KAPTAN_F722_PLATFORM_FIRMWARE: fn() = $crate::firmware_anchor;
|
static KAPTAN_F722_PLATFORM_FLIGHT: fn() = $crate::firmware_anchor;
|
||||||
};
|
};
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Link anchor for the platform-owned F722 RTIC application.
|
/// Link anchor for the platform-owned F722 board-I/O application.
|
||||||
#[doc(hidden)]
|
#[doc(hidden)]
|
||||||
pub fn firmware_anchor() {}
|
pub fn firmware_anchor() {}
|
||||||
|
|
||||||
@@ -432,7 +375,6 @@ mod tests {
|
|||||||
fn route_descriptors_select_the_mpu6000_profile() {
|
fn route_descriptors_select_the_mpu6000_profile() {
|
||||||
let routes = F7BoardBuilder::new()
|
let routes = F7BoardBuilder::new()
|
||||||
.imu(Mpu6000::on_spi1().with_drdy(Pa4))
|
.imu(Mpu6000::on_spi1().with_drdy(Pa4))
|
||||||
.motors(MotorOutputs::kakute_tim3_tim5().dshot600())
|
|
||||||
.mavlink(Mavlink::on_usart1())
|
.mavlink(Mavlink::on_usart1())
|
||||||
.vertical_sensor(VerticalSensor::mtf01_on_uart4())
|
.vertical_sensor(VerticalSensor::mtf01_on_uart4())
|
||||||
.build()
|
.build()
|
||||||
@@ -444,7 +386,6 @@ mod tests {
|
|||||||
fn route_descriptors_select_the_icm42688_profile() {
|
fn route_descriptors_select_the_icm42688_profile() {
|
||||||
let routes = F7BoardBuilder::new()
|
let routes = F7BoardBuilder::new()
|
||||||
.imu(Icm42688::on_spi3().with_drdy(Pc3))
|
.imu(Icm42688::on_spi3().with_drdy(Pc3))
|
||||||
.motors(MotorOutputs::geprc_tim3_tim5().dshot600())
|
|
||||||
.mavlink(Mavlink::on_usart2())
|
.mavlink(Mavlink::on_usart2())
|
||||||
.vertical_sensor(VerticalSensor::none())
|
.vertical_sensor(VerticalSensor::none())
|
||||||
.build()
|
.build()
|
||||||
|
|||||||
@@ -0,0 +1,119 @@
|
|||||||
|
use std::{fs, path::PathBuf};
|
||||||
|
|
||||||
|
fn platform_source(relative: &str) -> String {
|
||||||
|
let root = PathBuf::from(env!("CARGO_MANIFEST_DIR"));
|
||||||
|
fs::read_to_string(root.join(relative)).expect("platform source must exist")
|
||||||
|
}
|
||||||
|
|
||||||
|
#[test]
|
||||||
|
fn former_motor_pins_are_inhibited_before_all_other_initialization() {
|
||||||
|
let module = platform_source("src/f722/mod.rs");
|
||||||
|
let interlock = module
|
||||||
|
.find("motor_inhibit::inhibit_motor_pins")
|
||||||
|
.expect("F722 initialization must claim the former motor pins first");
|
||||||
|
let validation = module
|
||||||
|
.find("config.validate()")
|
||||||
|
.expect("F722 initialization must validate its board configuration");
|
||||||
|
let board_initialization = module
|
||||||
|
.find("board::initialize")
|
||||||
|
.expect("F722 initialization must subsequently claim telemetry I/O");
|
||||||
|
|
||||||
|
assert!(interlock < validation);
|
||||||
|
assert!(validation < board_initialization);
|
||||||
|
|
||||||
|
let interlock = platform_source("src/f722/motor_inhibit.rs");
|
||||||
|
let reset = interlock
|
||||||
|
.find("port.bsrr()")
|
||||||
|
.expect("interlock must reset the former motor-pin output latches");
|
||||||
|
let output_mode = interlock
|
||||||
|
.find("port.moder()")
|
||||||
|
.expect("interlock must configure the former motor pins as GPIO outputs");
|
||||||
|
assert!(
|
||||||
|
reset < output_mode,
|
||||||
|
"output latches must be low before GPIO output mode is selected"
|
||||||
|
);
|
||||||
|
for pin in ["br0", "br1", "br4", "br5"] {
|
||||||
|
assert!(interlock.contains(pin), "interlock must reset PB pin {pin}");
|
||||||
|
}
|
||||||
|
for pin in ["moder0", "moder1", "moder4", "moder5"] {
|
||||||
|
assert!(
|
||||||
|
interlock.contains(pin),
|
||||||
|
"interlock must configure PB pin {pin}"
|
||||||
|
);
|
||||||
|
}
|
||||||
|
for pin in ["ot0", "ot1", "ot4", "ot5"] {
|
||||||
|
assert!(
|
||||||
|
interlock.contains(pin),
|
||||||
|
"interlock must set PB pin {pin} push-pull"
|
||||||
|
);
|
||||||
|
}
|
||||||
|
for pin in ["ospeedr0", "ospeedr1", "ospeedr4", "ospeedr5"] {
|
||||||
|
assert!(
|
||||||
|
interlock.contains(pin),
|
||||||
|
"interlock must set PB pin {pin} low speed"
|
||||||
|
);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[test]
|
||||||
|
fn telemetry_platform_has_no_physical_actuator_peripheral_path() {
|
||||||
|
let sources = [
|
||||||
|
platform_source("src/lib.rs"),
|
||||||
|
platform_source("src/f722/mod.rs"),
|
||||||
|
platform_source("src/f722/board.rs"),
|
||||||
|
platform_source("src/f722/firmware.rs"),
|
||||||
|
platform_source("src/f722/interrupts.rs"),
|
||||||
|
]
|
||||||
|
.join("\n");
|
||||||
|
|
||||||
|
for forbidden in [
|
||||||
|
"mod actuator",
|
||||||
|
"MotorOutput",
|
||||||
|
"MotorRoute",
|
||||||
|
"MotorOutputs",
|
||||||
|
"TIM3",
|
||||||
|
"TIM5",
|
||||||
|
"Dshot",
|
||||||
|
"Pwm",
|
||||||
|
"device.TIM3",
|
||||||
|
"device.TIM5",
|
||||||
|
] {
|
||||||
|
assert!(
|
||||||
|
!sources.contains(forbidden),
|
||||||
|
"telemetry platform must not expose actuator path {forbidden}"
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
let manifest = platform_source("Cargo.toml");
|
||||||
|
assert!(
|
||||||
|
!manifest.contains("kaptan-actuator"),
|
||||||
|
"telemetry platform must not link the physical actuator crate"
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
#[test]
|
||||||
|
fn telemetry_has_no_inbound_command_authority() {
|
||||||
|
let board = platform_source("src/f722/board.rs");
|
||||||
|
let firmware = platform_source("src/f722/firmware.rs");
|
||||||
|
|
||||||
|
assert!(
|
||||||
|
!board.contains("MavlinkReceiver"),
|
||||||
|
"telemetry must not retain an inbound MAVLink dispatch path"
|
||||||
|
);
|
||||||
|
assert!(
|
||||||
|
!board.contains("ArmRequest"),
|
||||||
|
"telemetry must not decode arm commands"
|
||||||
|
);
|
||||||
|
assert!(
|
||||||
|
!board.contains("MAV_RESULT_ACCEPTED"),
|
||||||
|
"telemetry must not accept an arm command"
|
||||||
|
);
|
||||||
|
assert!(
|
||||||
|
!firmware.contains("shared = [mavlink, motors]"),
|
||||||
|
"the MAVLink worker must not share a motor resource"
|
||||||
|
);
|
||||||
|
assert!(
|
||||||
|
!firmware.contains("MAVLINK_EVENT_RECEIVED"),
|
||||||
|
"telemetry must not schedule a worker from an inbound command"
|
||||||
|
);
|
||||||
|
}
|
||||||
Reference in new issue
Block a user