379 lines
16 KiB
Rust
379 lines
16 KiB
Rust
#![no_std]
|
|
#![no_main]
|
|
#![deny(
|
|
clippy::mem_forget,
|
|
reason = "mem::forget is generally not safe to do with esp_hal types, especially those \
|
|
holding buffers for the duration of a data transfer."
|
|
)]
|
|
|
|
|
|
use alloc::{string::String, sync::Arc};
|
|
use embassy_executor::Spawner;
|
|
use embassy_time::{Duration, Instant, Timer, WithTimeout};
|
|
|
|
use enum_map::EnumMap;
|
|
use esp_hal::{gpio::{Event, Input, InputConfig, Output, OutputConfig, Pin}, handler, time::Rate};
|
|
use esp_hal::{
|
|
clock::CpuClock, timer::{systimer::SystemTimer, timg::{TimerGroup, Wdt}}
|
|
};
|
|
|
|
use embassy_sync::{
|
|
blocking_mutex::raw::NoopRawMutex, channel::DynamicReceiver, once_lock::OnceLock, pubsub::PubSubChannel, signal::Signal
|
|
};
|
|
use esp_storage::FlashStorage;
|
|
use log::*;
|
|
use renderbug_bike::{events::{Prediction, SensorSource, SensorState}, gpio_interrupt::{InterruptDispatch, PinInterrupt}, graphics::display::DisplayControls, logging::RenderbugLogger, simdata::IMUReading, storage::{SharedFlash, SimDataRecorder}, tasks::{oled::{OledUI, OledUiSurfacePool, oled_ui}, safetyui::{SafetyUi, safety_ui_main}, ui::UiSurfacePool}, tracing::Tracer};
|
|
use renderbug_bike::events::Measurement;
|
|
use static_cell::StaticCell;
|
|
use esp_backtrace as _;
|
|
use esp_hal::spi::master::any::Degrade;
|
|
|
|
use renderbug_bike::tasks::ui::{Ui, ui_main};
|
|
use esp_hal::dma::DmaChannelConvert;
|
|
|
|
use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, pubsub::DynSubscriber};
|
|
use embassy_sync::channel::Channel;
|
|
|
|
extern crate alloc;
|
|
|
|
// This creates a default app-descriptor required by the esp-idf bootloader.
|
|
// For more information see: <https://docs.espressif.com/projects/esp-idf/en/stable/esp32/api-reference/system/app_image_format.html#application-description>
|
|
esp_bootloader_esp_idf::esp_app_desc!();
|
|
|
|
#[cfg(feature="radio")]
|
|
static WIFI_INIT: StaticCell<esp_radio::Controller<'static>> = StaticCell::new();
|
|
|
|
rtos_trace::global_trace!(Tracer);
|
|
|
|
static INTERRUPTS: OnceLock<InterruptDispatch<'static, 4>> = OnceLock::new();
|
|
#[handler]
|
|
fn gpio_interrupt_handler() {
|
|
INTERRUPTS.try_get().unwrap().process_interrupts();
|
|
}
|
|
|
|
#[esp_rtos::main]
|
|
async fn main(spawner: Spawner) {
|
|
// If we aren't using the second CPU, we can use the bootloader space for the heap instead
|
|
if cfg!(not(feature="dual-core")) {
|
|
esp_alloc::heap_allocator!(#[esp_hal::ram(reclaimed)] size: 73744);
|
|
esp_alloc::heap_allocator!(size: 32 * 1024);
|
|
} else {
|
|
esp_alloc::heap_allocator!(size: 100000);
|
|
}
|
|
|
|
RenderbugLogger::init_logger();
|
|
|
|
let config = esp_hal::Config::default().with_cpu_clock(CpuClock::max());
|
|
let peripherals = esp_hal::init(config);
|
|
|
|
info!("Boot memory stats: {}", esp_alloc::HEAP.stats());
|
|
|
|
let sys_timer = SystemTimer::new(peripherals.SYSTIMER);
|
|
esp_rtos::start(sys_timer.alarm0);
|
|
info!("Embassy initialized!");
|
|
|
|
static MOTION_BUS: StaticCell<Channel<CriticalSectionRawMutex,Measurement,5> > = StaticCell::new();
|
|
let motion_bus = MOTION_BUS.init_with(|| { Channel::new() });
|
|
|
|
static RECORDING_BUS: StaticCell<PubSubChannel<CriticalSectionRawMutex,Measurement,1, 1, 1> > = StaticCell::new();
|
|
let recording_bus = RECORDING_BUS.init_with(|| { PubSubChannel::new() });
|
|
|
|
info!("Setting up rendering pipeline");
|
|
let mut surfaces = UiSurfacePool::default();
|
|
let ui = Ui::new(&mut surfaces);
|
|
|
|
let display_controls = DisplayControls::default();
|
|
let oled_controls = DisplayControls::default();
|
|
|
|
let mut oled_surfaces = OledUiSurfacePool::default();
|
|
let oled_uniforms = Default::default();
|
|
let oledui = OledUI::new(&mut oled_surfaces, oled_controls.clone(), Arc::clone(&oled_uniforms));
|
|
|
|
let mut safety_surfaces = UiSurfacePool::default();
|
|
let safety_ui = SafetyUi::new(&mut safety_surfaces, display_controls.clone());
|
|
|
|
let timer0 = TimerGroup::new(peripherals.TIMG0);
|
|
let mut wdt = timer0.wdt;
|
|
wdt.set_timeout(esp_hal::timer::timg::MwdtStage::Stage0, esp_hal::time::Duration::from_secs(5));
|
|
wdt.enable();
|
|
|
|
// Spawn the rendering task as soon as possible so it can start pushing pixels
|
|
spawner.must_spawn(renderbug_bike::tasks::render::render(peripherals.SPI2.degrade(), peripherals.DMA_CH2.degrade(), peripherals.GPIO5.degrade(), surfaces, safety_surfaces, display_controls, wdt));
|
|
|
|
let imu_interrupt = PinInterrupt::new(Input::new(peripherals.GPIO36.degrade(), InputConfig::default()), Event::RisingEdge);
|
|
let pd_interrupt = PinInterrupt::new(Input::new(peripherals.GPIO16.degrade(), InputConfig::default()), Event::RisingEdge);
|
|
let sd_detect_interrupt = PinInterrupt::new(Input::new(peripherals.GPIO42.degrade(), InputConfig::default()), Event::RisingEdge);
|
|
let gps_pulse_interrupt = PinInterrupt::new(Input::new(peripherals.GPIO45.degrade(), InputConfig::default()), Event::RisingEdge);
|
|
|
|
INTERRUPTS.init(InterruptDispatch::new([
|
|
imu_interrupt.clone(),
|
|
pd_interrupt.clone(),
|
|
sd_detect_interrupt.clone(),
|
|
gps_pulse_interrupt.clone()
|
|
])).ok();
|
|
let mut io = esp_hal::gpio::Io::new(peripherals.IO_MUX);
|
|
io.set_interrupt_handler(gpio_interrupt_handler);
|
|
|
|
#[cfg(feature="motion")]
|
|
{
|
|
use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
|
|
use esp_hal::{i2c::master::{Config, I2c}, Async};
|
|
use embassy_sync::{blocking_mutex::raw::NoopRawMutex, mutex::Mutex};
|
|
|
|
static I2C_BUS: StaticCell<Mutex<NoopRawMutex, I2c<'static, Async>>> = StaticCell::new();
|
|
|
|
info!("Launching i2c sensor tasks");
|
|
let sda = peripherals.GPIO4;
|
|
let scl = peripherals.GPIO3;
|
|
let i2c = I2c::new(peripherals.I2C1, Config::default()).unwrap().with_scl(scl).with_sda(sda).into_async();
|
|
let i2c_bus = I2C_BUS.init(Mutex::new(i2c));
|
|
#[cfg(feature="mpu")]
|
|
spawner.must_spawn(renderbug_bike::tasks::mpu::mpu_task(motion_bus.dyn_sender(), I2cDevice::new(i2c_bus), imu_interrupt));
|
|
#[cfg(feature="gps")]
|
|
spawner.must_spawn(renderbug_bike::tasks::gps::gps_task(motion_bus.dyn_sender(), I2cDevice::new(i2c_bus)));
|
|
}
|
|
|
|
#[cfg(feature="oled")]
|
|
{
|
|
use esp_hal::i2c::master::{Config, I2c};
|
|
use renderbug_bike::graphics::ssd1306::SsdOutput;
|
|
|
|
let rst = Output::new(peripherals.GPIO21, esp_hal::gpio::Level::Low, OutputConfig::default());
|
|
let i2c = I2c::new(
|
|
peripherals.I2C0,
|
|
Config::default().with_frequency(Rate::from_khz(400))
|
|
).unwrap().with_scl(peripherals.GPIO18).with_sda(peripherals.GPIO17).into_async();
|
|
let output = SsdOutput::new(i2c, rst, oled_controls).await;
|
|
spawner.must_spawn(renderbug_bike::tasks::oled_render::oled_render(output, oled_surfaces, oled_uniforms));
|
|
}
|
|
|
|
let mut storage = renderbug_bike::storage::SharedFlash::new(esp_storage::FlashStorage::new());
|
|
let mut partition_buf = [8; 1024];
|
|
let partitions = esp_bootloader_esp_idf::partitions::read_partition_table(&mut storage, &mut partition_buf).unwrap();
|
|
|
|
#[cfg(feature="simulation")]
|
|
{
|
|
use renderbug_bike::tasks::simulation::SimDataTable;
|
|
for sim_data in SimDataTable::open(storage, partitions).expect("Could not find sim data!") {
|
|
let srcid = sim_data.srcid();
|
|
info!("Found simulation data for {srcid:?}");
|
|
if spawner.spawn(renderbug_bike::tasks::simulation::simulation_task(sim_data, motion_bus.dyn_sender())).is_err() {
|
|
error!("Unable to spawn simulation task for {srcid:?}! Increase the task pool size.");
|
|
}
|
|
}
|
|
}
|
|
|
|
#[cfg(not(feature="simulation"))]
|
|
{
|
|
use renderbug_bike::storage::SimDataRecorder;
|
|
|
|
let recorder = SimDataRecorder::open(storage, partitions).expect("Unable to open sim data partition for writing");
|
|
//spawner.spawn(record_telemetry(recording_bus.dyn_receiver(), recorder)).unwrap();
|
|
}
|
|
|
|
spawner.spawn(print_sensor_readings(recording_bus.dyn_subscriber().unwrap())).unwrap();
|
|
|
|
#[cfg(feature="radio")]
|
|
let (wifi, network_device, ble) = {
|
|
info!("Configuring wifi");
|
|
esp_radio::wifi_set_log_verbose();
|
|
let wifi_init = WIFI_INIT.init_with(|| {esp_radio::init().expect("Failed to initialize radio controller")});
|
|
|
|
let ble = esp_radio::ble::controller::BleConnector::new(wifi_init, peripherals.BT, esp_radio::ble::Config::default()).unwrap();
|
|
|
|
let wifi_config = esp_radio::wifi::Config::default()
|
|
.with_rx_queue_size(64)
|
|
.with_tx_queue_size(256)
|
|
.with_static_rx_buf_num(32)
|
|
.with_static_tx_buf_num(32)
|
|
.with_dynamic_rx_buf_num(128)
|
|
.with_dynamic_tx_buf_num(128)
|
|
.with_rx_ba_win(7)
|
|
.with_power_save_mode(esp_radio::wifi::PowerSaveMode::Minimum);
|
|
let (wifi, interfaces) = esp_radio::wifi::new(wifi_init, peripherals.WIFI, wifi_config)
|
|
.expect("Failed to initialize WIFI!");
|
|
|
|
(wifi, interfaces.sta, ble)
|
|
};
|
|
|
|
let core2_main = |spawner: Spawner| {
|
|
info!("Starting application tasks");
|
|
|
|
static PREDICTIONS: StaticCell<PubSubChannel<NoopRawMutex, Prediction, 15, 6, 1>> = StaticCell::new();
|
|
let predictions = PREDICTIONS.init(PubSubChannel::new());
|
|
|
|
#[cfg(not(feature="demo"))]
|
|
{
|
|
info!("Launching motion engine");
|
|
spawner.must_spawn(renderbug_bike::tasks::motion::motion_task(motion_bus.dyn_receiver(), predictions.dyn_publisher().unwrap(), recording_bus.dyn_publisher().unwrap()));
|
|
}
|
|
|
|
#[cfg(feature="demo")]
|
|
{
|
|
warn!("Launching with demo sequencer");
|
|
spawner.must_spawn(renderbug_bike::tasks::demo::demo_task(predictions.dyn_publisher().unwrap()));
|
|
}
|
|
|
|
info!("Launching Safety UI");
|
|
spawner.must_spawn(safety_ui_main(predictions.dyn_subscriber().unwrap(), safety_ui));
|
|
info!("Launching UI");
|
|
spawner.must_spawn(ui_main(predictions.dyn_subscriber().unwrap(), ui));
|
|
info!("Launching OLED UI");
|
|
spawner.must_spawn(oled_ui(predictions.dyn_subscriber().unwrap(), oledui));
|
|
|
|
#[cfg(feature="radio")]
|
|
{
|
|
use embassy_net::StackResources;
|
|
use esp_hal::rng::Rng;
|
|
use static_cell::ConstStaticCell;
|
|
|
|
info!("Setting up networking stack");
|
|
static RESOURCES: ConstStaticCell<StackResources<5>> = ConstStaticCell::new(StackResources::new());
|
|
let config = embassy_net::Config::dhcpv4(Default::default());
|
|
let seed = Rng::new().random() as i32;
|
|
let (stack, runner) = embassy_net::new(network_device, config, RESOURCES.take(), seed as u64);
|
|
info!("Launching network services");
|
|
spawner.must_spawn(renderbug_bike::tasks::wifi::net_task(runner));
|
|
|
|
info!("Starting connectivity task");
|
|
spawner.must_spawn(renderbug_bike::tasks::wifi::wifi_connect_task(wifi, motion_bus.dyn_sender()));
|
|
|
|
info!("Launching HTTP telemetry");
|
|
spawner.must_spawn(renderbug_bike::tasks::wifi::http_telemetry_task(predictions.dyn_subscriber().unwrap(), stack, motion_bus.dyn_sender()));
|
|
|
|
info!("Starting BLE services");
|
|
spawner.must_spawn(renderbug_bike::tasks::ble::ble_task(ble, predictions.dyn_subscriber().unwrap(), spawner));
|
|
}
|
|
|
|
#[cfg(feature="dual-core")]
|
|
{
|
|
info!("Launching core 2 watchdog");
|
|
let timer1 = TimerGroup::new(peripherals.TIMG1);
|
|
let mut ui_wdt = timer1.wdt;
|
|
ui_wdt.set_timeout(esp_hal::timer::timg::MwdtStage::Stage0, esp_hal::time::Duration::from_secs(60));
|
|
ui_wdt.enable();
|
|
spawner.must_spawn(wdt_task(ui_wdt));
|
|
}
|
|
|
|
spawner.must_spawn(print_sensor_status(predictions.dyn_subscriber().unwrap()));
|
|
|
|
info!("Ready to rock and roll in {}ms", Instant::now().as_millis());
|
|
};
|
|
|
|
#[allow(static_mut_refs)]
|
|
#[cfg(feature="dual-core")]
|
|
{
|
|
use core::mem::MaybeUninit;
|
|
|
|
use esp_hal::interrupt::software::SoftwareInterruptControl;
|
|
use esp_hal::system::Stack;
|
|
use esp_rtos::embassy::Executor;
|
|
|
|
// We can be sneaky and stick the stack for the second core into the bootloader ram
|
|
#[esp_hal::ram(reclaimed)]
|
|
static mut CORE2_STACK: MaybeUninit<Stack<73744>> = MaybeUninit::uninit();
|
|
let swi = SoftwareInterruptControl::new(peripherals.SW_INTERRUPT);
|
|
// SAFETY: The internal implementation of Stack is itself MaybeUninit
|
|
esp_rtos::start_second_core(peripherals.CPU_CTRL, swi.software_interrupt0, swi.software_interrupt1, unsafe { CORE2_STACK.assume_init_mut() }, || {
|
|
info!("Second CPU core started");
|
|
static CORE2_EXEC: StaticCell<Executor> = StaticCell::new();
|
|
let exec = CORE2_EXEC.init_with(|| { Executor::new() });
|
|
exec.run(core2_main);
|
|
});
|
|
}
|
|
|
|
#[cfg(not(feature="dual-core"))]
|
|
core2_main(spawner);
|
|
|
|
loop {
|
|
//info!("Memory stats: {}", esp_alloc::HEAP.stats());
|
|
Timer::after_secs(1).await;
|
|
}
|
|
}
|
|
|
|
#[embassy_executor::task]
|
|
async fn wdt_task(mut wdt: Wdt<esp_hal::peripherals::TIMG1<'static>>) {
|
|
loop {
|
|
// Watchdog is set to trip after 5 seconds, so we wait just long enough before it trips
|
|
// TODO: We should maybe extend this timeout to 30s or 1m when the system is sleeping
|
|
trace!("Bark wdt");
|
|
wdt.feed();
|
|
Timer::after_secs(3).await;
|
|
}
|
|
}
|
|
|
|
#[embassy_executor::task]
|
|
async fn record_telemetry(firehose: DynamicReceiver<'static, Measurement>, mut storage: SimDataRecorder<SharedFlash<FlashStorage>>) {
|
|
loop {
|
|
match firehose.receive().await {
|
|
Measurement::IMU { accel, gyro } => {
|
|
let reading = IMUReading {
|
|
accel_x: accel.x as f64,
|
|
accel_y: accel.y as f64,
|
|
accel_z: accel.z as f64,
|
|
gyro_x: gyro.x as f64,
|
|
gyro_y: gyro.y as f64,
|
|
gyro_z: gyro.z as f64
|
|
};
|
|
storage.write_next(reading).unwrap();
|
|
info!("Wrote IMU to flash");
|
|
},
|
|
_ => ()
|
|
}
|
|
}
|
|
}
|
|
|
|
#[embassy_executor::task]
|
|
async fn print_sensor_readings(mut events: DynSubscriber<'static, Measurement>) {
|
|
loop {
|
|
match events.next_message_pure().await {
|
|
Measurement::IMU { accel, gyro } => {
|
|
esp_println::println!("accel=({},{},{}) gyro=({},{},{})", accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z);
|
|
},
|
|
Measurement::GPS(gps) => {
|
|
esp_println::println!("gps={gps:?}");
|
|
},
|
|
_ => ()
|
|
}
|
|
}
|
|
}
|
|
|
|
#[embassy_executor::task]
|
|
async fn print_sensor_status(mut events: DynSubscriber<'static, Prediction>) {
|
|
|
|
info!("telemetry ready");
|
|
let mut sensor_states: EnumMap<SensorSource, SensorState> = EnumMap::default();
|
|
loop {
|
|
let next = events.next_message_pure().with_timeout(Duration::from_secs(5)).await;
|
|
match next {
|
|
Ok(Prediction::SensorStatus(sensor, status)) => {
|
|
sensor_states[sensor] = status;
|
|
let mut report_str = String::new();
|
|
for (sensor, state) in &sensor_states {
|
|
let state_icon = match state {
|
|
SensorState::AcquiringFix => "?",
|
|
SensorState::Degraded => "-",
|
|
SensorState::Offline => "X",
|
|
SensorState::Online => "O"
|
|
};
|
|
report_str += alloc::format!("{sensor:?}={state_icon} ").as_str();
|
|
}
|
|
info!("{report_str}");
|
|
},
|
|
Err(_) => {
|
|
let mut report_str = String::new();
|
|
for (sensor, state) in &sensor_states {
|
|
let state_icon = match state {
|
|
SensorState::AcquiringFix => "?",
|
|
SensorState::Degraded => "-",
|
|
SensorState::Offline => "X",
|
|
SensorState::Online => "O"
|
|
};
|
|
report_str += alloc::format!("{sensor:?}={state_icon} ").as_str();
|
|
}
|
|
info!("{report_str}");
|
|
},
|
|
_ => ()
|
|
}
|
|
}
|
|
} |