| 1 | #![no_std] |
| 2 | #![no_main] |
| 3 | |
| 4 | mod mpu; |
| 5 | mod rf; |
| 6 | mod status; |
| 7 | mod control; |
| 8 | mod comm; |
| 9 | |
| 10 | use defmt::trace; |
| 11 | use embassy_stm32::can::filter::Mask32; |
| 12 | use embassy_stm32::can::{self, Fifo}; |
| 13 | use embassy_stm32::can::Can; |
| 14 | use embassy_stm32::peripherals::CAN1; |
| 15 | use embassy_stm32::time::Hertz; |
| 16 | use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex; |
| 17 | |
| 18 | use embassy_sync::mutex::Mutex; |
| 19 | use mpu::BUFFERED_QUATERNIONS; |
| 20 | use comm::CAN_BUFFER; |
| 21 | use mpu6050_dmp::quaternion::Quaternion; |
| 22 | use rf::transmit; |
| 23 | use status::SystemStatus; |
| 24 | |
| 25 | use embassy_executor::Spawner; |
| 26 | use embassy_stm32::{ |
| 27 | bind_interrupts, |
| 28 | exti::ExtiInput, |
| 29 | gpio::{Level, Output, Speed}, |
| 30 | i2c::{self, I2c}, |
| 31 | rcc::{self}, |
| 32 | usart::{self, Uart}, |
| 33 | Config, |
| 34 | }; |
| 35 | use embassy_sync::channel::Channel; |
| 36 | |
| 37 | use crate::control::Frame; |
| 38 | use crate::{comm::can_tx_task, comm::can_rx_task, control::update_control_loop, mpu::read_mpu}; |
| 39 | use crate::mpu::telemetry_sender; |
| 40 | use crate::status::status_leds; |
| 41 | use crate::status::SharedStatus; |
| 42 | |
| 43 | use {defmt_rtt as _, panic_probe as _}; |
| 44 | |
| 45 | bind_interrupts!(struct Irqs { |
| 46 | I2C1_EV => i2c::EventInterruptHandler<embassy_stm32::peripherals::I2C1>; |
| 47 | I2C1_ER => i2c::ErrorInterruptHandler<embassy_stm32::peripherals::I2C1>; |
| 48 | USART3 => usart::InterruptHandler<embassy_stm32::peripherals::USART3>; |
| 49 | CAN1_RX0 => can::Rx0InterruptHandler<CAN1>; |
| 50 | CAN1_RX1 => can::Rx1InterruptHandler<CAN1>; |
| 51 | CAN1_SCE => can::SceInterruptHandler<CAN1>; |
| 52 | CAN1_TX => can::TxInterruptHandler<CAN1>; |
| 53 | }); |
| 54 | |
| 55 | #[embassy_executor::main] |
| 56 | async fn main(spawner: Spawner) { |
| 57 | trace! {"Starting system up..."}; |
| 58 | let mut config = Config::default(); |
| 59 | |
| 60 | config.rcc.hse = Some(rcc::Hse { |
| 61 | freq: embassy_stm32::time::Hertz(12_000_000), |
| 62 | mode: rcc::HseMode::Oscillator, |
| 63 | }); |
| 64 | |
| 65 | config.rcc.sys = rcc::Sysclk::PLL1_R; |
| 66 | config.rcc.pll = Some(rcc::Pll { |
| 67 | source: rcc::PllSource::HSE, |
| 68 | prediv: rcc::PllPreDiv::DIV2, |
| 69 | mul: rcc::PllMul::MUL8, |
| 70 | divp: None, |
| 71 | divq: Some(rcc::PllQDiv::DIV2), // PLL1_Q clock (32 / 2 * 6 / 2), used for RNG |
| 72 | divr: Some(rcc::PllRDiv::DIV2), // sysclk 48Mhz clock (32 / 2 * 6 / 2) |
| 73 | }); |
| 74 | |
| 75 | let p = embassy_stm32::init(config); |
| 76 | |
| 77 | let mut can_bus = Can::new(p.CAN1, p.PB8, p.PB9, Irqs); |
| 78 | |
| 79 | // enable the fifo 0 bank for storing the CAN message queue |
| 80 | can_bus.modify_filters().enable_bank(0, Fifo::Fifo0, Mask32::accept_all()); |
| 81 | |
| 82 | // enable loopback for testing |
| 83 | can_bus.modify_config() |
| 84 | .set_loopback(true) // Receive own frames |
| 85 | .set_silent(true) |
| 86 | .set_bitrate(250_000); |
| 87 | can_bus.enable().await; |
| 88 | |
| 89 | let (can_tx, can_rx) = can_bus.split(); |
| 90 | |
| 91 | let mut i2c_config = i2c::Config::default(); |
| 92 | i2c_config.frequency = Hertz::khz(400); |
| 93 | i2c_config.gpio_speed = Speed::VeryHigh; |
| 94 | |
| 95 | let iic = I2c::new( |
| 96 | p.I2C1, p.PA9, p.PA10, Irqs, p.DMA1_CH6, p.DMA1_CH7, i2c_config, |
| 97 | ); |
| 98 | let _lsb_pin = Output::new(p.PA11, Level::Low, Speed::Low); |
| 99 | |
| 100 | let ok_pin = p.PC14; |
| 101 | let fail_pin = p.PC15; |
| 102 | let txpin = p.PA6; |
| 103 | |
| 104 | let _rxen = Output::new(p.PB0, Level::High, Speed::Low); |
| 105 | let _txen = Output::new(p.PB1, Level::Low, Speed::Low); |
| 106 | |
| 107 | core::mem::forget(_rxen); |
| 108 | core::mem::forget(_txen); |
| 109 | core::mem::forget(_lsb_pin); |
| 110 | |
| 111 | let imu_int = ExtiInput::new(p.PA12, p.EXTI12, embassy_stm32::gpio::Pull::Down); |
| 112 | |
| 113 | // create shared channel between the MPU polling thread and the UART sender for the quaternion data buffer. |
| 114 | static QUATERNION_CHANNEL: Channel<ThreadModeRawMutex, Quaternion, BUFFERED_QUATERNIONS> = |
| 115 | Channel::<ThreadModeRawMutex, Quaternion, BUFFERED_QUATERNIONS>::new(); |
| 116 | static SHARED_STATUS: SharedStatus = Mutex::new(SystemStatus::FAIL); |
| 117 | static CAN_MSG_QUEUE: Channel<ThreadModeRawMutex, u32, CAN_BUFFER> = Channel::<ThreadModeRawMutex, u32, CAN_BUFFER>::new(); |
| 118 | |
| 119 | let tmtry_uart = Uart::new( |
| 120 | p.USART3, |
| 121 | p.PC5, |
| 122 | p.PC4, |
| 123 | Irqs, |
| 124 | p.DMA1_CH2, |
| 125 | p.DMA1_CH3, |
| 126 | usart::Config::default(), |
| 127 | ) |
| 128 | .unwrap(); |
| 129 | tmtry_uart.set_baudrate(115200).unwrap(); |
| 130 | |
| 131 | trace! {"Initializing tasks..."}; |
| 132 | |
| 133 | // dedicated task for the RF transmitter |
| 134 | spawner.spawn(transmit(txpin.into())).unwrap(); |
| 135 | // dedicated task for MPU data readings |
| 136 | spawner |
| 137 | .spawn(read_mpu(iic, imu_int, QUATERNION_CHANNEL.sender())) |
| 138 | .unwrap(); |
| 139 | // dedicated task for UART telemetry |
| 140 | #[cfg(telemetry)] |
| 141 | spawner |
| 142 | .spawn(telemetry_sender(tmtry_uart, QUATERNION_CHANNEL.receiver())) |
| 143 | .unwrap(); |
| 144 | // dedicated task for CAN bus communications |
| 145 | spawner |
| 146 | .spawn(can_rx_task(can_rx)) |
| 147 | .unwrap(); |
| 148 | // transmitter task |
| 149 | spawner |
| 150 | .spawn(can_tx_task(can_tx, CAN_MSG_QUEUE.receiver())) |
| 151 | .unwrap(); |
| 152 | |
| 153 | // dedicated task for the quadcopter control loop |
| 154 | spawner |
| 155 | .spawn(update_control_loop(QUATERNION_CHANNEL.receiver(), None)) |
| 156 | .unwrap(); |
| 157 | // dedicated task for status leds |
| 158 | spawner |
| 159 | .spawn(status_leds(ok_pin.into(), fail_pin.into(), &SHARED_STATUS)) |
| 160 | .unwrap(); |
| 161 | } |
| 162 | |
