public/kaptan
1
Fork 0
Code Issues 24 Pull Requests Actions Packages Projects Releases Wiki Activity

support bi-directional mavlink

Browse Source 
Signed-off-by: Onur Özkan <work@onurozkan.dev>
This commit is contained in:
Onur Özkan committed 2026-07-21 13:43:07 +03:00
1 parent 2d35d935b4
commit d32d9f4d36
16 files changed
+651 -344
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

boards/kakute-f7-mini-v3/src/main.rs
+107 -53
View File
@@ -14,14 +14,15 @@ const IMU_EVENT_DRDY: u8 = 1 << 0;
const IMU_EVENT_TRANSFER_COMPLETE: u8 = 1 << 1;
const IMU_EVENT_TRANSFER_FAULT: u8 = 1 << 2;
const IMU_EVENT_TIMER: u8 = 1 << 3;
const TELEMETRY_EVENT_SAMPLE: u8 = 1 << 0;
const TELEMETRY_EVENT_HEARTBEAT: u8 = 1 << 1;
const TELEMETRY_EVENT_DMA: u8 = 1 << 2;
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;
static IMU_EVENTS: AtomicU8 = AtomicU8::new(0);
static TELEMETRY_EVENTS: AtomicU8 = AtomicU8::new(0);
static MAVLINK_EVENTS: AtomicU8 = AtomicU8::new(0);
static IMU_QUEUED: AtomicBool = AtomicBool::new(false);
static TELEMETRY_QUEUED: AtomicBool = AtomicBool::new(false);
static MAVLINK_QUEUED: AtomicBool = AtomicBool::new(false);
static SAMPLES: LatestImuMailbox = LatestImuMailbox::new();
static UART_COMPLETION: AtomicU8 = AtomicU8::new(0);
@@ -29,8 +30,8 @@ static UART_COMPLETION: AtomicU8 = AtomicU8::new(0);
mod app {
use super::{
IMU_EVENT_DRDY, IMU_EVENT_TIMER, IMU_EVENT_TRANSFER_COMPLETE, IMU_EVENT_TRANSFER_FAULT,
IMU_EVENTS, IMU_QUEUED, Ordering, SAMPLES, TELEMETRY_EVENT_DMA, TELEMETRY_EVENT_HEARTBEAT,
TELEMETRY_EVENT_SAMPLE, TELEMETRY_EVENTS, TELEMETRY_QUEUED, UART_COMPLETION,
IMU_EVENTS, IMU_QUEUED, MAVLINK_EVENT_DMA, MAVLINK_EVENT_HEARTBEAT, MAVLINK_EVENT_RECEIVED,
MAVLINK_EVENT_SAMPLE, MAVLINK_EVENTS, MAVLINK_QUEUED, Ordering, SAMPLES, UART_COMPLETION,
};
use crate::board::{
CORE_CLOCK_MHZ, HEARTBEAT_TIMER_CONFIG, MAVLINK_COMPONENT_ID, MAVLINK_SYSTEM_ID,
@@ -39,22 +40,30 @@ mod app {
use crate::routes::KakuteMpu6000SpiRoute;
use kaptan_dma::TxCompletion;
use kaptan_drivers::mpu6000::Mpu6000;
use kaptan_runtime::{ImuEvent, ImuService, TelemetryService};
use kaptan_mavlink::{
encoder::MavlinkEncoder,
receiver::MavlinkReceiver,
transmitter::{FrameClass, MavlinkTransmitter},
};
use kaptan_runtime::{ImuEvent, ImuService};
use kaptan_spi::SpiCompletion;
use kaptan_stm32f7::stm32f722::{
ActiveLowLedPd2, DcacheDisabledDmaMemory, DwtMicrosecondClock, Spi1, Tim2Periodic,
Usart1Tx, Usart1TxDma, configure_exti4_pin_pa4, configure_spi1_pins_pa5_pa6_pa7,
configure_usart1_tx_pin_pa9, select_hsi16_profile,
configure_usart1_rx_pin_pa10, configure_usart1_tx_pin_pa9, select_hsi16_profile,
};
use stm32f7::stm32f722;
#[shared]
struct Shared {}
struct Shared {
mavlink_receiver: MavlinkReceiver<4>,
}
#[local]
struct Local {
imu: ImuService<DwtMicrosecondClock, KakuteMpu6000SpiRoute>,
telemetry: TelemetryService<DcacheDisabledDmaMemory, Usart1TxDma, 4>,
mavlink_encoder: MavlinkEncoder,
mavlink_transmitter: MavlinkTransmitter<DcacheDisabledDmaMemory, Usart1TxDma, 4>,
led: ActiveLowLedPd2,
heartbeat_timer: Tim2Periodic,
heartbeat_divider: u8,
@@ -79,6 +88,7 @@ mod app {
configure_spi1_pins_pa5_pa6_pa7(&device.RCC, &device.GPIOA);
configure_exti4_pin_pa4(&device.GPIOA);
configure_usart1_tx_pin_pa9(&device.GPIOA);
configure_usart1_rx_pin_pa10(&device.GPIOA);
let spi = Spi1::new(&device.RCC, device.SPI1, device.DMA2);
let route = KakuteMpu6000SpiRoute::new(&device.RCC, spi, device.GPIOB);
let mut imu = ImuService::new(clock, Mpu6000::new(route, MPU6000_CONFIG));
@@ -92,19 +102,18 @@ mod app {
};
// SAFETY: D-cache is disabled above and `memory.x` excludes DTCM.
let memory = unsafe { DcacheDisabledDmaMemory::new() };
let telemetry = TelemetryService::new(
MAVLINK_SYSTEM_ID,
MAVLINK_COMPONENT_ID,
memory,
Usart1TxDma::new(&device.RCC, uart),
);
let mavlink_transmitter =
MavlinkTransmitter::new(memory, Usart1TxDma::new(&device.RCC, uart));
configure_exti4(&device.RCC, device.SYSCFG, device.EXTI);
(
Shared {},
Shared {
mavlink_receiver: MavlinkReceiver::new(),
},
Local {
imu,
telemetry,
mavlink_encoder: MavlinkEncoder::new(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID),
mavlink_transmitter,
led: ActiveLowLedPd2::new(&device.RCC, device.GPIOD),
heartbeat_timer: Tim2Periodic::new(
&device.RCC,
@@ -155,7 +164,7 @@ mod app {
fn tim2(context: tim2::Context) {
context.local.heartbeat_timer.acknowledge_update();
schedule_imu(IMU_EVENT_TIMER);
schedule_telemetry(TELEMETRY_EVENT_HEARTBEAT);
schedule_mavlink(MAVLINK_EVENT_HEARTBEAT);
}
#[task(binds = DMA2_STREAM7, priority = 10)]
@@ -163,16 +172,35 @@ mod app {
match Usart1TxDma::acknowledge_interrupt() {
TxCompletion::Complete => {
UART_COMPLETION.store(1, Ordering::Release);
schedule_telemetry(TELEMETRY_EVENT_DMA);
schedule_mavlink(MAVLINK_EVENT_DMA);
}
TxCompletion::TransferFault | TxCompletion::Halted => {
UART_COMPLETION.store(2, Ordering::Release);
schedule_telemetry(TELEMETRY_EVENT_DMA);
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) {
let events = IMU_EVENTS.swap(0, Ordering::AcqRel);
@@ -190,7 +218,7 @@ mod app {
{
ImuEvent::Sample(sample) => {
SAMPLES.publish(sample);
schedule_telemetry(TELEMETRY_EVENT_SAMPLE);
schedule_mavlink(MAVLINK_EVENT_SAMPLE);
}
ImuEvent::Fault(_) | ImuEvent::Initialized | ImuEvent::Idle => {}
}
@@ -202,47 +230,72 @@ mod app {
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) {
if imu_worker::spawn().is_err() {
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, local = [telemetry, led, heartbeat_divider, observed_sample])]
async fn telemetry_worker(context: telemetry_worker::Context) {
let events = TELEMETRY_EVENTS.swap(0, Ordering::AcqRel);
if events & TELEMETRY_EVENT_DMA != 0 {
#[task(priority = 9, shared = [mavlink_receiver], local = [mavlink_encoder, mavlink_transmitter, led, heartbeat_divider, observed_sample])]
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.telemetry.on_dma_complete(completion);
context
.local
.mavlink_transmitter
.on_dma_complete(completion);
}
if events & TELEMETRY_EVENT_SAMPLE != 0 {
if let Some(sample) = SAMPLES.take_newer(context.local.observed_sample) {
// Telemetry never blocks flight work. A later sample or
// heartbeat gives the bounded queue another chance to send.
let _ = context.local.telemetry.publish_sample(&sample);
}
if events & MAVLINK_EVENT_SAMPLE != 0
&& 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 & TELEMETRY_EVENT_HEARTBEAT != 0 {
if events & MAVLINK_EVENT_HEARTBEAT != 0 {
*context.local.heartbeat_divider = context.local.heartbeat_divider.wrapping_add(1);
if *context.local.heartbeat_divider == 10 {
*context.local.heartbeat_divider = 0;
let _ = context.local.telemetry.publish_heartbeat();
let _ = context.local.mavlink_transmitter.send_frame(
FrameClass::Critical,
context
.local
.mavlink_encoder
.heartbeat(kaptan_mavlink::message::MavState::MAV_STATE_STANDBY),
);
context.local.led.toggle();
}
}
TELEMETRY_QUEUED.store(false, Ordering::Release);
if TELEMETRY_EVENTS.load(Ordering::Acquire) != 0
&& !TELEMETRY_QUEUED.swap(true, Ordering::AcqRel)
{
if telemetry_worker::spawn().is_err() {
TELEMETRY_QUEUED.store(false, Ordering::Release);
if events & MAVLINK_EVENT_RECEIVED != 0 {
while let Some(frame) = context
.shared
.mavlink_receiver
.lock(MavlinkReceiver::take_received)
{
// TODO: Echo the frame for testing.
let _ = context
.local
.mavlink_transmitter
.send_frame(FrameClass::Critical, frame);
}
}
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) {
@@ -252,18 +305,19 @@ mod app {
}
}
fn schedule_telemetry(event: u8) {
TELEMETRY_EVENTS.fetch_or(event, Ordering::Release);
if !TELEMETRY_QUEUED.swap(true, Ordering::AcqRel) && telemetry_worker::spawn().is_err() {
TELEMETRY_QUEUED.store(false, Ordering::Release);
fn schedule_mavlink(event: u8) {
MAVLINK_EVENTS.fetch_or(event, Ordering::Release);
if !MAVLINK_QUEUED.swap(true, Ordering::AcqRel) && mavlink_worker::spawn().is_err() {
MAVLINK_QUEUED.store(false, Ordering::Release);
}
}
fn configure_exti4(rcc: &stm32f722::RCC, syscfg: stm32f722::SYSCFG, exti: stm32f722::EXTI) {
rcc.apb2enr().modify(|_, write| write.syscfgen().set_bit());
syscfg
.exticr2()
.modify(|_, write| unsafe { write.exti4().bits(0) });
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());