support bi-directional mavlink
Signed-off-by: Onur Özkan <work@onurozkan.dev>
This commit is contained in:
1 parent
2d35d935b4
commit
d32d9f4d36
16 files changed
+651
-344
No files matched your search
Generated
+3
-3
@@ -156,6 +156,7 @@ dependencies = [
|
|||||||
"cortex-m-rt",
|
"cortex-m-rt",
|
||||||
"kaptan-dma",
|
"kaptan-dma",
|
||||||
"kaptan-drivers",
|
"kaptan-drivers",
|
||||||
|
"kaptan-mavlink",
|
||||||
"kaptan-runtime",
|
"kaptan-runtime",
|
||||||
"kaptan-spi",
|
"kaptan-spi",
|
||||||
"kaptan-stm32f7",
|
"kaptan-stm32f7",
|
||||||
@@ -168,8 +169,10 @@ dependencies = [
|
|||||||
name = "kaptan-mavlink"
|
name = "kaptan-mavlink"
|
||||||
version = "0.1.0"
|
version = "0.1.0"
|
||||||
dependencies = [
|
dependencies = [
|
||||||
|
"kaptan-dma",
|
||||||
"kaptan-sensor",
|
"kaptan-sensor",
|
||||||
"kaptan-time",
|
"kaptan-time",
|
||||||
|
"kaptan-tx",
|
||||||
"mavlink",
|
"mavlink",
|
||||||
"mavlink-bindgen",
|
"mavlink-bindgen",
|
||||||
"mavlink-core",
|
"mavlink-core",
|
||||||
@@ -179,13 +182,10 @@ dependencies = [
|
|||||||
name = "kaptan-runtime"
|
name = "kaptan-runtime"
|
||||||
version = "0.1.0"
|
version = "0.1.0"
|
||||||
dependencies = [
|
dependencies = [
|
||||||
"kaptan-dma",
|
|
||||||
"kaptan-drivers",
|
"kaptan-drivers",
|
||||||
"kaptan-mavlink",
|
|
||||||
"kaptan-sensor",
|
"kaptan-sensor",
|
||||||
"kaptan-spi",
|
"kaptan-spi",
|
||||||
"kaptan-time",
|
"kaptan-time",
|
||||||
"kaptan-tx",
|
|
||||||
]
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
|
|||||||
@@ -17,6 +17,7 @@ cortex-m = "0.7"
|
|||||||
cortex-m-rt = "0.7"
|
cortex-m-rt = "0.7"
|
||||||
kaptan-drivers = { path = "../../libs/kaptan-drivers" }
|
kaptan-drivers = { path = "../../libs/kaptan-drivers" }
|
||||||
kaptan-dma = { path = "../../libs/kaptan-dma" }
|
kaptan-dma = { path = "../../libs/kaptan-dma" }
|
||||||
|
kaptan-mavlink = { path = "../../libs/kaptan-mavlink" }
|
||||||
kaptan-runtime = { path = "../../libs/kaptan-runtime" }
|
kaptan-runtime = { path = "../../libs/kaptan-runtime" }
|
||||||
kaptan-spi = { path = "../../libs/kaptan-spi" }
|
kaptan-spi = { path = "../../libs/kaptan-spi" }
|
||||||
kaptan-stm32f7 = { path = "../../libs/kaptan-stm32f7", features = ["stm32f722"] }
|
kaptan-stm32f7 = { path = "../../libs/kaptan-stm32f7", features = ["stm32f722"] }
|
||||||
@@ -29,4 +30,4 @@ name = "kaptan-kakute-f7-mini-v3"
|
|||||||
path = "src/main.rs"
|
path = "src/main.rs"
|
||||||
required-features = ["firmware"]
|
required-features = ["firmware"]
|
||||||
test = false
|
test = false
|
||||||
bench = false
|
bench = false
|
||||||
@@ -42,9 +42,11 @@ dfu-util --alt 0 --dfuse-address 0x08000000:leave \
|
|||||||
| `GND` | `G` |
|
| `GND` | `G` |
|
||||||
| `TX` | `R1` |
|
| `TX` | `R1` |
|
||||||
|
|
||||||
The transmitter emits unsigned MAVLink 2 at 115200 baud, 8N1. It is binary
|
The firmware emits unsigned MAVLink 2 at 115200 baud, 8N1. It is binary
|
||||||
data, not terminal text. Read it with the python decoder:
|
data, not terminal text. Read it with the python decoder:
|
||||||
|
|
||||||
```sh
|
```sh
|
||||||
./scripts/read-mavlink.py $serial_device_path
|
./scripts/test-mavlink.py $serial_device_path
|
||||||
```
|
```
|
||||||
|
|
||||||
|
The test helper sends MAVLink `TIMESYNC` and`PING` frames while printing all received MAVLink messages.
|
||||||
@@ -14,14 +14,15 @@ const IMU_EVENT_DRDY: u8 = 1 << 0;
|
|||||||
const IMU_EVENT_TRANSFER_COMPLETE: u8 = 1 << 1;
|
const IMU_EVENT_TRANSFER_COMPLETE: u8 = 1 << 1;
|
||||||
const IMU_EVENT_TRANSFER_FAULT: u8 = 1 << 2;
|
const IMU_EVENT_TRANSFER_FAULT: u8 = 1 << 2;
|
||||||
const IMU_EVENT_TIMER: u8 = 1 << 3;
|
const IMU_EVENT_TIMER: u8 = 1 << 3;
|
||||||
const TELEMETRY_EVENT_SAMPLE: u8 = 1 << 0;
|
const MAVLINK_EVENT_SAMPLE: u8 = 1 << 0;
|
||||||
const TELEMETRY_EVENT_HEARTBEAT: u8 = 1 << 1;
|
const MAVLINK_EVENT_HEARTBEAT: u8 = 1 << 1;
|
||||||
const TELEMETRY_EVENT_DMA: u8 = 1 << 2;
|
const MAVLINK_EVENT_DMA: u8 = 1 << 2;
|
||||||
|
const MAVLINK_EVENT_RECEIVED: u8 = 1 << 3;
|
||||||
|
|
||||||
static IMU_EVENTS: AtomicU8 = AtomicU8::new(0);
|
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 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 SAMPLES: LatestImuMailbox = LatestImuMailbox::new();
|
||||||
static UART_COMPLETION: AtomicU8 = AtomicU8::new(0);
|
static UART_COMPLETION: AtomicU8 = AtomicU8::new(0);
|
||||||
|
|
||||||
@@ -29,8 +30,8 @@ static UART_COMPLETION: AtomicU8 = AtomicU8::new(0);
|
|||||||
mod app {
|
mod app {
|
||||||
use super::{
|
use super::{
|
||||||
IMU_EVENT_DRDY, IMU_EVENT_TIMER, IMU_EVENT_TRANSFER_COMPLETE, IMU_EVENT_TRANSFER_FAULT,
|
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,
|
IMU_EVENTS, IMU_QUEUED, MAVLINK_EVENT_DMA, MAVLINK_EVENT_HEARTBEAT, MAVLINK_EVENT_RECEIVED,
|
||||||
TELEMETRY_EVENT_SAMPLE, TELEMETRY_EVENTS, TELEMETRY_QUEUED, UART_COMPLETION,
|
MAVLINK_EVENT_SAMPLE, MAVLINK_EVENTS, MAVLINK_QUEUED, Ordering, SAMPLES, UART_COMPLETION,
|
||||||
};
|
};
|
||||||
use crate::board::{
|
use crate::board::{
|
||||||
CORE_CLOCK_MHZ, HEARTBEAT_TIMER_CONFIG, MAVLINK_COMPONENT_ID, MAVLINK_SYSTEM_ID,
|
CORE_CLOCK_MHZ, HEARTBEAT_TIMER_CONFIG, MAVLINK_COMPONENT_ID, MAVLINK_SYSTEM_ID,
|
||||||
@@ -39,22 +40,30 @@ mod app {
|
|||||||
use crate::routes::KakuteMpu6000SpiRoute;
|
use crate::routes::KakuteMpu6000SpiRoute;
|
||||||
use kaptan_dma::TxCompletion;
|
use kaptan_dma::TxCompletion;
|
||||||
use kaptan_drivers::mpu6000::Mpu6000;
|
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_spi::SpiCompletion;
|
||||||
use kaptan_stm32f7::stm32f722::{
|
use kaptan_stm32f7::stm32f722::{
|
||||||
ActiveLowLedPd2, DcacheDisabledDmaMemory, DwtMicrosecondClock, Spi1, Tim2Periodic,
|
ActiveLowLedPd2, DcacheDisabledDmaMemory, DwtMicrosecondClock, Spi1, Tim2Periodic,
|
||||||
Usart1Tx, Usart1TxDma, configure_exti4_pin_pa4, configure_spi1_pins_pa5_pa6_pa7,
|
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;
|
use stm32f7::stm32f722;
|
||||||
|
|
||||||
#[shared]
|
#[shared]
|
||||||
struct Shared {}
|
struct Shared {
|
||||||
|
mavlink_receiver: MavlinkReceiver<4>,
|
||||||
|
}
|
||||||
|
|
||||||
#[local]
|
#[local]
|
||||||
struct Local {
|
struct Local {
|
||||||
imu: ImuService<DwtMicrosecondClock, KakuteMpu6000SpiRoute>,
|
imu: ImuService<DwtMicrosecondClock, KakuteMpu6000SpiRoute>,
|
||||||
telemetry: TelemetryService<DcacheDisabledDmaMemory, Usart1TxDma, 4>,
|
mavlink_encoder: MavlinkEncoder,
|
||||||
|
mavlink_transmitter: MavlinkTransmitter<DcacheDisabledDmaMemory, Usart1TxDma, 4>,
|
||||||
led: ActiveLowLedPd2,
|
led: ActiveLowLedPd2,
|
||||||
heartbeat_timer: Tim2Periodic,
|
heartbeat_timer: Tim2Periodic,
|
||||||
heartbeat_divider: u8,
|
heartbeat_divider: u8,
|
||||||
@@ -79,6 +88,7 @@ mod app {
|
|||||||
configure_spi1_pins_pa5_pa6_pa7(&device.RCC, &device.GPIOA);
|
configure_spi1_pins_pa5_pa6_pa7(&device.RCC, &device.GPIOA);
|
||||||
configure_exti4_pin_pa4(&device.GPIOA);
|
configure_exti4_pin_pa4(&device.GPIOA);
|
||||||
configure_usart1_tx_pin_pa9(&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 spi = Spi1::new(&device.RCC, device.SPI1, device.DMA2);
|
||||||
let route = KakuteMpu6000SpiRoute::new(&device.RCC, spi, device.GPIOB);
|
let route = KakuteMpu6000SpiRoute::new(&device.RCC, spi, device.GPIOB);
|
||||||
let mut imu = ImuService::new(clock, Mpu6000::new(route, MPU6000_CONFIG));
|
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.
|
// SAFETY: D-cache is disabled above and `memory.x` excludes DTCM.
|
||||||
let memory = unsafe { DcacheDisabledDmaMemory::new() };
|
let memory = unsafe { DcacheDisabledDmaMemory::new() };
|
||||||
let telemetry = TelemetryService::new(
|
let mavlink_transmitter =
|
||||||
MAVLINK_SYSTEM_ID,
|
MavlinkTransmitter::new(memory, Usart1TxDma::new(&device.RCC, uart));
|
||||||
MAVLINK_COMPONENT_ID,
|
|
||||||
memory,
|
|
||||||
Usart1TxDma::new(&device.RCC, uart),
|
|
||||||
);
|
|
||||||
|
|
||||||
configure_exti4(&device.RCC, device.SYSCFG, device.EXTI);
|
configure_exti4(&device.RCC, device.SYSCFG, device.EXTI);
|
||||||
(
|
(
|
||||||
Shared {},
|
Shared {
|
||||||
|
mavlink_receiver: MavlinkReceiver::new(),
|
||||||
|
},
|
||||||
Local {
|
Local {
|
||||||
imu,
|
imu,
|
||||||
telemetry,
|
mavlink_encoder: MavlinkEncoder::new(MAVLINK_SYSTEM_ID, MAVLINK_COMPONENT_ID),
|
||||||
|
mavlink_transmitter,
|
||||||
led: ActiveLowLedPd2::new(&device.RCC, device.GPIOD),
|
led: ActiveLowLedPd2::new(&device.RCC, device.GPIOD),
|
||||||
heartbeat_timer: Tim2Periodic::new(
|
heartbeat_timer: Tim2Periodic::new(
|
||||||
&device.RCC,
|
&device.RCC,
|
||||||
@@ -155,7 +164,7 @@ mod app {
|
|||||||
fn tim2(context: tim2::Context) {
|
fn tim2(context: tim2::Context) {
|
||||||
context.local.heartbeat_timer.acknowledge_update();
|
context.local.heartbeat_timer.acknowledge_update();
|
||||||
schedule_imu(IMU_EVENT_TIMER);
|
schedule_imu(IMU_EVENT_TIMER);
|
||||||
schedule_telemetry(TELEMETRY_EVENT_HEARTBEAT);
|
schedule_mavlink(MAVLINK_EVENT_HEARTBEAT);
|
||||||
}
|
}
|
||||||
|
|
||||||
#[task(binds = DMA2_STREAM7, priority = 10)]
|
#[task(binds = DMA2_STREAM7, priority = 10)]
|
||||||
@@ -163,16 +172,35 @@ mod app {
|
|||||||
match Usart1TxDma::acknowledge_interrupt() {
|
match Usart1TxDma::acknowledge_interrupt() {
|
||||||
TxCompletion::Complete => {
|
TxCompletion::Complete => {
|
||||||
UART_COMPLETION.store(1, Ordering::Release);
|
UART_COMPLETION.store(1, Ordering::Release);
|
||||||
schedule_telemetry(TELEMETRY_EVENT_DMA);
|
schedule_mavlink(MAVLINK_EVENT_DMA);
|
||||||
}
|
}
|
||||||
TxCompletion::TransferFault | TxCompletion::Halted => {
|
TxCompletion::TransferFault | TxCompletion::Halted => {
|
||||||
UART_COMPLETION.store(2, Ordering::Release);
|
UART_COMPLETION.store(2, Ordering::Release);
|
||||||
schedule_telemetry(TELEMETRY_EVENT_DMA);
|
schedule_mavlink(MAVLINK_EVENT_DMA);
|
||||||
}
|
}
|
||||||
TxCompletion::NotPending => {}
|
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])]
|
#[task(priority = 13, local = [imu])]
|
||||||
async fn imu_worker(context: imu_worker::Context) {
|
async fn imu_worker(context: imu_worker::Context) {
|
||||||
let events = IMU_EVENTS.swap(0, Ordering::AcqRel);
|
let events = IMU_EVENTS.swap(0, Ordering::AcqRel);
|
||||||
@@ -190,7 +218,7 @@ mod app {
|
|||||||
{
|
{
|
||||||
ImuEvent::Sample(sample) => {
|
ImuEvent::Sample(sample) => {
|
||||||
SAMPLES.publish(sample);
|
SAMPLES.publish(sample);
|
||||||
schedule_telemetry(TELEMETRY_EVENT_SAMPLE);
|
schedule_mavlink(MAVLINK_EVENT_SAMPLE);
|
||||||
}
|
}
|
||||||
ImuEvent::Fault(_) | ImuEvent::Initialized | ImuEvent::Idle => {}
|
ImuEvent::Fault(_) | ImuEvent::Initialized | ImuEvent::Idle => {}
|
||||||
}
|
}
|
||||||
@@ -202,47 +230,72 @@ mod app {
|
|||||||
let _ = context.local.imu.on_data_ready();
|
let _ = context.local.imu.on_data_ready();
|
||||||
}
|
}
|
||||||
IMU_QUEUED.store(false, Ordering::Release);
|
IMU_QUEUED.store(false, Ordering::Release);
|
||||||
if IMU_EVENTS.load(Ordering::Acquire) != 0 && !IMU_QUEUED.swap(true, Ordering::AcqRel) {
|
if IMU_EVENTS.load(Ordering::Acquire) != 0
|
||||||
if imu_worker::spawn().is_err() {
|
&& !IMU_QUEUED.swap(true, Ordering::AcqRel)
|
||||||
IMU_QUEUED.store(false, Ordering::Release);
|
&& imu_worker::spawn().is_err()
|
||||||
}
|
{
|
||||||
|
IMU_QUEUED.store(false, Ordering::Release);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
#[task(priority = 9, local = [telemetry, led, heartbeat_divider, observed_sample])]
|
#[task(priority = 9, shared = [mavlink_receiver], local = [mavlink_encoder, mavlink_transmitter, led, heartbeat_divider, observed_sample])]
|
||||||
async fn telemetry_worker(context: telemetry_worker::Context) {
|
async fn mavlink_worker(mut context: mavlink_worker::Context) {
|
||||||
let events = TELEMETRY_EVENTS.swap(0, Ordering::AcqRel);
|
let events = MAVLINK_EVENTS.swap(0, Ordering::AcqRel);
|
||||||
if events & TELEMETRY_EVENT_DMA != 0 {
|
if events & MAVLINK_EVENT_DMA != 0 {
|
||||||
let completion = match UART_COMPLETION.swap(0, Ordering::AcqRel) {
|
let completion = match UART_COMPLETION.swap(0, Ordering::AcqRel) {
|
||||||
1 => TxCompletion::Complete,
|
1 => TxCompletion::Complete,
|
||||||
2 => TxCompletion::TransferFault,
|
2 => TxCompletion::TransferFault,
|
||||||
_ => TxCompletion::NotPending,
|
_ => TxCompletion::NotPending,
|
||||||
};
|
};
|
||||||
context.local.telemetry.on_dma_complete(completion);
|
context
|
||||||
|
.local
|
||||||
|
.mavlink_transmitter
|
||||||
|
.on_dma_complete(completion);
|
||||||
}
|
}
|
||||||
if events & TELEMETRY_EVENT_SAMPLE != 0 {
|
if events & MAVLINK_EVENT_SAMPLE != 0
|
||||||
if let Some(sample) = SAMPLES.take_newer(context.local.observed_sample) {
|
&& 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.
|
// MAVLink transmission never blocks flight work. A later sample
|
||||||
let _ = context.local.telemetry.publish_sample(&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);
|
*context.local.heartbeat_divider = context.local.heartbeat_divider.wrapping_add(1);
|
||||||
if *context.local.heartbeat_divider == 10 {
|
if *context.local.heartbeat_divider == 10 {
|
||||||
*context.local.heartbeat_divider = 0;
|
*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();
|
context.local.led.toggle();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
TELEMETRY_QUEUED.store(false, Ordering::Release);
|
if events & MAVLINK_EVENT_RECEIVED != 0 {
|
||||||
if TELEMETRY_EVENTS.load(Ordering::Acquire) != 0
|
while let Some(frame) = context
|
||||||
&& !TELEMETRY_QUEUED.swap(true, Ordering::AcqRel)
|
.shared
|
||||||
{
|
.mavlink_receiver
|
||||||
if telemetry_worker::spawn().is_err() {
|
.lock(MavlinkReceiver::take_received)
|
||||||
TELEMETRY_QUEUED.store(false, Ordering::Release);
|
{
|
||||||
|
// 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) {
|
fn schedule_imu(event: u8) {
|
||||||
@@ -252,18 +305,19 @@ mod app {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
fn schedule_telemetry(event: u8) {
|
fn schedule_mavlink(event: u8) {
|
||||||
TELEMETRY_EVENTS.fetch_or(event, Ordering::Release);
|
MAVLINK_EVENTS.fetch_or(event, Ordering::Release);
|
||||||
if !TELEMETRY_QUEUED.swap(true, Ordering::AcqRel) && telemetry_worker::spawn().is_err() {
|
if !MAVLINK_QUEUED.swap(true, Ordering::AcqRel) && mavlink_worker::spawn().is_err() {
|
||||||
TELEMETRY_QUEUED.store(false, Ordering::Release);
|
MAVLINK_QUEUED.store(false, Ordering::Release);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
fn configure_exti4(rcc: &stm32f722::RCC, syscfg: stm32f722::SYSCFG, exti: stm32f722::EXTI) {
|
fn configure_exti4(rcc: &stm32f722::RCC, syscfg: stm32f722::SYSCFG, exti: stm32f722::EXTI) {
|
||||||
rcc.apb2enr().modify(|_, write| write.syscfgen().set_bit());
|
rcc.apb2enr().modify(|_, write| write.syscfgen().set_bit());
|
||||||
syscfg
|
syscfg.exticr2().modify(|_, write| {
|
||||||
.exticr2()
|
// SAFETY: EXTI4 is connected to GPIOA by selector value zero.
|
||||||
.modify(|_, write| unsafe { write.exti4().bits(0) });
|
unsafe { write.exti4().bits(0) }
|
||||||
|
});
|
||||||
exti.imr().modify(|_, write| write.im4().set_bit());
|
exti.imr().modify(|_, write| write.im4().set_bit());
|
||||||
exti.rtsr().modify(|_, write| write.tr4().set_bit());
|
exti.rtsr().modify(|_, write| write.tr4().set_bit());
|
||||||
exti.ftsr().modify(|_, write| write.tr4().clear_bit());
|
exti.ftsr().modify(|_, write| write.tr4().clear_bit());
|
||||||
|
|||||||
@@ -14,10 +14,14 @@ default = ["dialect-kaptan"]
|
|||||||
dialect-kaptan = []
|
dialect-kaptan = []
|
||||||
|
|
||||||
[dependencies]
|
[dependencies]
|
||||||
|
kaptan-dma = { path = "../kaptan-dma" }
|
||||||
|
kaptan-sensor = { path = "../kaptan-sensor" }
|
||||||
|
kaptan-tx = { path = "../kaptan-tx" }
|
||||||
mavlink = { version = "0.18", default-features = false, features = ["embedded", "dialect-common"] }
|
mavlink = { version = "0.18", default-features = false, features = ["embedded", "dialect-common"] }
|
||||||
mavlink-core = { version = "0.18", default-features = false, features = ["embedded"] }
|
mavlink-core = { version = "0.18", default-features = false, features = ["embedded"] }
|
||||||
kaptan-sensor = { path = "../kaptan-sensor" }
|
|
||||||
|
[dev-dependencies]
|
||||||
kaptan-time = { path = "../kaptan-time" }
|
kaptan-time = { path = "../kaptan-time" }
|
||||||
|
|
||||||
[build-dependencies]
|
[build-dependencies]
|
||||||
mavlink-bindgen = "0.18"
|
mavlink-bindgen = "0.18"
|
||||||
@@ -0,0 +1,117 @@
|
|||||||
|
//! MAVLink frame encoding API.
|
||||||
|
|
||||||
|
use kaptan_sensor::ImuSnapshot;
|
||||||
|
use mavlink::MessageData;
|
||||||
|
|
||||||
|
use crate::{MavlinkFrame, message};
|
||||||
|
|
||||||
|
/// Serializes FC-domain data with MAVLink sender identity and sequence state.
|
||||||
|
pub struct MavlinkEncoder {
|
||||||
|
system_id: u8,
|
||||||
|
component_id: u8,
|
||||||
|
next_sequence: u8,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl MavlinkEncoder {
|
||||||
|
/// Creates an encoder beginning with MAVLink packet sequence zero.
|
||||||
|
pub const fn new(system_id: u8, component_id: u8) -> Self {
|
||||||
|
Self {
|
||||||
|
system_id,
|
||||||
|
component_id,
|
||||||
|
next_sequence: 0,
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
fn serialize<M: MessageData>(&mut self, message: &M) -> MavlinkFrame {
|
||||||
|
let mut raw = MavlinkFrame::new();
|
||||||
|
raw.serialize_message_data(
|
||||||
|
crate::message::MavHeader {
|
||||||
|
sequence: self.next_sequence,
|
||||||
|
system_id: self.system_id,
|
||||||
|
component_id: self.component_id,
|
||||||
|
},
|
||||||
|
message,
|
||||||
|
);
|
||||||
|
self.next_sequence = self.next_sequence.wrapping_add(1);
|
||||||
|
|
||||||
|
raw
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Encodes a Kaptan heartbeat.
|
||||||
|
pub fn heartbeat(&mut self, system_status: message::MavState) -> MavlinkFrame {
|
||||||
|
let heartbeat = message::HEARTBEAT_DATA {
|
||||||
|
custom_mode: 0,
|
||||||
|
mavtype: message::MavType::MAV_TYPE_QUADROTOR,
|
||||||
|
autopilot: message::MavAutopilot::MAV_AUTOPILOT_GENERIC,
|
||||||
|
base_mode: message::MavModeFlag::empty(),
|
||||||
|
system_status,
|
||||||
|
mavlink_version: 3,
|
||||||
|
};
|
||||||
|
|
||||||
|
self.serialize(&heartbeat)
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Encodes a standard `RAW_IMU` frame.
|
||||||
|
pub fn raw_imu_snapshot(&mut self, snapshot: &ImuSnapshot) -> MavlinkFrame {
|
||||||
|
let raw_imu = message::RAW_IMU_DATA {
|
||||||
|
time_usec: snapshot.timestamp.as_microseconds(),
|
||||||
|
xacc: snapshot.acceleration_milligravity[0],
|
||||||
|
yacc: snapshot.acceleration_milligravity[1],
|
||||||
|
zacc: snapshot.acceleration_milligravity[2],
|
||||||
|
xgyro: snapshot.angular_rate_milliradians_per_second[0],
|
||||||
|
ygyro: snapshot.angular_rate_milliradians_per_second[1],
|
||||||
|
zgyro: snapshot.angular_rate_milliradians_per_second[2],
|
||||||
|
xmag: 0,
|
||||||
|
ymag: 0,
|
||||||
|
zmag: 0,
|
||||||
|
};
|
||||||
|
|
||||||
|
self.serialize(&raw_imu)
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[cfg(test)]
|
||||||
|
mod tests {
|
||||||
|
use super::MavlinkEncoder;
|
||||||
|
use crate::message::MavState;
|
||||||
|
use kaptan_sensor::ImuSnapshot;
|
||||||
|
use kaptan_time::Timestamp;
|
||||||
|
|
||||||
|
#[test]
|
||||||
|
fn heartbeat_has_the_configured_sender_and_sequence() {
|
||||||
|
let mut encoder = MavlinkEncoder::new(42, 7);
|
||||||
|
let frame = encoder.heartbeat(MavState::MAV_STATE_STANDBY);
|
||||||
|
let bytes = frame.raw_bytes();
|
||||||
|
|
||||||
|
assert_eq!(bytes[0], 0xFD);
|
||||||
|
assert_eq!(bytes[2], 0);
|
||||||
|
assert_eq!(bytes[3], 0);
|
||||||
|
assert_eq!(bytes[4], 0);
|
||||||
|
assert_eq!(bytes[5], 42);
|
||||||
|
assert_eq!(bytes[6], 7);
|
||||||
|
assert_eq!(&bytes[7..10], &[0, 0, 0]);
|
||||||
|
}
|
||||||
|
|
||||||
|
#[test]
|
||||||
|
fn heartbeat_preserves_the_requested_health_state() {
|
||||||
|
let mut encoder = MavlinkEncoder::new(1, 1);
|
||||||
|
let frame = encoder.heartbeat(MavState::MAV_STATE_CRITICAL);
|
||||||
|
|
||||||
|
assert_eq!(frame.raw_bytes()[17], MavState::MAV_STATE_CRITICAL as u8);
|
||||||
|
}
|
||||||
|
|
||||||
|
#[test]
|
||||||
|
fn snapshot_adapter_uses_flight_data_timestamp_and_units() {
|
||||||
|
let mut encoder = MavlinkEncoder::new(1, 1);
|
||||||
|
let snapshot = ImuSnapshot::new(
|
||||||
|
Timestamp::from_microseconds(123),
|
||||||
|
[1, 2, 3],
|
||||||
|
[4, 5, 6],
|
||||||
|
2_500,
|
||||||
|
);
|
||||||
|
|
||||||
|
let frame = encoder.raw_imu_snapshot(&snapshot);
|
||||||
|
|
||||||
|
assert_eq!(&frame.raw_bytes()[7..10], &[27, 0, 0]);
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -2,78 +2,16 @@
|
|||||||
|
|
||||||
#![no_std]
|
#![no_std]
|
||||||
|
|
||||||
use kaptan_sensor::ImuSnapshot;
|
/// MAVLink frame encoding API for boards.
|
||||||
use mavlink::MessageData;
|
pub mod encoder;
|
||||||
|
/// Serial-byte MAVLink receive API for boards.
|
||||||
|
pub mod receiver;
|
||||||
|
/// UART DMA MAVLink frame transmission API for boards.
|
||||||
|
pub mod transmitter;
|
||||||
|
|
||||||
/// An owned MAVLink 2 frame supplied by rust-mavlink.
|
/// An owned MAVLink 2 frame suitable for UART transmission.
|
||||||
pub type MavlinkFrame = message::MAVLinkV2MessageRaw;
|
pub type MavlinkFrame = message::MAVLinkV2MessageRaw;
|
||||||
|
|
||||||
/// Owns MAVLink sender identity and packet sequence state.
|
|
||||||
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
|
||||||
pub struct MavlinkSender {
|
|
||||||
system_id: u8,
|
|
||||||
component_id: u8,
|
|
||||||
next_sequence: u8,
|
|
||||||
}
|
|
||||||
|
|
||||||
impl MavlinkSender {
|
|
||||||
/// Creates a sender beginning with MAVLink packet sequence zero.
|
|
||||||
pub const fn new(system_id: u8, component_id: u8) -> Self {
|
|
||||||
Self {
|
|
||||||
system_id,
|
|
||||||
component_id,
|
|
||||||
next_sequence: 0,
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Serializes a Kaptan heartbeat with the supplied system-health state.
|
|
||||||
pub fn heartbeat(&mut self, system_status: message::MavState) -> MavlinkFrame {
|
|
||||||
let heartbeat = message::HEARTBEAT_DATA {
|
|
||||||
custom_mode: 0,
|
|
||||||
mavtype: message::MavType::MAV_TYPE_QUADROTOR,
|
|
||||||
autopilot: message::MavAutopilot::MAV_AUTOPILOT_GENERIC,
|
|
||||||
base_mode: message::MavModeFlag::empty(),
|
|
||||||
system_status,
|
|
||||||
mavlink_version: 3,
|
|
||||||
};
|
|
||||||
|
|
||||||
self.serialize(&heartbeat)
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Serializes a standard `RAW_IMU` frame from a flight-data snapshot.
|
|
||||||
pub fn raw_imu_snapshot(&mut self, snapshot: &ImuSnapshot) -> MavlinkFrame {
|
|
||||||
let raw_imu = message::RAW_IMU_DATA {
|
|
||||||
time_usec: snapshot.timestamp.as_microseconds(),
|
|
||||||
xacc: snapshot.acceleration_milligravity[0],
|
|
||||||
yacc: snapshot.acceleration_milligravity[1],
|
|
||||||
zacc: snapshot.acceleration_milligravity[2],
|
|
||||||
xgyro: snapshot.angular_rate_milliradians_per_second[0],
|
|
||||||
ygyro: snapshot.angular_rate_milliradians_per_second[1],
|
|
||||||
zgyro: snapshot.angular_rate_milliradians_per_second[2],
|
|
||||||
xmag: 0,
|
|
||||||
ymag: 0,
|
|
||||||
zmag: 0,
|
|
||||||
};
|
|
||||||
|
|
||||||
self.serialize(&raw_imu)
|
|
||||||
}
|
|
||||||
|
|
||||||
fn serialize<M: MessageData>(&mut self, message: &M) -> MavlinkFrame {
|
|
||||||
let mut raw = message::MAVLinkV2MessageRaw::new();
|
|
||||||
raw.serialize_message_data(
|
|
||||||
message::MavHeader {
|
|
||||||
sequence: self.next_sequence,
|
|
||||||
system_id: self.system_id,
|
|
||||||
component_id: self.component_id,
|
|
||||||
},
|
|
||||||
message,
|
|
||||||
);
|
|
||||||
self.next_sequence = self.next_sequence.wrapping_add(1);
|
|
||||||
|
|
||||||
raw
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Supported MAVLink message types for Kaptan.
|
/// Supported MAVLink message types for Kaptan.
|
||||||
pub mod message {
|
pub mod message {
|
||||||
pub use mavlink::MAVLinkV2MessageRaw;
|
pub use mavlink::MAVLinkV2MessageRaw;
|
||||||
@@ -83,53 +21,3 @@ pub mod message {
|
|||||||
#[cfg(kaptan_has_messages)]
|
#[cfg(kaptan_has_messages)]
|
||||||
include!(concat!(env!("OUT_DIR"), "/mod.rs"));
|
include!(concat!(env!("OUT_DIR"), "/mod.rs"));
|
||||||
}
|
}
|
||||||
|
|
||||||
#[cfg(test)]
|
|
||||||
mod tests {
|
|
||||||
use super::MavlinkSender;
|
|
||||||
use kaptan_sensor::ImuSnapshot;
|
|
||||||
use kaptan_time::Timestamp;
|
|
||||||
|
|
||||||
#[test]
|
|
||||||
fn heartbeat_has_the_configured_sender_and_sequence() {
|
|
||||||
let mut sender = MavlinkSender::new(42, 7);
|
|
||||||
let frame = sender.heartbeat(crate::message::MavState::MAV_STATE_STANDBY);
|
|
||||||
let bytes = frame.raw_bytes();
|
|
||||||
|
|
||||||
assert_eq!(bytes[0], 0xFD);
|
|
||||||
assert_eq!(bytes[2], 0);
|
|
||||||
assert_eq!(bytes[3], 0);
|
|
||||||
assert_eq!(bytes[4], 0);
|
|
||||||
assert_eq!(bytes[5], 42);
|
|
||||||
assert_eq!(bytes[6], 7);
|
|
||||||
assert_eq!(&bytes[7..10], &[0, 0, 0]);
|
|
||||||
}
|
|
||||||
|
|
||||||
#[test]
|
|
||||||
fn heartbeat_preserves_the_requested_health_state() {
|
|
||||||
let mut sender = MavlinkSender::new(1, 1);
|
|
||||||
let frame = sender.heartbeat(crate::message::MavState::MAV_STATE_CRITICAL);
|
|
||||||
|
|
||||||
// MAVLink 2 header is 10 octets; HEARTBEAT.system_status is payload
|
|
||||||
// octet seven according to the common dialect wire layout.
|
|
||||||
assert_eq!(
|
|
||||||
frame.raw_bytes()[17],
|
|
||||||
crate::message::MavState::MAV_STATE_CRITICAL as u8
|
|
||||||
);
|
|
||||||
}
|
|
||||||
|
|
||||||
#[test]
|
|
||||||
fn snapshot_adapter_uses_flight_data_timestamp_and_units() {
|
|
||||||
let mut sender = MavlinkSender::new(1, 1);
|
|
||||||
let snapshot = ImuSnapshot::new(
|
|
||||||
Timestamp::from_microseconds(123),
|
|
||||||
[1, 2, 3],
|
|
||||||
[4, 5, 6],
|
|
||||||
2_500,
|
|
||||||
);
|
|
||||||
|
|
||||||
let frame = sender.raw_imu_snapshot(&snapshot);
|
|
||||||
|
|
||||||
assert_eq!(&frame.raw_bytes()[7..10], &[27, 0, 0]);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -0,0 +1,181 @@
|
|||||||
|
//! MAVLink serial-byte receive API.
|
||||||
|
|
||||||
|
use crate::{MavlinkFrame, message};
|
||||||
|
|
||||||
|
/// Bounded MAVLink 2 receiver for a board's serial-byte interrupt.
|
||||||
|
///
|
||||||
|
/// Boards pass UART bytes to [`Self::receive_byte`] and take complete,
|
||||||
|
/// CRC-valid frames from [`Self::take_received`]. The API never decodes or
|
||||||
|
/// dispatches received message payloads.
|
||||||
|
pub struct MavlinkReceiver<const N: usize> {
|
||||||
|
frame: MavlinkFrame,
|
||||||
|
received: usize,
|
||||||
|
expected: usize,
|
||||||
|
frames: [Option<MavlinkFrame>; N],
|
||||||
|
front: usize,
|
||||||
|
length: usize,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<const N: usize> Default for MavlinkReceiver<N> {
|
||||||
|
fn default() -> Self {
|
||||||
|
Self::new()
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<const N: usize> MavlinkReceiver<N> {
|
||||||
|
/// Creates a receiver with space for `N` complete MAVLink frames.
|
||||||
|
pub const fn new() -> Self {
|
||||||
|
Self {
|
||||||
|
frame: MavlinkFrame::new(),
|
||||||
|
received: 0,
|
||||||
|
expected: 0,
|
||||||
|
frames: [None; N],
|
||||||
|
front: 0,
|
||||||
|
length: 0,
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Receives one UART byte and returns whether it queued a complete frame.
|
||||||
|
///
|
||||||
|
/// Bytes outside MAVLink 2 frames and frames with invalid CRCs are
|
||||||
|
/// discarded. If the completed-frame queue is full, the new frame is
|
||||||
|
/// dropped without displacing an earlier received frame.
|
||||||
|
pub fn receive_byte(&mut self, byte: u8) -> bool {
|
||||||
|
if self.received == 0 {
|
||||||
|
if byte == 0xfd {
|
||||||
|
self.frame.as_mut_slice()[0] = byte;
|
||||||
|
self.received = 1;
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
self.frame.as_mut_slice()[self.received] = byte;
|
||||||
|
self.received += 1;
|
||||||
|
|
||||||
|
if self.received == 10 {
|
||||||
|
let header = self.frame.as_slice();
|
||||||
|
let incompatibility_flags = header[2];
|
||||||
|
if incompatibility_flags & !1 != 0 {
|
||||||
|
self.reset();
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
self.expected = 1 + 9 + usize::from(header[1]) + 2;
|
||||||
|
if incompatibility_flags & 1 != 0 {
|
||||||
|
self.expected += 13;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if self.expected != 0 && self.received == self.expected {
|
||||||
|
let frame = core::mem::replace(&mut self.frame, MavlinkFrame::new());
|
||||||
|
self.received = 0;
|
||||||
|
self.expected = 0;
|
||||||
|
if frame.has_valid_crc::<message::MavMessage>() {
|
||||||
|
return self.push(frame);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
false
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Discards the in-progress frame after a UART transport fault.
|
||||||
|
pub fn reset(&mut self) {
|
||||||
|
self.frame = MavlinkFrame::new();
|
||||||
|
self.received = 0;
|
||||||
|
self.expected = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Takes the oldest complete received frame, if one is available.
|
||||||
|
pub fn take_received(&mut self) -> Option<MavlinkFrame> {
|
||||||
|
if self.length == 0 {
|
||||||
|
return None;
|
||||||
|
}
|
||||||
|
let frame = self.frames[self.front].take();
|
||||||
|
self.front = (self.front + 1) % N;
|
||||||
|
self.length -= 1;
|
||||||
|
frame
|
||||||
|
}
|
||||||
|
|
||||||
|
fn push(&mut self, frame: MavlinkFrame) -> bool {
|
||||||
|
if N == 0 || self.length == N {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
let index = (self.front + self.length) % N;
|
||||||
|
self.frames[index] = Some(frame);
|
||||||
|
self.length += 1;
|
||||||
|
true
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[cfg(test)]
|
||||||
|
mod tests {
|
||||||
|
use super::MavlinkReceiver;
|
||||||
|
use crate::{MavlinkFrame, message};
|
||||||
|
|
||||||
|
fn heartbeat(sequence: u8) -> MavlinkFrame {
|
||||||
|
let mut frame = MavlinkFrame::new();
|
||||||
|
frame.serialize_message_data(
|
||||||
|
message::MavHeader {
|
||||||
|
sequence,
|
||||||
|
system_id: 42,
|
||||||
|
component_id: 7,
|
||||||
|
},
|
||||||
|
&message::HEARTBEAT_DATA {
|
||||||
|
custom_mode: 0,
|
||||||
|
mavtype: message::MavType::MAV_TYPE_QUADROTOR,
|
||||||
|
autopilot: message::MavAutopilot::MAV_AUTOPILOT_GENERIC,
|
||||||
|
base_mode: message::MavModeFlag::empty(),
|
||||||
|
system_status: message::MavState::MAV_STATE_STANDBY,
|
||||||
|
mavlink_version: 3,
|
||||||
|
},
|
||||||
|
);
|
||||||
|
frame
|
||||||
|
}
|
||||||
|
|
||||||
|
#[test]
|
||||||
|
fn returns_complete_crc_valid_frames_unchanged() {
|
||||||
|
let expected = heartbeat(0);
|
||||||
|
let mut receiver = MavlinkReceiver::<1>::new();
|
||||||
|
|
||||||
|
for byte in expected.raw_bytes() {
|
||||||
|
receiver.receive_byte(*byte);
|
||||||
|
}
|
||||||
|
|
||||||
|
assert_eq!(
|
||||||
|
receiver.take_received().unwrap().raw_bytes(),
|
||||||
|
expected.raw_bytes()
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
#[test]
|
||||||
|
fn discards_frames_with_an_invalid_crc() {
|
||||||
|
let frame = heartbeat(0);
|
||||||
|
let mut bytes = frame.into_inner();
|
||||||
|
bytes[10] ^= 1;
|
||||||
|
let corrupt = MavlinkFrame::from_bytes_unparsed(bytes);
|
||||||
|
let mut receiver = MavlinkReceiver::<1>::new();
|
||||||
|
|
||||||
|
for byte in corrupt.raw_bytes() {
|
||||||
|
assert!(!receiver.receive_byte(*byte));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[test]
|
||||||
|
fn preserves_arrival_order() {
|
||||||
|
let first = heartbeat(0);
|
||||||
|
let second = heartbeat(1);
|
||||||
|
let mut receiver = MavlinkReceiver::<2>::new();
|
||||||
|
|
||||||
|
for byte in first.raw_bytes().iter().chain(second.raw_bytes()) {
|
||||||
|
receiver.receive_byte(*byte);
|
||||||
|
}
|
||||||
|
|
||||||
|
assert_eq!(
|
||||||
|
receiver.take_received().unwrap().raw_bytes(),
|
||||||
|
first.raw_bytes()
|
||||||
|
);
|
||||||
|
assert_eq!(
|
||||||
|
receiver.take_received().unwrap().raw_bytes(),
|
||||||
|
second.raw_bytes()
|
||||||
|
);
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,134 @@
|
|||||||
|
//! MAVLink frame encoding and UART transmission API.
|
||||||
|
|
||||||
|
use kaptan_dma::{DmaTxBufferFactory, DmaTxEngine, TxCompletion};
|
||||||
|
use kaptan_tx::{PendingFrame, TransmitQueue};
|
||||||
|
|
||||||
|
use crate::MavlinkFrame;
|
||||||
|
|
||||||
|
/// Delivery importance used for bounded MAVLink transmission admission.
|
||||||
|
pub use kaptan_tx::FrameClass;
|
||||||
|
|
||||||
|
/// Failure while preparing or starting a MAVLink frame transfer.
|
||||||
|
#[derive(Debug)]
|
||||||
|
pub enum MavlinkTransmitFault<MemoryError, EngineError> {
|
||||||
|
/// DMA-visible frame preparation failed.
|
||||||
|
Memory(MemoryError),
|
||||||
|
/// The UART DMA route rejected a transfer start.
|
||||||
|
Engine(EngineError),
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Owns MAVLink frame admission and UART DMA lifecycle.
|
||||||
|
///
|
||||||
|
/// Exactly one worker invokes this type. It accepts local encoded frames and
|
||||||
|
/// received frames alike; the caller owns the policy that decides which frames
|
||||||
|
/// are transmitted.
|
||||||
|
pub struct MavlinkTransmitter<Memory, Engine, const N: usize> {
|
||||||
|
transmit: TransmitQueue<MavlinkFrame, N>,
|
||||||
|
memory: Memory,
|
||||||
|
engine: Engine,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<Memory, Engine, const N: usize> MavlinkTransmitter<Memory, Engine, N> {
|
||||||
|
/// Creates an empty MAVLink transmitter.
|
||||||
|
pub const fn new(memory: Memory, engine: Engine) -> Self {
|
||||||
|
Self {
|
||||||
|
transmit: TransmitQueue::new(),
|
||||||
|
memory,
|
||||||
|
engine,
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
impl<Memory, Engine, const N: usize> MavlinkTransmitter<Memory, Engine, N>
|
||||||
|
where
|
||||||
|
Memory: DmaTxBufferFactory,
|
||||||
|
Engine: DmaTxEngine,
|
||||||
|
{
|
||||||
|
/// Admits an already-serialized MAVLink frame for UART transmission.
|
||||||
|
pub fn send_frame(
|
||||||
|
&mut self,
|
||||||
|
class: FrameClass,
|
||||||
|
frame: MavlinkFrame,
|
||||||
|
) -> Result<(), MavlinkTransmitFault<Memory::Error, Engine::StartError>> {
|
||||||
|
let _ = self.transmit.enqueue(PendingFrame { class, frame });
|
||||||
|
self.start_next()
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Releases a terminal DMA frame and opportunistically starts the next.
|
||||||
|
pub fn on_dma_complete(&mut self, completion: TxCompletion) {
|
||||||
|
match completion {
|
||||||
|
TxCompletion::NotPending => {}
|
||||||
|
TxCompletion::Complete => {
|
||||||
|
let _ = self.transmit.complete();
|
||||||
|
let _ = self.start_next();
|
||||||
|
}
|
||||||
|
TxCompletion::TransferFault | TxCompletion::Halted => {
|
||||||
|
let _ = self.transmit.fail();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
fn start_next(
|
||||||
|
&mut self,
|
||||||
|
) -> Result<(), MavlinkTransmitFault<Memory::Error, Engine::StartError>> {
|
||||||
|
let (transmit, memory, engine) = (&mut self.transmit, &mut self.memory, &mut self.engine);
|
||||||
|
transmit
|
||||||
|
.start_with(|frame| {
|
||||||
|
let buffer = memory
|
||||||
|
.prepare(frame.raw_bytes())
|
||||||
|
.map_err(MavlinkTransmitFault::Memory)?;
|
||||||
|
engine.start(buffer).map_err(MavlinkTransmitFault::Engine)
|
||||||
|
})
|
||||||
|
.map(|_| ())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[cfg(test)]
|
||||||
|
mod tests {
|
||||||
|
use super::{FrameClass, MavlinkTransmitter};
|
||||||
|
use crate::{encoder::MavlinkEncoder, message::MavState};
|
||||||
|
use kaptan_dma::{DmaReadableBuffer, DmaTxBufferFactory, DmaTxEngine, TxCompletion};
|
||||||
|
|
||||||
|
struct Memory;
|
||||||
|
|
||||||
|
impl DmaTxBufferFactory for Memory {
|
||||||
|
type Error = core::convert::Infallible;
|
||||||
|
|
||||||
|
fn prepare<'a>(&mut self, bytes: &'a [u8]) -> Result<DmaReadableBuffer<'a>, Self::Error> {
|
||||||
|
// SAFETY: this host-only test keeps the queue's active frame alive.
|
||||||
|
Ok(unsafe { DmaReadableBuffer::new_unchecked(bytes) })
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
struct Engine;
|
||||||
|
|
||||||
|
impl DmaTxEngine for Engine {
|
||||||
|
type StartError = core::convert::Infallible;
|
||||||
|
|
||||||
|
fn start(&mut self, _: DmaReadableBuffer<'_>) -> Result<(), Self::StartError> {
|
||||||
|
Ok(())
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#[test]
|
||||||
|
fn transfer_fault_drops_only_the_active_frame() {
|
||||||
|
let mut encoder = MavlinkEncoder::new(1, 1);
|
||||||
|
let mut transmitter = MavlinkTransmitter::<Memory, Engine, 2>::new(Memory, Engine);
|
||||||
|
transmitter
|
||||||
|
.send_frame(
|
||||||
|
FrameClass::BestEffort,
|
||||||
|
encoder.heartbeat(MavState::MAV_STATE_STANDBY),
|
||||||
|
)
|
||||||
|
.unwrap();
|
||||||
|
transmitter
|
||||||
|
.send_frame(
|
||||||
|
FrameClass::BestEffort,
|
||||||
|
encoder.heartbeat(MavState::MAV_STATE_STANDBY),
|
||||||
|
)
|
||||||
|
.unwrap();
|
||||||
|
assert!(transmitter.transmit.is_active());
|
||||||
|
|
||||||
|
transmitter.on_dma_complete(TxCompletion::TransferFault);
|
||||||
|
assert!(!transmitter.transmit.is_active());
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -10,10 +10,7 @@ version.workspace = true
|
|||||||
workspace = true
|
workspace = true
|
||||||
|
|
||||||
[dependencies]
|
[dependencies]
|
||||||
kaptan-dma = { path = "../kaptan-dma" }
|
|
||||||
kaptan-drivers = { path = "../kaptan-drivers" }
|
kaptan-drivers = { path = "../kaptan-drivers" }
|
||||||
kaptan-mavlink = { path = "../kaptan-mavlink" }
|
|
||||||
kaptan-sensor = { path = "../kaptan-sensor" }
|
kaptan-sensor = { path = "../kaptan-sensor" }
|
||||||
kaptan-spi = { path = "../kaptan-spi" }
|
kaptan-spi = { path = "../kaptan-spi" }
|
||||||
kaptan-time = { path = "../kaptan-time" }
|
kaptan-time = { path = "../kaptan-time" }
|
||||||
kaptan-tx = { path = "../kaptan-tx" }
|
|
||||||
@@ -1,8 +1,9 @@
|
|||||||
//! RTIC- and PAC-independent flight services.
|
//! RTIC- and PAC-independent flight services.
|
||||||
//!
|
//!
|
||||||
//! Acquisition and telemetry have separate owners. Boards connect hardware
|
//! Acquisition services and board-owned output workers have separate owners.
|
||||||
//! interrupts to their dedicated workers through bounded deferred events; they
|
//! Boards connect hardware interrupts to their dedicated workers through
|
||||||
//! must not place both services behind one priority-ceiling resource.
|
//! bounded deferred events; they must not place unrelated work behind one
|
||||||
|
//! priority-ceiling resource.
|
||||||
|
|
||||||
#![no_std]
|
#![no_std]
|
||||||
|
|
||||||
@@ -10,9 +11,6 @@
|
|||||||
pub mod imu;
|
pub mod imu;
|
||||||
/// Lock-free worker handoff primitives.
|
/// Lock-free worker handoff primitives.
|
||||||
pub mod mailbox;
|
pub mod mailbox;
|
||||||
/// Single-owner MAVLink and UART transmit service.
|
|
||||||
pub mod telemetry;
|
|
||||||
|
|
||||||
pub use imu::{ImuEvent, ImuService};
|
pub use imu::{ImuEvent, ImuService};
|
||||||
pub use mailbox::LatestImuMailbox;
|
pub use mailbox::LatestImuMailbox;
|
||||||
pub use telemetry::{TelemetryFault, TelemetryService};
|
|
||||||
@@ -7,11 +7,11 @@ use kaptan_time::Timestamp;
|
|||||||
|
|
||||||
/// A latest-value IMU mailbox.
|
/// A latest-value IMU mailbox.
|
||||||
///
|
///
|
||||||
/// One IMU worker publishes and one telemetry worker consumes. Publishing a
|
/// One IMU worker publishes and one output worker consumes. Publishing a newer
|
||||||
/// newer sample intentionally replaces an unread older sample: telemetry is a
|
/// sample intentionally replaces an unread older sample: an output observer is
|
||||||
/// best-effort observer and must never delay acquisition. The sequence counter
|
/// best-effort and must never delay acquisition. The sequence counter is a
|
||||||
/// is a seqlock; odd values indicate a writer and even values identify a
|
/// seqlock; odd values indicate a writer and even values identify a stable
|
||||||
/// stable snapshot.
|
/// snapshot.
|
||||||
pub struct LatestImuMailbox {
|
pub struct LatestImuMailbox {
|
||||||
sequence: AtomicU32,
|
sequence: AtomicU32,
|
||||||
timestamp_low: AtomicU32,
|
timestamp_low: AtomicU32,
|
||||||
|
|||||||
@@ -1,143 +0,0 @@
|
|||||||
//! Single-owner MAVLink transmit service.
|
|
||||||
|
|
||||||
use kaptan_dma::{DmaTxBufferFactory, DmaTxEngine, TxCompletion};
|
|
||||||
use kaptan_mavlink::{MavlinkFrame, MavlinkSender, message::MavState};
|
|
||||||
use kaptan_sensor::ImuSnapshot;
|
|
||||||
use kaptan_tx::{FrameClass, PendingFrame, TransmitQueue};
|
|
||||||
|
|
||||||
/// Failure while preparing or starting one telemetry frame.
|
|
||||||
#[derive(Debug)]
|
|
||||||
pub enum TelemetryFault<MemoryError, EngineError> {
|
|
||||||
/// DMA-visible frame preparation failed.
|
|
||||||
Memory(MemoryError),
|
|
||||||
/// The UART DMA route rejected a transfer start.
|
|
||||||
Engine(EngineError),
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Owns MAVLink packet sequencing and UART DMA frame lifecycle.
|
|
||||||
///
|
|
||||||
/// Exactly one worker invokes this service. It is not shared with sensor or
|
|
||||||
/// hardware IRQ execution.
|
|
||||||
pub struct TelemetryService<Memory, Engine, const N: usize> {
|
|
||||||
mavlink: MavlinkSender,
|
|
||||||
transmit: TransmitQueue<MavlinkFrame, N>,
|
|
||||||
memory: Memory,
|
|
||||||
engine: Engine,
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<Memory, Engine, const N: usize> TelemetryService<Memory, Engine, N> {
|
|
||||||
/// Creates an empty telemetry service.
|
|
||||||
pub const fn new(system_id: u8, component_id: u8, memory: Memory, engine: Engine) -> Self {
|
|
||||||
Self {
|
|
||||||
mavlink: MavlinkSender::new(system_id, component_id),
|
|
||||||
transmit: TransmitQueue::new(),
|
|
||||||
memory,
|
|
||||||
engine,
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
impl<Memory, Engine, const N: usize> TelemetryService<Memory, Engine, N>
|
|
||||||
where
|
|
||||||
Memory: DmaTxBufferFactory,
|
|
||||||
Engine: DmaTxEngine,
|
|
||||||
{
|
|
||||||
/// Queues the newest accepted IMU observation.
|
|
||||||
pub fn publish_sample(
|
|
||||||
&mut self,
|
|
||||||
sample: &ImuSnapshot,
|
|
||||||
) -> Result<(), TelemetryFault<Memory::Error, Engine::StartError>> {
|
|
||||||
let frame = self.mavlink.raw_imu_snapshot(sample);
|
|
||||||
self.enqueue_and_start(FrameClass::BestEffort, frame)
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Queues one liveness heartbeat.
|
|
||||||
pub fn publish_heartbeat(
|
|
||||||
&mut self,
|
|
||||||
) -> Result<(), TelemetryFault<Memory::Error, Engine::StartError>> {
|
|
||||||
let frame = self.mavlink.heartbeat(MavState::MAV_STATE_STANDBY);
|
|
||||||
self.enqueue_and_start(FrameClass::Critical, frame)
|
|
||||||
}
|
|
||||||
|
|
||||||
/// Releases a terminal DMA frame and opportunistically starts the next.
|
|
||||||
///
|
|
||||||
/// Telemetry is best-effort: a terminal route fault drops only the active
|
|
||||||
/// frame and a rejected follow-up start is retried on later publication.
|
|
||||||
pub fn on_dma_complete(&mut self, completion: TxCompletion) {
|
|
||||||
match completion {
|
|
||||||
TxCompletion::NotPending => {}
|
|
||||||
TxCompletion::Complete => {
|
|
||||||
let _ = self.transmit.complete();
|
|
||||||
let _ = self.start_next();
|
|
||||||
}
|
|
||||||
TxCompletion::TransferFault | TxCompletion::Halted => {
|
|
||||||
let _ = self.transmit.fail();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
fn enqueue_and_start(
|
|
||||||
&mut self,
|
|
||||||
class: FrameClass,
|
|
||||||
frame: MavlinkFrame,
|
|
||||||
) -> Result<(), TelemetryFault<Memory::Error, Engine::StartError>> {
|
|
||||||
let _ = self.transmit.enqueue(PendingFrame { class, frame });
|
|
||||||
self.start_next()
|
|
||||||
}
|
|
||||||
|
|
||||||
fn start_next(&mut self) -> Result<(), TelemetryFault<Memory::Error, Engine::StartError>> {
|
|
||||||
let (transmit, memory, engine) = (&mut self.transmit, &mut self.memory, &mut self.engine);
|
|
||||||
transmit
|
|
||||||
.start_with(|frame| {
|
|
||||||
let buffer = memory
|
|
||||||
.prepare(frame.raw_bytes())
|
|
||||||
.map_err(TelemetryFault::Memory)?;
|
|
||||||
engine.start(buffer).map_err(TelemetryFault::Engine)
|
|
||||||
})
|
|
||||||
.map(|_| ())
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
#[cfg(test)]
|
|
||||||
mod tests {
|
|
||||||
use super::TelemetryService;
|
|
||||||
use kaptan_dma::{DmaReadableBuffer, DmaTxBufferFactory, DmaTxEngine, TxCompletion};
|
|
||||||
use kaptan_sensor::ImuSnapshot;
|
|
||||||
use kaptan_time::Timestamp;
|
|
||||||
|
|
||||||
struct Memory;
|
|
||||||
|
|
||||||
impl DmaTxBufferFactory for Memory {
|
|
||||||
type Error = core::convert::Infallible;
|
|
||||||
|
|
||||||
fn prepare<'a>(&mut self, bytes: &'a [u8]) -> Result<DmaReadableBuffer<'a>, Self::Error> {
|
|
||||||
// SAFETY: this host-only test keeps the queue's active frame alive.
|
|
||||||
Ok(unsafe { DmaReadableBuffer::new_unchecked(bytes) })
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
struct Engine;
|
|
||||||
|
|
||||||
impl DmaTxEngine for Engine {
|
|
||||||
type StartError = core::convert::Infallible;
|
|
||||||
|
|
||||||
fn start(&mut self, _: DmaReadableBuffer<'_>) -> Result<(), Self::StartError> {
|
|
||||||
Ok(())
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
fn sample(timestamp: u64) -> ImuSnapshot {
|
|
||||||
ImuSnapshot::new(Timestamp::from_microseconds(timestamp), [0; 3], [0; 3], 0)
|
|
||||||
}
|
|
||||||
|
|
||||||
#[test]
|
|
||||||
fn transfer_fault_drops_only_the_active_frame() {
|
|
||||||
let mut service = TelemetryService::<Memory, Engine, 2>::new(1, 1, Memory, Engine);
|
|
||||||
service.publish_sample(&sample(1)).unwrap();
|
|
||||||
service.publish_sample(&sample(2)).unwrap();
|
|
||||||
assert!(service.transmit.is_active());
|
|
||||||
|
|
||||||
service.on_dma_complete(TxCompletion::TransferFault);
|
|
||||||
assert!(!service.transmit.is_active());
|
|
||||||
}
|
|
||||||
}
|
|
||||||
@@ -70,6 +70,16 @@ pub fn configure_usart1_tx_pin_pa9(port: &stm32f722::GPIOA) {
|
|||||||
port.pupdr().modify(|_, write| write.pupdr9().floating());
|
port.pupdr().modify(|_, write| write.pupdr9().floating());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Configures PA10 for USART1 receive alternate function 7.
|
||||||
|
pub fn configure_usart1_rx_pin_pa10(port: &stm32f722::GPIOA) {
|
||||||
|
port.moder().modify(|_, write| write.moder10().alternate());
|
||||||
|
port.afrh().modify(|_, write| {
|
||||||
|
// SAFETY: AF7 selects USART1 RX on PA10 on STM32F722.
|
||||||
|
unsafe { write.afrh10().bits(0b0111) }
|
||||||
|
});
|
||||||
|
port.pupdr().modify(|_, write| write.pupdr10().floating());
|
||||||
|
}
|
||||||
|
|
||||||
/// Active-low status LED connected to PD2.
|
/// Active-low status LED connected to PD2.
|
||||||
pub struct ActiveLowLedPd2 {
|
pub struct ActiveLowLedPd2 {
|
||||||
port: stm32f722::GPIOD,
|
port: stm32f722::GPIOD,
|
||||||
|
|||||||
@@ -16,10 +16,10 @@ pub use clock::{ClockProfileError, select_hsi16_profile};
|
|||||||
pub use dma_buffer::DcacheDisabledDmaMemory;
|
pub use dma_buffer::DcacheDisabledDmaMemory;
|
||||||
pub use gpio::{
|
pub use gpio::{
|
||||||
ActiveLowLedPd2, configure_exti4_pin_pa4, configure_spi1_pins_pa5_pa6_pa7,
|
ActiveLowLedPd2, configure_exti4_pin_pa4, configure_spi1_pins_pa5_pa6_pa7,
|
||||||
configure_usart1_tx_pin_pa9,
|
configure_usart1_rx_pin_pa10, configure_usart1_tx_pin_pa9,
|
||||||
};
|
};
|
||||||
pub use spi1::{Spi1, Spi1Error};
|
pub use spi1::{Spi1, Spi1Error};
|
||||||
pub use tim2::{Tim2Periodic, Tim2PeriodicConfig};
|
pub use tim2::{Tim2Periodic, Tim2PeriodicConfig};
|
||||||
pub use time::{DwtClockStartError, DwtMicrosecondClock};
|
pub use time::{DwtClockStartError, DwtMicrosecondClock};
|
||||||
pub use usart::{BaudRateError, Usart1Tx};
|
pub use usart::{BaudRateError, ReceiveInterrupt, Usart1Tx};
|
||||||
pub use usart1_tx_dma2_stream7::{StartError, Usart1TxDma};
|
pub use usart1_tx_dma2_stream7::{StartError, Usart1TxDma};
|
||||||
@@ -11,7 +11,18 @@ pub enum BaudRateError {
|
|||||||
OutOfRange,
|
OutOfRange,
|
||||||
}
|
}
|
||||||
|
|
||||||
/// Owns configured transmit-only USART1 hardware.
|
/// Terminal result from acknowledging the USART1 receive interrupt.
|
||||||
|
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
|
||||||
|
pub enum ReceiveInterrupt {
|
||||||
|
/// One received byte was removed from the UART data register.
|
||||||
|
Byte(u8),
|
||||||
|
/// A serial transport fault invalidated the current receive stream.
|
||||||
|
Fault,
|
||||||
|
/// This USART1 interrupt did not have a receive event pending.
|
||||||
|
NotPending,
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Owns configured full-duplex USART1 hardware.
|
||||||
///
|
///
|
||||||
/// Pin alternate-function setup remains board-owned because it depends on the
|
/// Pin alternate-function setup remains board-owned because it depends on the
|
||||||
/// schematic. Configure pins before constructing this type.
|
/// schematic. Configure pins before constructing this type.
|
||||||
@@ -20,7 +31,7 @@ pub struct Usart1Tx {
|
|||||||
}
|
}
|
||||||
|
|
||||||
impl Usart1Tx {
|
impl Usart1Tx {
|
||||||
/// Enables USART1 and configures 8N1 transmit-only operation.
|
/// Enables USART1 and configures 8N1 transmit and receive operation.
|
||||||
pub fn new(
|
pub fn new(
|
||||||
rcc: &stm32f722::RCC,
|
rcc: &stm32f722::RCC,
|
||||||
peripheral: stm32f722::USART1,
|
peripheral: stm32f722::USART1,
|
||||||
@@ -38,9 +49,17 @@ impl Usart1Tx {
|
|||||||
// SAFETY: `brr` is validated to fit the complete BRR register.
|
// SAFETY: `brr` is validated to fit the complete BRR register.
|
||||||
unsafe { write.bits(u32::from(brr)) }
|
unsafe { write.bits(u32::from(brr)) }
|
||||||
});
|
});
|
||||||
peripheral
|
peripheral.cr1().write(|write| {
|
||||||
.cr1()
|
write
|
||||||
.write(|write| write.te().set_bit().ue().set_bit());
|
.te()
|
||||||
|
.set_bit()
|
||||||
|
.re()
|
||||||
|
.set_bit()
|
||||||
|
.rxneie()
|
||||||
|
.set_bit()
|
||||||
|
.ue()
|
||||||
|
.set_bit()
|
||||||
|
});
|
||||||
peripheral.cr2().write(|write| write.stop().stop1());
|
peripheral.cr2().write(|write| write.stop().stop1());
|
||||||
|
|
||||||
Ok(Self { peripheral })
|
Ok(Self { peripheral })
|
||||||
@@ -53,6 +72,51 @@ impl Usart1Tx {
|
|||||||
pub(crate) fn transmit_data_register_address(&self) -> u32 {
|
pub(crate) fn transmit_data_register_address(&self) -> u32 {
|
||||||
self.peripheral.tdr().as_ptr() as u32
|
self.peripheral.tdr().as_ptr() as u32
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// Acknowledges one USART1 receive interrupt without accessing TX state.
|
||||||
|
///
|
||||||
|
/// The board invokes this only from the `USART1` interrupt. USART1 TX DMA
|
||||||
|
/// owns its disjoint data register and DMA stream, so this static route may
|
||||||
|
/// safely acknowledge RX while that transfer is active.
|
||||||
|
pub fn acknowledge_receive_interrupt() -> ReceiveInterrupt {
|
||||||
|
// SAFETY: the board binds USART1 exactly once and this function touches
|
||||||
|
// only its receive/error registers, never the TX DMA configuration.
|
||||||
|
let peripheral = unsafe { &*stm32f722::USART1::ptr() };
|
||||||
|
let status = peripheral.isr().read();
|
||||||
|
let fault = status.pe().is_error()
|
||||||
|
|| status.fe().is_error()
|
||||||
|
|| status.nf().is_noise()
|
||||||
|
|| status.ore().is_overrun();
|
||||||
|
|
||||||
|
if status.rxne().is_data_ready() {
|
||||||
|
let byte = peripheral.rdr().read().rdr().bits() as u8;
|
||||||
|
if fault {
|
||||||
|
clear_receive_errors(peripheral);
|
||||||
|
ReceiveInterrupt::Fault
|
||||||
|
} else {
|
||||||
|
ReceiveInterrupt::Byte(byte)
|
||||||
|
}
|
||||||
|
} else if fault {
|
||||||
|
clear_receive_errors(peripheral);
|
||||||
|
ReceiveInterrupt::Fault
|
||||||
|
} else {
|
||||||
|
ReceiveInterrupt::NotPending
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
fn clear_receive_errors(peripheral: &stm32f722::usart1::RegisterBlock) {
|
||||||
|
peripheral.icr().write(|write| {
|
||||||
|
write
|
||||||
|
.pecf()
|
||||||
|
.clear()
|
||||||
|
.fecf()
|
||||||
|
.clear()
|
||||||
|
.ncf()
|
||||||
|
.clear()
|
||||||
|
.orecf()
|
||||||
|
.clear()
|
||||||
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
const fn calculate_brr(peripheral_clock_hz: u32, baud: u32) -> Result<u16, BaudRateError> {
|
const fn calculate_brr(peripheral_clock_hz: u32, baud: u32) -> Result<u16, BaudRateError> {
|
||||||
|
|||||||
Reference in new issue
Block a user