boards/kakute-f7-mini-v3: declare F722 flight profile
This commit is contained in:
Onur Özkan committed
1 parent
ac54c8308d
commit
827983fb56
8 files changed
+65
-1300
No files matched your search
@@ -11,20 +11,11 @@ workspace = true
|
||||
|
||||
[features]
|
||||
firmware = []
|
||||
lidar-lite-v3 = []
|
||||
|
||||
[dependencies]
|
||||
cortex-m = "0.7"
|
||||
cortex-m-rt = "0.7"
|
||||
kaptan-actuator = { path = "../../libs/kaptan-actuator" }
|
||||
kaptan-drivers = { path = "../../libs/kaptan-drivers" }
|
||||
kaptan-dma = { path = "../../libs/kaptan-dma" }
|
||||
kaptan-mavlink = { path = "../../libs/kaptan-mavlink" }
|
||||
kaptan-runtime = { path = "../../libs/kaptan-runtime" }
|
||||
kaptan-spi = { path = "../../libs/kaptan-spi" }
|
||||
kaptan-stm32f7 = { path = "../../libs/kaptan-stm32f7", features = ["stm32f722"] }
|
||||
panic-halt = "1"
|
||||
rtic = { version = "2.3", default-features = false, features = ["thumbv7-backend"] }
|
||||
stm32f7 = { version = "0.16", default-features = false, features = ["rt", "stm32f722"] }
|
||||
kaptan-board = { path = "../../libs/kaptan-board" }
|
||||
platform-stm32f7 = { path = "../../platforms/stm32f7" }
|
||||
|
||||
[[bin]]
|
||||
name = "kaptan-kakute-f7-mini-v3"
|
||||
|
||||
@@ -30,8 +30,7 @@ The converter works with any firmware ELF. It uses rustup's `llvm-tools-preview`
|
||||
4. Flash the generated image:
|
||||
|
||||
```sh
|
||||
dfu-util --alt 0 --dfuse-address 0x08000000:leave \
|
||||
--download target/thumbv7em-none-eabihf/release/kaptan-kakute-f7-mini-v3.bin
|
||||
dfu-util --alt 0 --dfuse-address 0x08000000:leave --download target/thumbv7em-none-eabihf/release/kaptan-kakute-f7-mini-v3.bin
|
||||
```
|
||||
|
||||
## UART
|
||||
|
||||
@@ -1,84 +0,0 @@
|
||||
mod tim3_dshot;
|
||||
mod tim3_pwm;
|
||||
|
||||
use core::sync::atomic::{AtomicU8, Ordering};
|
||||
|
||||
use kaptan_actuator::{MotorOutput, MotorProtocol};
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
use self::{tim3_dshot::Tim3Dshot, tim3_pwm::Tim3Pwm};
|
||||
|
||||
const TRANSPORT_NONE: u8 = 0;
|
||||
const TRANSPORT_PWM: u8 = 1;
|
||||
const TRANSPORT_DSHOT: u8 = 2;
|
||||
|
||||
static ACTIVE_TRANSPORT: AtomicU8 = AtomicU8::new(TRANSPORT_NONE);
|
||||
|
||||
/// Kakute F7 Mini V3's protocol-neutral M1–M4 output route.
|
||||
pub(crate) struct KakuteMotorOutput {
|
||||
transport: Transport,
|
||||
}
|
||||
|
||||
enum Transport {
|
||||
Pwm(Tim3Pwm),
|
||||
Dshot(Tim3Dshot),
|
||||
}
|
||||
|
||||
impl KakuteMotorOutput {
|
||||
pub(crate) fn new(
|
||||
rcc: &stm32f722::RCC,
|
||||
tim3: stm32f722::TIM3,
|
||||
tim5: stm32f722::TIM5,
|
||||
protocol: MotorProtocol,
|
||||
) -> Self {
|
||||
let transport = match protocol {
|
||||
MotorProtocol::Pwm(config) => {
|
||||
ACTIVE_TRANSPORT.store(TRANSPORT_PWM, Ordering::Release);
|
||||
let _ = tim5;
|
||||
Transport::Pwm(Tim3Pwm::new(rcc, tim3, config))
|
||||
}
|
||||
MotorProtocol::Dshot(config) => {
|
||||
ACTIVE_TRANSPORT.store(TRANSPORT_DSHOT, Ordering::Release);
|
||||
Transport::Dshot(Tim3Dshot::new(rcc, tim3, tim5, config))
|
||||
}
|
||||
};
|
||||
Self { transport }
|
||||
}
|
||||
|
||||
/// Services the stable TIM3 interrupt route.
|
||||
pub(crate) fn service_tim3_interrupt() {
|
||||
if ACTIVE_TRANSPORT.load(Ordering::Acquire) == TRANSPORT_PWM {
|
||||
Tim3Pwm::on_tim3_update();
|
||||
}
|
||||
}
|
||||
|
||||
/// Services the stable TIM5 interrupt route.
|
||||
pub(crate) fn service_tim5_interrupt() {
|
||||
if ACTIVE_TRANSPORT.load(Ordering::Acquire) == TRANSPORT_DSHOT {
|
||||
Tim3Dshot::on_tim5_update();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl MotorOutput for KakuteMotorOutput {
|
||||
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(),
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,402 +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,
|
||||
) -> 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| unsafe { write.bits(0) });
|
||||
timer
|
||||
.psc()
|
||||
.write(|write| unsafe { write.psc().bits((timer_prescaler - 1) as u16) });
|
||||
timer
|
||||
.arr()
|
||||
.write(|write| unsafe { write.bits(u32::from(timing.bit_ticks() - 1)) });
|
||||
timer
|
||||
.ccmr1_output()
|
||||
.write(|write| unsafe { write.bits(PWM_MODE_1_WITH_PRELOAD) });
|
||||
timer
|
||||
.ccmr2_output()
|
||||
.write(|write| unsafe { write.bits(PWM_MODE_1_WITH_PRELOAD) });
|
||||
timer.dcr().write(|write| 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());
|
||||
|
||||
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| unsafe { write.psc().bits(0) });
|
||||
scheduler
|
||||
.arr()
|
||||
.write(|write| 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;
|
||||
}
|
||||
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();
|
||||
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| 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| 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() {
|
||||
let scheduler = unsafe { &*stm32f722::TIM5::ptr() };
|
||||
scheduler.sr().modify(|_, write| write.uif().clear());
|
||||
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| unsafe { write.bits(DSHOT_BUFFER_LENGTH as u32) });
|
||||
dma_stream()
|
||||
.m0ar()
|
||||
.write(|write| 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) {
|
||||
write_dma_frame(
|
||||
core::ptr::addr_of_mut!(DSHOT_ZERO_FRAME).cast(),
|
||||
FrameValue::Stop,
|
||||
timing,
|
||||
);
|
||||
write_dma_frame(
|
||||
core::ptr::addr_of_mut!(DSHOT_ARMED_IDLE_FRAME).cast(),
|
||||
FrameValue::Throttle(armed_idle),
|
||||
timing,
|
||||
);
|
||||
}
|
||||
|
||||
fn write_dma_frame(destination: *mut u32, value: FrameValue, timing: Timing) {
|
||||
let symbols = symbols(
|
||||
value.packet(),
|
||||
timing.zero_high_ticks(),
|
||||
timing.one_high_ticks(),
|
||||
);
|
||||
let mut bit = 0;
|
||||
while bit < FRAME_SYMBOLS {
|
||||
let mut channel = 0;
|
||||
while channel < DSHOT_CHANNELS {
|
||||
unsafe {
|
||||
destination
|
||||
.add(bit * DSHOT_CHANNELS + channel)
|
||||
.write(symbols[bit] as u32)
|
||||
};
|
||||
channel += 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 {
|
||||
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| 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| unsafe { write.bits(DSHOT_BUFFER_LENGTH as u32) });
|
||||
dma_stream()
|
||||
.par()
|
||||
.write(|write| unsafe { write.bits(tim3_dmar_address()) });
|
||||
dma_stream()
|
||||
.m0ar()
|
||||
.write(|write| unsafe { write.bits(frame as u32) });
|
||||
dma_stream()
|
||||
.fcr()
|
||||
.write(|write| unsafe { write.fth().bits(3).dmdis().set_bit() });
|
||||
true
|
||||
}
|
||||
|
||||
fn tim3_dmar_address() -> u32 {
|
||||
unsafe { (&*stm32f722::TIM3::ptr()).dmar().as_ptr() as u32 }
|
||||
}
|
||||
|
||||
fn port() -> &'static stm32f722::gpiob::RegisterBlock {
|
||||
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| 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,180 +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| unsafe {
|
||||
write
|
||||
.psc()
|
||||
.bits((APB1_TIMER_CLOCK_HZ / TIMER_TICK_HZ - 1) as u16)
|
||||
});
|
||||
timer
|
||||
.arr()
|
||||
.write(|write| unsafe { write.bits(u32::from(period_us - 1)) });
|
||||
timer
|
||||
.ccmr1_output()
|
||||
.write(|write| unsafe { write.bits(PWM_MODE_1_WITH_PRELOAD) });
|
||||
timer
|
||||
.ccmr2_output()
|
||||
.write(|write| unsafe { write.bits(PWM_MODE_1_WITH_PRELOAD) });
|
||||
write_pulse(&timer, config.minimum_pulse());
|
||||
timer
|
||||
.ccer()
|
||||
.write(|write| 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() {
|
||||
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| unsafe { write.bits(ticks) });
|
||||
timer.ccr2().write(|write| unsafe { write.bits(ticks) });
|
||||
timer.ccr3().write(|write| unsafe { write.bits(ticks) });
|
||||
timer.ccr4().write(|write| unsafe { write.bits(ticks) });
|
||||
}
|
||||
|
||||
fn port() -> &'static stm32f722::gpiob::RegisterBlock {
|
||||
unsafe { &*stm32f722::GPIOB::ptr() }
|
||||
}
|
||||
|
||||
fn configure_gpio_alternate_function() {
|
||||
let port = port();
|
||||
port.afrl().modify(|_, write| 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,85 +1,64 @@
|
||||
//! Kakute F7 Mini V3 static hardware selections.
|
||||
//! Declarative Kakute F7 Mini V3 board description.
|
||||
//!
|
||||
//! This module deliberately contains no MCU registers, RTIC concepts, unsafe
|
||||
//! code, interrupt names, DMA handling, or task scheduling. Those concerns
|
||||
//! belong to the STM32F7 platform crate.
|
||||
|
||||
use kaptan_actuator::{ArmingDuration, MotorProtocol};
|
||||
use kaptan_drivers::lidar_lite_v3::AcquisitionMode;
|
||||
use kaptan_drivers::mpu6000::{
|
||||
AccelerometerRange, DigitalLowPassFilter, GyroscopeRange, Mpu6000Config,
|
||||
};
|
||||
use kaptan_stm32f7::stm32f722::{I2c1Config, Tim2PeriodicConfig};
|
||||
|
||||
/// Core frequency in whole megahertz after selecting the PLL216 board profile.
|
||||
pub const CORE_CLOCK_MHZ: u32 = 216;
|
||||
/// APB2 frequency driving USART1 in the PLL216 board profile.
|
||||
pub const USART1_CLOCK_HZ: u32 = 108_000_000;
|
||||
/// Telemetry UART wire rate.
|
||||
pub const USART1_BAUD: u32 = 115_200;
|
||||
/// Emit one raw-IMU MAVLink frame every this many 100 Hz service ticks.
|
||||
///
|
||||
/// This caps telemetry at 20 Hz while preserving the full acquisition rate for
|
||||
/// flight-control work and leaves bandwidth for range and critical MAVLink
|
||||
/// traffic on the 115200-baud UART.
|
||||
pub const MAVLINK_RAW_IMU_TICK_DIVISOR: u8 = 5;
|
||||
/// I²C1 timing for 54 MHz PCLK1 and 100 kHz Standard-mode operation.
|
||||
///
|
||||
/// This conservative setting removes Fast-mode rise-time sensitivity while
|
||||
/// bringing up the board route. LIDAR-Lite v3 is I²C-compatible and accepts
|
||||
/// Standard-mode transfers. The range service starts at most one of its five
|
||||
/// STOP-separated transfers per 100 Hz tick, which also bounds range MAVLink
|
||||
/// output to 20 Hz.
|
||||
pub const I2C1_CONFIG: I2c1Config = I2c1Config::from_timing_register(0x2040_4768);
|
||||
/// LIDAR acquisition command used for each measurement cycle.
|
||||
pub const LIDAR_ACQUISITION_MODE: AcquisitionMode = AcquisitionMode::WithReceiverBiasCorrection;
|
||||
/// Number of 100 Hz service ticks to wait before the first LIDAR transaction.
|
||||
///
|
||||
/// Three ticks provide 30 ms, exceeding the sensor's required 22 ms startup
|
||||
/// interval without waiting in RTIC initialization.
|
||||
pub const LIDAR_STARTUP_TICKS: u8 = 3;
|
||||
/// ESC protocol selected for this board's M1–M4 output route.
|
||||
pub 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"),
|
||||
},
|
||||
);
|
||||
/// MAVLink sender system identifier.
|
||||
pub const MAVLINK_SYSTEM_ID: u8 = 1;
|
||||
/// MAVLink sender component identifier.
|
||||
pub const MAVLINK_COMPONENT_ID: u8 = 1;
|
||||
/// MAVLink sensor instance for the LIDAR-Lite v3.
|
||||
pub const LIDAR_SENSOR_ID: u8 = 0;
|
||||
/// LIDAR-Lite v3 minimum usable range, in MAVLink centimetres.
|
||||
pub const LIDAR_MIN_DISTANCE_CENTIMETRES: u16 = 20;
|
||||
/// LIDAR-Lite v3 maximum usable range, in MAVLink centimetres.
|
||||
pub const LIDAR_MAX_DISTANCE_CENTIMETRES: u16 = 4_000;
|
||||
/// Mounting orientation published with the range measurement.
|
||||
///
|
||||
/// Change this when the sensor is not mounted facing downward.
|
||||
pub const LIDAR_ORIENTATION: kaptan_mavlink::message::MavSensorOrientation =
|
||||
kaptan_mavlink::message::MavSensorOrientation::MAV_SENSOR_ROTATION_PITCH_270;
|
||||
|
||||
/// MPU6000 full-scale profile used by this board.
|
||||
pub const MPU6000_CONFIG: Mpu6000Config = Mpu6000Config {
|
||||
accelerometer: AccelerometerRange::PlusMinus4G,
|
||||
gyroscope: GyroscopeRange::PlusMinus500DegreesPerSecond,
|
||||
digital_low_pass_filter: DigitalLowPassFilter::Bandwidth42Hz,
|
||||
sample_rate_divider: 9,
|
||||
use kaptan_board::{BoardConfig, DataChannelRates, MavlinkEndpoint, VerticalSensingKind};
|
||||
use platform_stm32f7::{
|
||||
BoardError, F7Board, F7BoardBuilder, F722BoardRoutes, Mavlink, MotorOutputs, Mpu6000, Pa4,
|
||||
VerticalSensor,
|
||||
};
|
||||
|
||||
/// One-hundred-hertz IMU service update from the 108 MHz APB1 timer clock.
|
||||
///
|
||||
/// This matches the MPU6000 output data rate selected by `MPU6000_CONFIG`.
|
||||
/// The runtime emits its one-hertz heartbeat every hundredth tick.
|
||||
pub const IMU_SERVICE_TIMER_CONFIG: Tim2PeriodicConfig = Tim2PeriodicConfig {
|
||||
prescaler: 10_799,
|
||||
auto_reload: 99,
|
||||
/// Holybro Kakute F7 Mini V3.
|
||||
pub struct KakuteF7MiniV3;
|
||||
|
||||
#[cfg(not(feature = "lidar-lite-v3"))]
|
||||
const CONFIG: BoardConfig = BoardConfig {
|
||||
name: "Kakute F7 Mini V3",
|
||||
mavlink: MavlinkEndpoint {
|
||||
system_id: 1,
|
||||
component_id: 1,
|
||||
},
|
||||
vertical_sensing: VerticalSensingKind::RangeAndOpticalFlow,
|
||||
data_channel: DataChannelRates {
|
||||
service_hz: 100,
|
||||
raw_imu_hz: 20,
|
||||
range_hz: Some(20),
|
||||
optical_flow_hz: Some(20),
|
||||
heartbeat_hz: 2,
|
||||
},
|
||||
};
|
||||
|
||||
#[cfg(feature = "lidar-lite-v3")]
|
||||
const CONFIG: BoardConfig = BoardConfig {
|
||||
name: "Kakute F7 Mini V3",
|
||||
mavlink: MavlinkEndpoint {
|
||||
system_id: 1,
|
||||
component_id: 1,
|
||||
},
|
||||
vertical_sensing: VerticalSensingKind::Range,
|
||||
data_channel: DataChannelRates {
|
||||
service_hz: 100,
|
||||
raw_imu_hz: 20,
|
||||
range_hz: Some(20),
|
||||
optical_flow_hz: None,
|
||||
heartbeat_hz: 2,
|
||||
},
|
||||
};
|
||||
|
||||
impl F7Board for KakuteF7MiniV3 {
|
||||
const CONFIG: BoardConfig = CONFIG;
|
||||
|
||||
fn build(platform: F7BoardBuilder) -> Result<F722BoardRoutes, BoardError> {
|
||||
let platform = platform
|
||||
.imu(Mpu6000::on_spi1().with_drdy(Pa4))
|
||||
.motors(MotorOutputs::kakute_tim3_tim5().dshot600())
|
||||
.mavlink(Mavlink::on_usart1());
|
||||
#[cfg(feature = "lidar-lite-v3")]
|
||||
let platform = platform.vertical_sensor(VerticalSensor::lidar_lite_v3_on_i2c1());
|
||||
#[cfg(not(feature = "lidar-lite-v3"))]
|
||||
let platform = platform.vertical_sensor(VerticalSensor::mtf01_on_uart4());
|
||||
platform.build()
|
||||
}
|
||||
}
|
||||
@@ -1,472 +1,6 @@
|
||||
#![no_std]
|
||||
#![no_main]
|
||||
|
||||
use core::sync::atomic::{AtomicBool, AtomicU8, Ordering};
|
||||
|
||||
use cortex_m_rt as _;
|
||||
use kaptan_runtime::{LatestImuMailbox, LatestRangeMailbox};
|
||||
use panic_halt as _;
|
||||
|
||||
mod actuator;
|
||||
mod board;
|
||||
mod routes;
|
||||
|
||||
const IMU_EVENT_DRDY: u8 = 1 << 0;
|
||||
const IMU_EVENT_TRANSFER_COMPLETE: u8 = 1 << 1;
|
||||
const IMU_EVENT_TRANSFER_FAULT: u8 = 1 << 2;
|
||||
const IMU_EVENT_TIMER: u8 = 1 << 3;
|
||||
const IMU_EVENT_START: u8 = 1 << 4;
|
||||
const MAVLINK_EVENT_SAMPLE: u8 = 1 << 0;
|
||||
const MAVLINK_EVENT_HEARTBEAT: u8 = 1 << 1;
|
||||
const MAVLINK_EVENT_DMA: u8 = 1 << 2;
|
||||
const MAVLINK_EVENT_RECEIVED: u8 = 1 << 3;
|
||||
const MAVLINK_EVENT_RANGE: u8 = 1 << 4;
|
||||
|
||||
static IMU_EVENTS: AtomicU8 = AtomicU8::new(0);
|
||||
static MAVLINK_EVENTS: AtomicU8 = AtomicU8::new(0);
|
||||
static IMU_QUEUED: AtomicBool = AtomicBool::new(false);
|
||||
static MAVLINK_QUEUED: AtomicBool = AtomicBool::new(false);
|
||||
static RAW_IMU_TELEMETRY_DUE: AtomicBool = AtomicBool::new(false);
|
||||
static SAMPLES: LatestImuMailbox = LatestImuMailbox::new();
|
||||
static RANGES: LatestRangeMailbox = LatestRangeMailbox::new();
|
||||
static UART_COMPLETION: AtomicU8 = AtomicU8::new(0);
|
||||
|
||||
#[rtic::app(device = stm32f7::stm32f722, peripherals = true, dispatchers = [TIM6_DAC, TIM7])]
|
||||
mod app {
|
||||
use super::{
|
||||
IMU_EVENT_DRDY, IMU_EVENT_START, IMU_EVENT_TIMER, IMU_EVENT_TRANSFER_COMPLETE,
|
||||
IMU_EVENT_TRANSFER_FAULT, IMU_EVENTS, IMU_QUEUED, MAVLINK_EVENT_DMA,
|
||||
MAVLINK_EVENT_HEARTBEAT, MAVLINK_EVENT_RANGE, MAVLINK_EVENT_RECEIVED, MAVLINK_EVENT_SAMPLE,
|
||||
MAVLINK_EVENTS, MAVLINK_QUEUED, Ordering, RANGES, RAW_IMU_TELEMETRY_DUE, SAMPLES,
|
||||
UART_COMPLETION,
|
||||
};
|
||||
use crate::actuator::KakuteMotorOutput;
|
||||
use crate::board::{
|
||||
CORE_CLOCK_MHZ, I2C1_CONFIG, IMU_SERVICE_TIMER_CONFIG, LIDAR_ACQUISITION_MODE,
|
||||
LIDAR_MAX_DISTANCE_CENTIMETRES, LIDAR_MIN_DISTANCE_CENTIMETRES, LIDAR_ORIENTATION,
|
||||
LIDAR_SENSOR_ID, LIDAR_STARTUP_TICKS, MAVLINK_COMPONENT_ID, MAVLINK_RAW_IMU_TICK_DIVISOR,
|
||||
MAVLINK_SYSTEM_ID, MOTOR_PROTOCOL, MPU6000_CONFIG, USART1_BAUD, USART1_CLOCK_HZ,
|
||||
};
|
||||
use crate::routes::KakuteMpu6000SpiRoute;
|
||||
use kaptan_actuator::MotorOutput;
|
||||
use kaptan_dma::TxCompletion;
|
||||
use kaptan_drivers::{lidar_lite_v3::LidarLiteV3, mpu6000::Mpu6000};
|
||||
use kaptan_mavlink::{
|
||||
arm::{ArmRequest, decode_arm_request},
|
||||
encoder::MavlinkEncoder,
|
||||
receiver::MavlinkReceiver,
|
||||
transmitter::{FrameClass, MavlinkTransmitter},
|
||||
};
|
||||
use kaptan_runtime::{ImuEvent, ImuService, RangeEvent, RangeService};
|
||||
use kaptan_spi::SpiCompletion;
|
||||
use kaptan_stm32f7::stm32f722::{
|
||||
ActiveLowLedPd2, DcacheDisabledDmaMemory, DwtMicrosecondClock, I2c1, Spi1, Tim2Periodic,
|
||||
Usart1Tx, Usart1TxDma, configure_exti4_pin_pa4, configure_i2c1_pins_pb8_pb9,
|
||||
configure_spi1_pins_pa5_pa6_pa7, configure_usart1_rx_pin_pa10, configure_usart1_tx_pin_pa9,
|
||||
select_pll216_profile,
|
||||
};
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
#[shared]
|
||||
struct Shared {
|
||||
mavlink_receiver: MavlinkReceiver<4>,
|
||||
range: RangeService<DwtMicrosecondClock, I2c1>,
|
||||
}
|
||||
|
||||
#[local]
|
||||
struct Local {
|
||||
imu: ImuService<DwtMicrosecondClock, KakuteMpu6000SpiRoute>,
|
||||
mavlink_encoder: MavlinkEncoder,
|
||||
mavlink_transmitter: MavlinkTransmitter<DcacheDisabledDmaMemory, Usart1TxDma, 4>,
|
||||
led: ActiveLowLedPd2,
|
||||
imu_service_timer: Tim2Periodic,
|
||||
heartbeat_divider: u8,
|
||||
observed_sample: u32,
|
||||
observed_range: u32,
|
||||
lidar_startup_ticks: u8,
|
||||
raw_imu_telemetry_ticks: u8,
|
||||
esc: KakuteMotorOutput,
|
||||
}
|
||||
|
||||
#[init]
|
||||
fn init(mut context: init::Context) -> (Shared, Local) {
|
||||
let device = context.device;
|
||||
if select_pll216_profile(&device.RCC, &device.PWR, &device.FLASH).is_err() {
|
||||
safe_idle();
|
||||
}
|
||||
context.core.SCB.disable_dcache(&mut context.core.CPUID);
|
||||
let clock = match DwtMicrosecondClock::start::<CORE_CLOCK_MHZ>(
|
||||
&mut context.core.DCB,
|
||||
&mut context.core.DWT,
|
||||
) {
|
||||
Ok(clock) => clock,
|
||||
Err(_) => safe_idle(),
|
||||
};
|
||||
|
||||
configure_spi1_pins_pa5_pa6_pa7(&device.RCC, &device.GPIOA);
|
||||
configure_i2c1_pins_pb8_pb9(&device.RCC, &device.GPIOB);
|
||||
configure_exti4_pin_pa4(&device.GPIOA);
|
||||
configure_usart1_tx_pin_pa9(&device.GPIOA);
|
||||
configure_usart1_rx_pin_pa10(&device.GPIOA);
|
||||
let esc = KakuteMotorOutput::new(&device.RCC, device.TIM3, device.TIM5, MOTOR_PROTOCOL);
|
||||
let spi = Spi1::new(&device.RCC, device.SPI1, device.DMA2);
|
||||
let route = KakuteMpu6000SpiRoute::new(&device.RCC, spi, device.GPIOB);
|
||||
let range_clock = clock.fork();
|
||||
let imu = ImuService::new(clock, Mpu6000::new(route, MPU6000_CONFIG));
|
||||
let range = RangeService::new(
|
||||
range_clock,
|
||||
LidarLiteV3::new(I2c1::new(&device.RCC, device.I2C1, I2C1_CONFIG)),
|
||||
LIDAR_ACQUISITION_MODE,
|
||||
);
|
||||
|
||||
let uart = match Usart1Tx::new(&device.RCC, device.USART1, USART1_CLOCK_HZ, USART1_BAUD) {
|
||||
Ok(uart) => uart,
|
||||
Err(_) => safe_idle(),
|
||||
};
|
||||
// SAFETY: D-cache is disabled above and `memory.x` excludes DTCM.
|
||||
let memory = unsafe { DcacheDisabledDmaMemory::new() };
|
||||
let mavlink_transmitter =
|
||||
MavlinkTransmitter::new(memory, Usart1TxDma::new(&device.RCC, uart));
|
||||
|
||||
configure_exti4(&device.RCC, device.SYSCFG, device.EXTI);
|
||||
// The first SPI DMA transaction is submitted by `imu_worker`, after
|
||||
// RTIC has enabled the DMA and SPI IRQs. Initialization itself only
|
||||
// constructs inert hardware owners.
|
||||
IMU_EVENTS.store(IMU_EVENT_START, Ordering::Release);
|
||||
(
|
||||
Shared {
|
||||
mavlink_receiver: MavlinkReceiver::new(),
|
||||
range,
|
||||
},
|
||||
Local {
|
||||
imu,
|
||||
mavlink_encoder: MavlinkEncoder::new(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID),
|
||||
mavlink_transmitter,
|
||||
led: ActiveLowLedPd2::new(&device.RCC, device.GPIOD),
|
||||
imu_service_timer: Tim2Periodic::new(
|
||||
&device.RCC,
|
||||
device.TIM2,
|
||||
IMU_SERVICE_TIMER_CONFIG,
|
||||
),
|
||||
heartbeat_divider: 0,
|
||||
observed_sample: 0,
|
||||
observed_range: 0,
|
||||
lidar_startup_ticks: 0,
|
||||
raw_imu_telemetry_ticks: 0,
|
||||
esc,
|
||||
},
|
||||
)
|
||||
}
|
||||
|
||||
#[idle]
|
||||
fn idle(_: idle::Context) -> ! {
|
||||
loop {
|
||||
cortex_m::asm::wfi();
|
||||
}
|
||||
}
|
||||
|
||||
#[task(binds = EXTI4, priority = 15)]
|
||||
fn exti4(_: exti4::Context) {
|
||||
// SAFETY: this handler exclusively acknowledges the board's EXTI4 bit.
|
||||
unsafe {
|
||||
(&*stm32f722::EXTI::ptr())
|
||||
.pr()
|
||||
.write(|write| write.pr4().clear())
|
||||
};
|
||||
schedule_imu(IMU_EVENT_DRDY);
|
||||
}
|
||||
|
||||
#[task(binds = DMA2_STREAM0, priority = 14)]
|
||||
fn spi_rx_dma(_: spi_rx_dma::Context) {
|
||||
match Spi1::acknowledge_rx_interrupt() {
|
||||
SpiCompletion::Complete => schedule_imu(IMU_EVENT_TRANSFER_COMPLETE),
|
||||
SpiCompletion::TransferFault => schedule_imu(IMU_EVENT_TRANSFER_FAULT),
|
||||
SpiCompletion::NotPending => {}
|
||||
}
|
||||
}
|
||||
|
||||
#[task(binds = DMA2_STREAM3, priority = 14)]
|
||||
fn spi_tx_dma(_: spi_tx_dma::Context) {
|
||||
if Spi1::acknowledge_tx_interrupt() == SpiCompletion::TransferFault {
|
||||
schedule_imu(IMU_EVENT_TRANSFER_FAULT);
|
||||
}
|
||||
}
|
||||
|
||||
#[task(binds = SPI1, priority = 14)]
|
||||
fn spi_error(_: spi_error::Context) {
|
||||
if Spi1::acknowledge_error_interrupt() {
|
||||
schedule_imu(IMU_EVENT_TRANSFER_FAULT);
|
||||
}
|
||||
}
|
||||
|
||||
#[task(binds = TIM3, priority = 12)]
|
||||
fn motor_update(_: motor_update::Context) {
|
||||
KakuteMotorOutput::service_tim3_interrupt();
|
||||
}
|
||||
|
||||
#[task(binds = TIM5, priority = 12)]
|
||||
fn motor_scheduler(_: motor_scheduler::Context) {
|
||||
KakuteMotorOutput::service_tim5_interrupt();
|
||||
}
|
||||
|
||||
#[task(binds = TIM2, priority = 4, shared = [range], local = [imu_service_timer, lidar_startup_ticks, raw_imu_telemetry_ticks])]
|
||||
fn tim2(mut context: tim2::Context) {
|
||||
context.local.imu_service_timer.acknowledge_update();
|
||||
schedule_imu(IMU_EVENT_TIMER);
|
||||
if raw_imu_telemetry_due(context.local.raw_imu_telemetry_ticks) {
|
||||
RAW_IMU_TELEMETRY_DUE.store(true, Ordering::Release);
|
||||
}
|
||||
if *context.local.lidar_startup_ticks < LIDAR_STARTUP_TICKS {
|
||||
*context.local.lidar_startup_ticks += 1;
|
||||
if *context.local.lidar_startup_ticks == LIDAR_STARTUP_TICKS {
|
||||
context.shared.range.lock(|range| {
|
||||
let _ = range.finish_startup();
|
||||
});
|
||||
}
|
||||
} else {
|
||||
context.shared.range.lock(|range| {
|
||||
publish_range_event(range.on_periodic_tick());
|
||||
});
|
||||
}
|
||||
schedule_mavlink(MAVLINK_EVENT_HEARTBEAT);
|
||||
}
|
||||
|
||||
#[task(binds = I2C1_EV, priority = 11, shared = [range])]
|
||||
fn i2c1_event(mut context: i2c1_event::Context) {
|
||||
context.shared.range.lock(|range| {
|
||||
let event = range.on_controller_interrupt(|i2c| i2c.acknowledge_event_interrupt());
|
||||
publish_range_event(event);
|
||||
});
|
||||
}
|
||||
|
||||
#[task(binds = I2C1_ER, priority = 11, shared = [range])]
|
||||
fn i2c1_error(mut context: i2c1_error::Context) {
|
||||
context.shared.range.lock(|range| {
|
||||
let event = range.on_controller_interrupt(|i2c| i2c.acknowledge_error_interrupt());
|
||||
publish_range_event(event);
|
||||
});
|
||||
}
|
||||
|
||||
#[task(binds = DMA2_STREAM7, priority = 10)]
|
||||
fn uart_dma(_: uart_dma::Context) {
|
||||
match Usart1TxDma::acknowledge_interrupt() {
|
||||
TxCompletion::Complete => {
|
||||
UART_COMPLETION.store(1, Ordering::Release);
|
||||
schedule_mavlink(MAVLINK_EVENT_DMA);
|
||||
}
|
||||
TxCompletion::TransferFault | TxCompletion::Halted => {
|
||||
UART_COMPLETION.store(2, Ordering::Release);
|
||||
schedule_mavlink(MAVLINK_EVENT_DMA);
|
||||
}
|
||||
TxCompletion::NotPending => {}
|
||||
}
|
||||
}
|
||||
|
||||
#[task(binds = USART1, priority = 8, shared = [mavlink_receiver])]
|
||||
fn uart_rx(mut context: uart_rx::Context) {
|
||||
match Usart1Tx::acknowledge_receive_interrupt() {
|
||||
kaptan_stm32f7::stm32f722::ReceiveInterrupt::Byte(byte) => {
|
||||
let admitted = context
|
||||
.shared
|
||||
.mavlink_receiver
|
||||
.lock(|receiver| receiver.receive_byte(byte));
|
||||
if admitted {
|
||||
schedule_mavlink(MAVLINK_EVENT_RECEIVED);
|
||||
}
|
||||
}
|
||||
kaptan_stm32f7::stm32f722::ReceiveInterrupt::Fault => {
|
||||
context.shared.mavlink_receiver.lock(MavlinkReceiver::reset);
|
||||
}
|
||||
kaptan_stm32f7::stm32f722::ReceiveInterrupt::NotPending => {}
|
||||
}
|
||||
}
|
||||
|
||||
#[task(priority = 13, local = [imu])]
|
||||
async fn imu_worker(context: imu_worker::Context) {
|
||||
// TODO: Route IMU faults to a dedicated supervisor.
|
||||
let events = IMU_EVENTS.swap(0, Ordering::AcqRel);
|
||||
if events & IMU_EVENT_START != 0 {
|
||||
let _ = context.local.imu.begin();
|
||||
}
|
||||
if events & IMU_EVENT_TRANSFER_FAULT != 0 {
|
||||
let _ = context
|
||||
.local
|
||||
.imu
|
||||
.on_transfer_complete(SpiCompletion::TransferFault);
|
||||
}
|
||||
if events & IMU_EVENT_TRANSFER_COMPLETE != 0 {
|
||||
match context
|
||||
.local
|
||||
.imu
|
||||
.on_transfer_complete(SpiCompletion::Complete)
|
||||
{
|
||||
ImuEvent::Sample(sample) => {
|
||||
SAMPLES.publish(sample);
|
||||
schedule_mavlink(MAVLINK_EVENT_SAMPLE);
|
||||
}
|
||||
ImuEvent::Fault(_) | ImuEvent::Initialized | ImuEvent::Idle => {}
|
||||
}
|
||||
}
|
||||
if events & IMU_EVENT_TIMER != 0 {
|
||||
let _ = context.local.imu.on_periodic_tick();
|
||||
}
|
||||
if events & IMU_EVENT_DRDY != 0 {
|
||||
let _ = context.local.imu.on_data_ready();
|
||||
}
|
||||
IMU_QUEUED.store(false, Ordering::Release);
|
||||
if IMU_EVENTS.load(Ordering::Acquire) != 0
|
||||
&& !IMU_QUEUED.swap(true, Ordering::AcqRel)
|
||||
&& imu_worker::spawn().is_err()
|
||||
{
|
||||
IMU_QUEUED.store(false, Ordering::Release);
|
||||
}
|
||||
}
|
||||
|
||||
#[task(priority = 9, shared = [mavlink_receiver], local = [mavlink_encoder, mavlink_transmitter, led, heartbeat_divider, observed_sample, observed_range, esc])]
|
||||
async fn mavlink_worker(mut context: mavlink_worker::Context) {
|
||||
let events = MAVLINK_EVENTS.swap(0, Ordering::AcqRel);
|
||||
if events & MAVLINK_EVENT_DMA != 0 {
|
||||
let completion = match UART_COMPLETION.swap(0, Ordering::AcqRel) {
|
||||
1 => TxCompletion::Complete,
|
||||
2 => TxCompletion::TransferFault,
|
||||
_ => TxCompletion::NotPending,
|
||||
};
|
||||
context
|
||||
.local
|
||||
.mavlink_transmitter
|
||||
.on_dma_complete(completion);
|
||||
}
|
||||
if events & MAVLINK_EVENT_SAMPLE != 0
|
||||
&& RAW_IMU_TELEMETRY_DUE.swap(false, Ordering::AcqRel)
|
||||
&& let Some(sample) = SAMPLES.take_newer(context.local.observed_sample)
|
||||
{
|
||||
// MAVLink transmission never blocks flight work. A later sample
|
||||
// or heartbeat gives the bounded queue another chance to send.
|
||||
let _ = context.local.mavlink_transmitter.send_frame(
|
||||
FrameClass::BestEffort,
|
||||
context.local.mavlink_encoder.raw_imu_snapshot(&sample),
|
||||
);
|
||||
}
|
||||
if events & MAVLINK_EVENT_RANGE != 0
|
||||
&& let Some(sample) = RANGES.take_newer(context.local.observed_range)
|
||||
{
|
||||
// A range observation is control-relevant and must not be
|
||||
// perpetually displaced by the much higher-rate raw IMU stream.
|
||||
// Critical admission may evict pending best-effort IMU traffic,
|
||||
// while an already-active DMA transfer remains untouched.
|
||||
let _ = context.local.mavlink_transmitter.send_frame(
|
||||
FrameClass::Critical,
|
||||
context.local.mavlink_encoder.distance_sensor_snapshot(
|
||||
&sample,
|
||||
LIDAR_SENSOR_ID,
|
||||
LIDAR_ORIENTATION,
|
||||
LIDAR_MIN_DISTANCE_CENTIMETRES,
|
||||
LIDAR_MAX_DISTANCE_CENTIMETRES,
|
||||
),
|
||||
);
|
||||
}
|
||||
if events & MAVLINK_EVENT_HEARTBEAT != 0 {
|
||||
*context.local.heartbeat_divider = context.local.heartbeat_divider.wrapping_add(1);
|
||||
if *context.local.heartbeat_divider == 100 {
|
||||
*context.local.heartbeat_divider = 0;
|
||||
let _ = context.local.mavlink_transmitter.send_frame(
|
||||
FrameClass::Critical,
|
||||
context.local.mavlink_encoder.heartbeat(
|
||||
kaptan_mavlink::message::MavState::MAV_STATE_STANDBY,
|
||||
context.local.esc.is_armed(),
|
||||
),
|
||||
);
|
||||
context.local.led.toggle();
|
||||
}
|
||||
}
|
||||
if events & MAVLINK_EVENT_RECEIVED != 0 {
|
||||
while let Some(frame) = context
|
||||
.shared
|
||||
.mavlink_receiver
|
||||
.lock(MavlinkReceiver::take_received)
|
||||
{
|
||||
if let Some(request) =
|
||||
decode_arm_request(&frame, MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID)
|
||||
{
|
||||
// TODO: Acknowledge arm only after output activation is confirmed.
|
||||
let _ = context.local.mavlink_transmitter.send_frame(
|
||||
FrameClass::Critical,
|
||||
context.local.mavlink_encoder.command_ack(
|
||||
kaptan_mavlink::message::MavCmd::MAV_CMD_COMPONENT_ARM_DISARM,
|
||||
kaptan_mavlink::message::MavResult::MAV_RESULT_ACCEPTED,
|
||||
),
|
||||
);
|
||||
match request {
|
||||
ArmRequest::Arm => {
|
||||
context.local.esc.arm();
|
||||
}
|
||||
ArmRequest::Disarm => {
|
||||
context.local.esc.disarm();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
MAVLINK_QUEUED.store(false, Ordering::Release);
|
||||
if MAVLINK_EVENTS.load(Ordering::Acquire) != 0
|
||||
&& !MAVLINK_QUEUED.swap(true, Ordering::AcqRel)
|
||||
&& mavlink_worker::spawn().is_err()
|
||||
{
|
||||
MAVLINK_QUEUED.store(false, Ordering::Release);
|
||||
}
|
||||
}
|
||||
|
||||
fn schedule_imu(event: u8) {
|
||||
IMU_EVENTS.fetch_or(event, Ordering::Release);
|
||||
if !IMU_QUEUED.swap(true, Ordering::AcqRel) && imu_worker::spawn().is_err() {
|
||||
IMU_QUEUED.store(false, Ordering::Release);
|
||||
}
|
||||
}
|
||||
|
||||
/// Advances the raw-IMU admission divider.
|
||||
///
|
||||
/// Starting from zero, exactly twenty of every one hundred 100 Hz ticks
|
||||
/// return `true`; keeping the initial permit false avoids a boot-time
|
||||
/// twenty-first transmission.
|
||||
fn raw_imu_telemetry_due(divider: &mut u8) -> bool {
|
||||
*divider = divider.wrapping_add(1);
|
||||
if *divider == MAVLINK_RAW_IMU_TICK_DIVISOR {
|
||||
*divider = 0;
|
||||
true
|
||||
} else {
|
||||
false
|
||||
}
|
||||
}
|
||||
|
||||
fn schedule_mavlink(event: u8) {
|
||||
MAVLINK_EVENTS.fetch_or(event, Ordering::Release);
|
||||
if !MAVLINK_QUEUED.swap(true, Ordering::AcqRel) && mavlink_worker::spawn().is_err() {
|
||||
MAVLINK_QUEUED.store(false, Ordering::Release);
|
||||
}
|
||||
}
|
||||
|
||||
fn publish_range_event(event: RangeEvent<kaptan_stm32f7::stm32f722::I2c1Error>) {
|
||||
if let RangeEvent::Measurement(sample) = event {
|
||||
RANGES.publish(sample);
|
||||
schedule_mavlink(MAVLINK_EVENT_RANGE);
|
||||
}
|
||||
}
|
||||
|
||||
fn configure_exti4(rcc: &stm32f722::RCC, syscfg: stm32f722::SYSCFG, exti: stm32f722::EXTI) {
|
||||
rcc.apb2enr().modify(|_, write| write.syscfgen().set_bit());
|
||||
syscfg.exticr2().modify(|_, write| {
|
||||
// SAFETY: EXTI4 is connected to GPIOA by selector value zero.
|
||||
unsafe { write.exti4().bits(0) }
|
||||
});
|
||||
exti.imr().modify(|_, write| write.im4().set_bit());
|
||||
exti.rtsr().modify(|_, write| write.tr4().set_bit());
|
||||
exti.ftsr().modify(|_, write| write.tr4().clear_bit());
|
||||
exti.pr().write(|write| write.pr4().clear());
|
||||
}
|
||||
|
||||
fn safe_idle() -> ! {
|
||||
loop {
|
||||
cortex_m::asm::wfi();
|
||||
}
|
||||
}
|
||||
}
|
||||
platform_stm32f7::flight_firmware!(board::KakuteF7MiniV3);
|
||||
@@ -1,72 +0,0 @@
|
||||
//! Static electrical routes selected by the Kakute F7 Mini V3.
|
||||
//!
|
||||
//! The board describes which controller and pins are connected. Protocol
|
||||
//! framing and controller state remain in their respective reusable crates.
|
||||
|
||||
use kaptan_spi::{SpiCompletion, SpiDevice};
|
||||
use kaptan_stm32f7::stm32f722::{Spi1, Spi1Error};
|
||||
use stm32f7::stm32f722;
|
||||
|
||||
/// Chip-select-scoped SPI1/PB2 route wired to the MPU6000.
|
||||
pub struct KakuteMpu6000SpiRoute {
|
||||
spi: Spi1,
|
||||
chip_select: stm32f722::GPIOB,
|
||||
}
|
||||
|
||||
impl KakuteMpu6000SpiRoute {
|
||||
/// Configures board-selected PB2 chip select and combines it with SPI1.
|
||||
pub(crate) fn new(rcc: &stm32f722::RCC, spi: Spi1, chip_select: stm32f722::GPIOB) -> Self {
|
||||
rcc.ahb1enr().modify(|_, write| write.gpioben().set_bit());
|
||||
chip_select
|
||||
.moder()
|
||||
.modify(|_, write| write.moder2().output());
|
||||
chip_select
|
||||
.otyper()
|
||||
.modify(|_, write| write.ot2().push_pull());
|
||||
chip_select
|
||||
.ospeedr()
|
||||
.modify(|_, write| write.ospeedr2().low_speed());
|
||||
chip_select
|
||||
.pupdr()
|
||||
.modify(|_, write| write.pupdr2().floating());
|
||||
chip_select.bsrr().write(|write| write.bs2().set_bit());
|
||||
|
||||
Self { spi, chip_select }
|
||||
}
|
||||
|
||||
fn select(&self) {
|
||||
self.chip_select.bsrr().write(|write| write.br2().set_bit());
|
||||
}
|
||||
|
||||
fn deselect(&self) {
|
||||
self.chip_select.bsrr().write(|write| write.bs2().set_bit());
|
||||
}
|
||||
}
|
||||
|
||||
impl SpiDevice for KakuteMpu6000SpiRoute {
|
||||
type StartError = Spi1Error;
|
||||
|
||||
fn start_transfer(
|
||||
&mut self,
|
||||
transmit: &[u8],
|
||||
receive: &mut [u8],
|
||||
) -> Result<(), Self::StartError> {
|
||||
self.select();
|
||||
if let Err(error) = self.spi.start_transfer(transmit, receive) {
|
||||
self.deselect();
|
||||
return Err(error);
|
||||
}
|
||||
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn complete_transfer(&mut self, completion: SpiCompletion) {
|
||||
self.spi.complete_transfer(completion);
|
||||
if matches!(
|
||||
completion,
|
||||
SpiCompletion::Complete | SpiCompletion::TransferFault
|
||||
) {
|
||||
self.deselect();
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user