bump a lot of big changes I dont want to break down into individual commits
This commit is contained in:
274
src/bin/main.rs
274
src/bin/main.rs
@@ -5,64 +5,37 @@
|
||||
reason = "mem::forget is generally not safe to do with esp_hal types, especially those \
|
||||
holding buffers for the duration of a data transfer."
|
||||
)]
|
||||
#![feature(future_join)]
|
||||
|
||||
use core::ptr::addr_of_mut;
|
||||
|
||||
use bleps::ad_structure::{create_advertising_data, AdStructure, BR_EDR_NOT_SUPPORTED, LE_GENERAL_DISCOVERABLE};
|
||||
use bleps::attribute_server::{AttributeServer, NotificationData};
|
||||
use bleps::{gatt, Ble, HciConnector};
|
||||
use embassy_executor::Spawner;
|
||||
use embassy_sync::channel::DynamicReceiver;
|
||||
use embassy_time::{Instant, Timer};
|
||||
use esp_backtrace as _;
|
||||
|
||||
#[allow(unused_imports)]
|
||||
use esp_hal::{
|
||||
gpio::Pin,
|
||||
interrupt::software::SoftwareInterruptControl,
|
||||
clock::CpuClock,
|
||||
system::{AppCoreGuard, CpuControl, Stack},
|
||||
timer::{AnyTimer, systimer::SystemTimer, timg::TimerGroup}
|
||||
clock::CpuClock, interrupt::software::SoftwareInterruptControl, system::{AppCoreGuard, CpuControl, Stack}, timer::{systimer::SystemTimer, timg::{TimerGroup, Wdt}},
|
||||
gpio::Pin
|
||||
};
|
||||
|
||||
use esp_hal_embassy::{Executor, InterruptExecutor};
|
||||
use esp_wifi::ble::controller::BleConnector;
|
||||
use figments::surface::BufferedSurfacePool;
|
||||
use log::info;
|
||||
use renderbug_embassy::events::{BusGarage, Measurement, Telemetry};
|
||||
use serde_json::json;
|
||||
use log::*;
|
||||
use renderbug_embassy::{logging::RenderbugLogger, tasks::ui::UiSurfacePool};
|
||||
use renderbug_embassy::events::BusGarage;
|
||||
use static_cell::StaticCell;
|
||||
|
||||
#[cfg(feature="simulation")]
|
||||
use renderbug_embassy::tasks::simulation::{motion_simulation_task, location_simulation_task};
|
||||
#[cfg(not(feature="simulation"))]
|
||||
use renderbug_embassy::tasks::{gps::gps_task, mpu::mpu_task};
|
||||
#[cfg(not(feature="simulation"))]
|
||||
use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
|
||||
#[cfg(not(feature="simulation"))]
|
||||
use esp_hal::{
|
||||
time::Rate,
|
||||
i2c::master::{Config, I2c},
|
||||
Async,
|
||||
};
|
||||
#[cfg(not(feature="simulation"))]
|
||||
use embassy_sync::{
|
||||
mutex::Mutex,
|
||||
blocking_mutex::raw::CriticalSectionRawMutex
|
||||
};
|
||||
use esp_backtrace as _;
|
||||
|
||||
use renderbug_embassy::tasks::{
|
||||
render::render,
|
||||
motion::motion_task,
|
||||
ui::{Ui, ui_main}
|
||||
};
|
||||
|
||||
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(not(feature="simulation"))]
|
||||
static I2C_BUS: StaticCell<Mutex<CriticalSectionRawMutex, I2c<'static, Async>>> = StaticCell::new();
|
||||
static STATIC_HI_EXEC: StaticCell<InterruptExecutor<0>> = StaticCell::new();
|
||||
|
||||
static BUS_GARAGE: StaticCell<BusGarage> = StaticCell::new();
|
||||
static mut CORE2_STACK: Stack<8192> = Stack::new();
|
||||
@@ -70,198 +43,113 @@ static CORE_HANDLE: StaticCell<AppCoreGuard> = StaticCell::new();
|
||||
|
||||
#[esp_hal_embassy::main]
|
||||
async fn main(spawner: Spawner) {
|
||||
esp_println::logger::init_logger_from_env();
|
||||
critical_section::with(|_| {
|
||||
RenderbugLogger::init_logger();
|
||||
//esp_println::logger::init_logger_from_env();
|
||||
});
|
||||
|
||||
let config = esp_hal::Config::default().with_cpu_clock(CpuClock::max());
|
||||
let peripherals = esp_hal::init(config);
|
||||
|
||||
esp_alloc::heap_allocator!(size: 128 * 1024);
|
||||
|
||||
let timer0 = SystemTimer::new(peripherals.SYSTIMER);
|
||||
esp_hal_embassy::init([timer0.alarm0, timer0.alarm1, timer0.alarm2]);
|
||||
let sys_timer = SystemTimer::new(peripherals.SYSTIMER);
|
||||
esp_hal_embassy::init([sys_timer.alarm0, sys_timer.alarm1, sys_timer.alarm2]);
|
||||
info!("Embassy initialized!");
|
||||
|
||||
let timer0 = TimerGroup::new(peripherals.TIMG0);
|
||||
|
||||
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(10));
|
||||
//ui_wdt.enable(); //FIXME: Re-enable UI watchdog once we have a brain task running
|
||||
|
||||
let garage = BUS_GARAGE.init(Default::default());
|
||||
|
||||
info!("Setting up rendering pipeline");
|
||||
let mut surfaces = BufferedSurfacePool::default();
|
||||
let ui = Ui::new(&mut surfaces, &garage.display);
|
||||
let mut surfaces = UiSurfacePool::default();
|
||||
let ui = Ui::new(&mut surfaces, garage.display.clone());
|
||||
|
||||
let mut wdt = timer0.wdt;
|
||||
wdt.set_timeout(esp_hal::timer::timg::MwdtStage::Stage0, esp_hal::time::Duration::from_secs(3));
|
||||
let swi = SoftwareInterruptControl::new(peripherals.SW_INTERRUPT);
|
||||
static STATIC_HI_EXEC: StaticCell<InterruptExecutor<0>> = StaticCell::new();
|
||||
let hi_exec = STATIC_HI_EXEC.init(InterruptExecutor::new(swi.software_interrupt0));
|
||||
let hi_spawn = hi_exec.start(esp_hal::interrupt::Priority::Priority1);
|
||||
hi_spawn.must_spawn(render(peripherals.RMT, peripherals.GPIO5.degrade(), surfaces, &garage.display));
|
||||
#[allow(unused_variables)]
|
||||
let hi_spawn = hi_exec.start(esp_hal::interrupt::Priority::max());
|
||||
|
||||
#[cfg(not(feature="simulation"))]
|
||||
#[cfg(not(feature="headless"))]
|
||||
{
|
||||
wdt.enable();
|
||||
hi_spawn.must_spawn(renderbug_embassy::tasks::render::render(peripherals.RMT, peripherals.GPIO5.degrade(), surfaces, garage.display.clone(), wdt));
|
||||
}
|
||||
#[cfg(feature="headless")]
|
||||
garage.display.render_is_running.signal(true);
|
||||
|
||||
#[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::CriticalSectionRawMutex, mutex::Mutex};
|
||||
|
||||
static I2C_BUS: StaticCell<Mutex<CriticalSectionRawMutex, I2c<'static, Async>>> = StaticCell::new();
|
||||
|
||||
info!("Launching i2c sensor tasks");
|
||||
let i2c = I2c::new(peripherals.I2C1, Config::default().with_frequency(Rate::from_khz(400)).with_timeout(esp_hal::i2c::master::BusTimeout::Maximum)).unwrap().with_scl(peripherals.GPIO4).with_sda(peripherals.GPIO3).into_async();
|
||||
let sda = peripherals.GPIO3;
|
||||
let scl = peripherals.GPIO4;
|
||||
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));
|
||||
spawner.must_spawn(mpu_task(garage.motion.dyn_sender(), I2cDevice::new(i2c_bus)));
|
||||
spawner.must_spawn(gps_task(garage.motion.dyn_sender(), I2cDevice::new(i2c_bus)));
|
||||
#[cfg(feature="mpu")]
|
||||
spawner.must_spawn(renderbug_embassy::tasks::mpu::mpu_task(garage.motion.dyn_sender(), I2cDevice::new(i2c_bus)));
|
||||
#[cfg(feature="gps")]
|
||||
spawner.must_spawn(renderbug_embassy::tasks::gps::gps_task(garage.motion.dyn_sender(), I2cDevice::new(i2c_bus)));
|
||||
}
|
||||
|
||||
#[cfg(feature="simulation")]
|
||||
{
|
||||
spawner.must_spawn(motion_simulation_task(garage.motion.dyn_sender()));
|
||||
spawner.must_spawn(location_simulation_task(garage.motion.dyn_sender()));
|
||||
spawner.must_spawn(renderbug_embassy::tasks::simulation::motion_simulation_task(garage.motion.dyn_sender()));
|
||||
spawner.must_spawn(renderbug_embassy::tasks::simulation::location_simulation_task(garage.motion.dyn_sender()));
|
||||
}
|
||||
|
||||
info!("Launching motion engine");
|
||||
spawner.must_spawn(motion_task(garage.motion.dyn_receiver(), garage.scenes.dyn_sender(), garage.telemetry.dyn_sender()));
|
||||
spawner.must_spawn(motion_task(garage.motion.dyn_receiver(), garage.notify.dyn_sender(), garage.predict.dyn_sender()));
|
||||
|
||||
info!("Starting communications and UI on core 2");
|
||||
info!("Starting core 2");
|
||||
let mut cpu_control = CpuControl::new(peripherals.CPU_CTRL);
|
||||
CORE_HANDLE.init(cpu_control.start_app_core(unsafe { &mut *addr_of_mut!(CORE2_STACK) }, || {
|
||||
static STATIC_EXEC: StaticCell<Executor> = StaticCell::new();
|
||||
let exec = STATIC_EXEC.init(Executor::new());
|
||||
exec.run(|spawner| {
|
||||
let timer1 = TimerGroup::new(peripherals.TIMG0);
|
||||
spawner.must_spawn(wifi_task(garage.telemetry.dyn_receiver(), timer1.timer0.into(), peripherals.RNG, peripherals.WIFI, peripherals.BT));
|
||||
spawner.must_spawn(ui_main(garage.scenes.dyn_receiver(), ui));
|
||||
#[cfg(feature="radio")]
|
||||
{
|
||||
info!("Launching wifi");
|
||||
spawner.must_spawn(renderbug_embassy::tasks::wifi::wireless_task(garage.notify.dyn_receiver(), timer0.timer0.into(), peripherals.RNG, peripherals.WIFI, peripherals.BT));
|
||||
}
|
||||
info!("Launching UI");
|
||||
spawner.must_spawn(ui_main(garage.notify.dyn_receiver(), ui));
|
||||
#[cfg(feature="demo")]
|
||||
{
|
||||
warn!("Launching with demo sequencer");
|
||||
spawner.must_spawn(renderbug_embassy::tasks::demo::demo_task(garage.notify.dyn_sender()));
|
||||
}
|
||||
#[cfg(not(feature="demo"))]
|
||||
{
|
||||
info!("Launching prediction engine");
|
||||
spawner.must_spawn(renderbug_embassy::tasks::predict::prediction_task(garage.predict.dyn_receiver(), garage.notify.dyn_sender()));
|
||||
}
|
||||
info!("Launching core 2 watchdog");
|
||||
spawner.must_spawn(wdt_task(ui_wdt));
|
||||
|
||||
info!("System is ready in {}ms", Instant::now().as_millis());
|
||||
});
|
||||
}).unwrap());
|
||||
|
||||
info!("System is ready in {}ms", Instant::now().as_millis());
|
||||
}
|
||||
|
||||
// TODO: Wifi task needs to know when there is data to upload, so it only connects when needed.
|
||||
#[embassy_executor::task]
|
||||
async fn wifi_task(telemetry: DynamicReceiver<'static, Telemetry>, timer: AnyTimer<'static>, rng: esp_hal::peripherals::RNG<'static>, _wifi_device: esp_hal::peripherals::WIFI<'static>, bluetooth_device: esp_hal::peripherals::BT<'static>) {
|
||||
let rng = esp_hal::rng::Rng::new(rng);
|
||||
let wifi_init =
|
||||
esp_wifi::init(timer, rng).expect("Failed to initialize WIFI/BLE controller");
|
||||
|
||||
info!("Setting up BLE stack");
|
||||
let connector = BleConnector::new(&wifi_init, bluetooth_device);
|
||||
let get_millis = || esp_hal::time::Instant::now().duration_since_epoch().as_millis();
|
||||
let hci = HciConnector::new(connector, get_millis);
|
||||
let mut ble = Ble::new(&hci);
|
||||
ble.init().unwrap();
|
||||
ble.cmd_set_le_advertising_parameters().unwrap();
|
||||
ble.cmd_set_le_advertising_data(
|
||||
create_advertising_data(&[
|
||||
AdStructure::Flags(LE_GENERAL_DISCOVERABLE | BR_EDR_NOT_SUPPORTED),
|
||||
AdStructure::ServiceUuids16(&[Uuid::Uuid16(0x0001)]),
|
||||
AdStructure::CompleteLocalName("Renderbug!")
|
||||
])
|
||||
.unwrap()
|
||||
).unwrap();
|
||||
ble.cmd_set_le_advertise_enable(true).unwrap();
|
||||
|
||||
let mut wf1 = |_offset: usize, data: &[u8]| {
|
||||
info!("Read serial data! {data:?}");
|
||||
};
|
||||
|
||||
let s = &b""[..];
|
||||
// Other useful characteristics:
|
||||
// 0x2A67 - Location and speed
|
||||
// 0x2A00 - Device name
|
||||
// 0x2B90 - Device time
|
||||
// Permitted characteristics:
|
||||
// Acceleration
|
||||
// Force
|
||||
// Length
|
||||
// Linear position
|
||||
// Rotational speed
|
||||
// Temperature
|
||||
// Torque
|
||||
// Useful app that logs data: https://github.com/a2ruan/ArduNetApp?tab=readme-ov-file
|
||||
// Requires service 4fafc201-1fb5-459e-8fcc-c5c9c331914b, characteristic beb5483e-36e1-4688-b7f5-ea07361b26a8
|
||||
gatt!([service {
|
||||
uuid: "6E400001-B5A3-F393-E0A9-E50E24DCCA9E", // Nordic UART
|
||||
characteristics: [
|
||||
characteristic {
|
||||
uuid: "6E400003-B5A3-F393-E0A9-E50E24DCCA9E", // TX from device, everything is sent as notifications
|
||||
notify: true,
|
||||
name: "tx",
|
||||
value: s
|
||||
},
|
||||
characteristic {
|
||||
uuid: "6E400002-B5A3-F393-E0A9-E50E24DCCA9E", // RX from phone
|
||||
write: wf1
|
||||
},
|
||||
]
|
||||
}]);
|
||||
let mut rng = bleps::no_rng::NoRng;
|
||||
let mut srv = AttributeServer::new(&mut ble, &mut gatt_attributes, &mut rng);
|
||||
|
||||
info!("BLE running!");
|
||||
// TODO: Somehow need to recreate the attributeserver after disconnecting?
|
||||
async fn wdt_task(mut wdt: Wdt<esp_hal::peripherals::TIMG1<'static>>) {
|
||||
loop {
|
||||
let notification = match telemetry.try_receive() {
|
||||
Err(_) => None,
|
||||
//TODO: Should make Telemetry values serde-encodable
|
||||
Ok(Telemetry::Measurement(Measurement::IMU { accel, gyro })) => {
|
||||
let json_data = json!({
|
||||
"x": accel.x,
|
||||
"y": accel.y,
|
||||
"z": accel.z,
|
||||
"gx": gyro.x,
|
||||
"gy": gyro.y,
|
||||
"gz": gyro.z
|
||||
});
|
||||
let json_buf = serde_json::to_string(&json_data).unwrap();
|
||||
Some(json_buf)
|
||||
},
|
||||
Ok(Telemetry::Measurement(Measurement::GPS(Some(measurement)))) => {
|
||||
info!("gps telemetry");
|
||||
let json_data = json!({
|
||||
"lat": measurement.x,
|
||||
"lng": measurement.y
|
||||
});
|
||||
let json_buf = serde_json::to_string(&json_data).unwrap();
|
||||
Some(json_buf)
|
||||
},
|
||||
_ => None
|
||||
/*Ok(Measurement::Prediction(prediction)) => {
|
||||
let json_data = json!({
|
||||
"predict_lat": prediction.x,
|
||||
"predict_lng": prediction.y
|
||||
});
|
||||
let json_buf = serde_json::to_string(&json_data).unwrap();
|
||||
Some(json_buf)
|
||||
}*/
|
||||
};
|
||||
|
||||
match notification {
|
||||
None => {
|
||||
srv.do_work().unwrap();
|
||||
Timer::after_millis(5).await;
|
||||
},
|
||||
Some(serial_data) => {
|
||||
for chunk in serial_data.as_bytes().chunks(20) {
|
||||
srv.do_work_with_notification(Some(NotificationData::new(tx_handle, chunk))).unwrap();
|
||||
}
|
||||
srv.do_work_with_notification(Some(NotificationData::new(tx_handle, &b"\n"[..]))).unwrap();
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
let (mut wifi, _interfaces) = esp_wifi::wifi::new(&wifi_init, wifi_device)
|
||||
.expect("Failed to initialize WIFI controller"); }
|
||||
|
||||
loop {
|
||||
//let results = wifi.scan_n_async(16).await.unwrap();
|
||||
wifi.set_configuration(&esp_wifi::wifi::Configuration::Client(
|
||||
ClientConfiguration {
|
||||
ssid: "The Frequency".to_string(),
|
||||
auth_method: esp_wifi::wifi::AuthMethod::WPA2Personal,
|
||||
password: "thepasswordkenneth".to_string(),
|
||||
..Default::default()
|
||||
}
|
||||
)).unwrap();
|
||||
|
||||
if wifi.connect_async().await.is_ok() {
|
||||
info!("Connected to wifi!");
|
||||
while wifi.is_connected().unwrap() {
|
||||
Timer::after_secs(60).await;
|
||||
}
|
||||
info!("Disconnected.");
|
||||
}
|
||||
Timer::after_secs(30).await;
|
||||
}
|
||||
*/
|
||||
// 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
|
||||
wdt.feed();
|
||||
Timer::after_secs(5).await;
|
||||
}
|
||||
}
|
||||
195
src/display.rs
195
src/display.rs
@@ -1,120 +1,48 @@
|
||||
use figments::{hardware::{Brightness, Gamma, OutputAsync}, mappings::linear::LinearSpace, power::AsMilliwatts, prelude::*, smart_leds::{BrightnessWriter, BrightnessWriterAsync}};
|
||||
use core::{fmt::Debug, ops::IndexMut};
|
||||
//use std::io::Write;
|
||||
use figments::prelude::*;
|
||||
use core::fmt::Debug;
|
||||
use alloc::sync::Arc;
|
||||
|
||||
//use super::{Output};
|
||||
use figments::hardware::Output;
|
||||
use smart_leds::{SmartLedsWrite, SmartLedsWriteAsync};
|
||||
use figments_render::{
|
||||
gamma::{GammaCurve, WithGamma}, output::{Brightness, GammaCorrected, OutputAsync}, power::AsMilliwatts, smart_leds::PowerManagedWriterAsync
|
||||
};
|
||||
use smart_leds::SmartLedsWriteAsync;
|
||||
|
||||
pub trait LinearPixbuf: Pixbuf + Sample<'static, LinearSpace, Output = Self::Format> + IndexMut<usize, Output = Self::Format> + AsRef<[Self::Format]> {}
|
||||
impl<T> LinearPixbuf for T where T: Pixbuf + Sample<'static, LinearSpace, Output = Self::Format> + IndexMut<usize, Output = Self::Format> + AsRef<[Self::Format]> {}
|
||||
use crate::events::DisplayControls;
|
||||
|
||||
pub struct BikeOutput<T: SmartLedsWrite> {
|
||||
// FIXME: We need a way to specify a different buffer format from the 'native' hardware output, due to sometimes testing with GRB strips instead of RGB
|
||||
pub struct BikeOutput<T: SmartLedsWriteAsync> {
|
||||
pixbuf: [T::Color; 178],
|
||||
writer: BrightnessWriter<T>
|
||||
writer: PowerManagedWriterAsync<T>,
|
||||
controls: Arc<DisplayControls>
|
||||
}
|
||||
|
||||
impl<T: SmartLedsWrite> Debug for BikeOutput<T> {
|
||||
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
|
||||
f.debug_struct("BikeOutput").field("pixbuf", &core::any::type_name_of_val(&self.pixbuf)).field("writer", &core::any::type_name_of_val(&self.writer)).finish()
|
||||
impl<T:SmartLedsWriteAsync> GammaCorrected for BikeOutput<T> where T::Color: PixelBlend<Rgb<u8>> + PixelFormat + Debug + WithGamma, T::Error: Debug {
|
||||
fn set_gamma(&mut self, gamma: GammaCurve) {
|
||||
self.writer.controls().set_gamma(gamma);
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: SmartLedsWrite> BikeOutput<T> where T::Color: HardwarePixel + Gamma + Fract8Ops, T::Error: core::fmt::Debug {
|
||||
pub fn new(target: T, max_mw: u32) -> Self {
|
||||
impl<T: SmartLedsWriteAsync> BikeOutput<T> where T::Color: PixelBlend<Rgb<u8>> + PixelFormat + WithGamma + 'static, T::Error: core::fmt::Debug {
|
||||
pub fn new(target: T, max_mw: u32, controls: Arc<DisplayControls>) -> Self {
|
||||
Self {
|
||||
pixbuf: [Default::default(); 178],
|
||||
writer: BrightnessWriter::new(target, max_mw)
|
||||
writer: PowerManagedWriterAsync::new(target, max_mw),
|
||||
controls
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: SmartLedsWrite> Brightness for BikeOutput<T> where T::Color: HardwarePixel + Gamma + Fract8Ops, T::Error: core::fmt::Debug {
|
||||
fn set_brightness(&mut self, brightness: u8) {
|
||||
self.writer.set_brightness(brightness);
|
||||
}
|
||||
|
||||
fn set_on(&mut self, is_on: bool) {
|
||||
self.writer.set_on(is_on);
|
||||
}
|
||||
|
||||
fn set_gamma(&mut self, gamma: f32) {
|
||||
self.writer.set_gamma(gamma);
|
||||
pub fn blank(&mut self) {
|
||||
self.pixbuf = [Default::default(); 178];
|
||||
}
|
||||
}
|
||||
|
||||
impl<'a, T: SmartLedsWrite + 'a> Output<'a, BikeSpace> for BikeOutput<T> where T::Color: 'static + HardwarePixel + Gamma + Fract8Ops, [T::Color; 178]: AsMilliwatts + Gamma + Copy, T::Error: core::fmt::Debug {
|
||||
fn blank(&mut self) -> Result<(), Self::Error> {
|
||||
self.pixbuf.blank();
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn commit(&mut self) -> Result<(), Self::Error> {
|
||||
self.writer.write(&self.pixbuf)
|
||||
}
|
||||
|
||||
type HardwarePixel = T::Color;
|
||||
type Error = T::Error;
|
||||
}
|
||||
|
||||
impl<'a, T: SmartLedsWrite> Sample<'a, BikeSpace> for BikeOutput<T> where T::Color: HardwarePixel + Gamma + Fract8Ops, [T::Color; 178]: AsMilliwatts + 'static {
|
||||
type Output = T::Color;
|
||||
|
||||
fn sample(&mut self, rect: &Rectangle<BikeSpace>) -> impl Iterator<Item = (Coordinates<BikeSpace>, &'a mut Self::Output)> {
|
||||
let bufref = unsafe {
|
||||
&mut *(&mut self.pixbuf as *mut [T::Color; 178])
|
||||
};
|
||||
BikeIter {
|
||||
pixbuf: bufref,
|
||||
cur: rect.top_left,
|
||||
end: rect.bottom_right
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ASYNC
|
||||
|
||||
pub struct BikeOutputAsync<T: SmartLedsWriteAsync> {
|
||||
pixbuf: [T::Color; 178],
|
||||
writer: BrightnessWriterAsync<T>
|
||||
}
|
||||
|
||||
impl<T: SmartLedsWriteAsync> Debug for BikeOutputAsync<T> {
|
||||
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
|
||||
f.debug_struct("BikeOutput").field("pixbuf", &core::any::type_name_of_val(&self.pixbuf)).field("writer", &core::any::type_name_of_val(&self.writer)).finish()
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: SmartLedsWriteAsync> BikeOutputAsync<T> where T::Color: HardwarePixel + Gamma + Fract8Ops, T::Error: core::fmt::Debug {
|
||||
pub fn new(target: T, max_mw: u32) -> Self {
|
||||
Self {
|
||||
pixbuf: [Default::default(); 178],
|
||||
writer: BrightnessWriterAsync::new(target, max_mw)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl<T: SmartLedsWriteAsync> Brightness for BikeOutputAsync<T> where T::Color: HardwarePixel + Gamma + Fract8Ops, T::Error: core::fmt::Debug {
|
||||
fn set_brightness(&mut self, brightness: u8) {
|
||||
self.writer.set_brightness(brightness);
|
||||
}
|
||||
|
||||
fn set_on(&mut self, is_on: bool) {
|
||||
self.writer.set_on(is_on);
|
||||
}
|
||||
|
||||
fn set_gamma(&mut self, gamma: f32) {
|
||||
self.writer.set_gamma(gamma);
|
||||
}
|
||||
}
|
||||
|
||||
impl<'a, T: SmartLedsWriteAsync + 'a> OutputAsync<'a, BikeSpace> for BikeOutputAsync<T> where T::Color: 'static + HardwarePixel + Gamma + Fract8Ops, [T::Color; 178]: AsMilliwatts + Gamma + Copy, T::Error: core::fmt::Debug {
|
||||
async fn blank_async(&mut self) -> Result<(), Self::Error> {
|
||||
self.pixbuf.blank();
|
||||
Ok(())
|
||||
}
|
||||
|
||||
async fn commit_async(&mut self) -> Result<(), Self::Error> {
|
||||
impl<'a, T: SmartLedsWriteAsync + 'a> OutputAsync<'a, SegmentSpace> for BikeOutput<T> where T::Color: PixelBlend<Rgb<u8>> + Debug + 'static + AsMilliwatts + PixelFormat + WithGamma, [T::Color; 178]: AsMilliwatts + WithGamma + Copy, T::Error: core::fmt::Debug {
|
||||
async fn commit_async(&mut self) -> Result<(), T::Error> {
|
||||
let c = self.controls.as_ref();
|
||||
self.writer.controls().set_brightness(c.brightness());
|
||||
self.writer.controls().set_on(c.is_on());
|
||||
// TODO: We should grab the power used here and somehow feed it back into the telemetry layer, probably just via another atomic u32
|
||||
self.writer.write(&self.pixbuf).await
|
||||
}
|
||||
|
||||
@@ -122,14 +50,36 @@ impl<'a, T: SmartLedsWriteAsync + 'a> OutputAsync<'a, BikeSpace> for BikeOutputA
|
||||
type Error = T::Error;
|
||||
}
|
||||
|
||||
impl<'a, T: SmartLedsWriteAsync> Sample<'a, BikeSpace> for BikeOutputAsync<T> where T::Color: HardwarePixel + Gamma + Fract8Ops, [T::Color; 178]: AsMilliwatts + 'static {
|
||||
impl<'a, T: SmartLedsWriteAsync> Sample<'a, SegmentSpace> for BikeOutput<T> where T::Color: Debug + PixelFormat + 'a, [T::Color; 178]: Sample<'a, SegmentSpace, Output = T::Color> {
|
||||
type Output = T::Color;
|
||||
|
||||
fn sample(&mut self, rect: &Rectangle<BikeSpace>) -> impl Iterator<Item = (Coordinates<BikeSpace>, &'a mut Self::Output)> {
|
||||
fn sample(&mut self, rect: &Rectangle<SegmentSpace>) -> impl Iterator<Item = (Coordinates<SegmentSpace>, &'a mut T::Color)> {
|
||||
self.pixbuf.sample(rect)
|
||||
}
|
||||
}
|
||||
|
||||
const STRIP_MAP: [Segment; 6] = [
|
||||
Segment { start: 0, length: 28 },
|
||||
Segment { start: 28, length: 17 },
|
||||
Segment { start: 45, length: 14 },
|
||||
Segment { start: 59, length: 17 },
|
||||
Segment { start: 76, length: 14 },
|
||||
Segment { start: 90, length: 88 }
|
||||
];
|
||||
|
||||
pub struct Segment {
|
||||
start: usize,
|
||||
length: usize,
|
||||
}
|
||||
|
||||
impl<'a, T: PixelFormat> Sample<'a, SegmentSpace> for [T; 178] where T: 'static {
|
||||
type Output = T;
|
||||
|
||||
fn sample(&mut self, rect: &Rectangle<SegmentSpace>) -> impl Iterator<Item = (Coordinates<SegmentSpace>, &'a mut Self::Output)> {
|
||||
let bufref = unsafe {
|
||||
&mut *(&mut self.pixbuf as *mut [T::Color; 178])
|
||||
&mut *(self as *mut [T; 178])
|
||||
};
|
||||
BikeIter {
|
||||
SegmentIter {
|
||||
pixbuf: bufref,
|
||||
cur: rect.top_left,
|
||||
end: rect.bottom_right
|
||||
@@ -137,40 +87,27 @@ impl<'a, T: SmartLedsWriteAsync> Sample<'a, BikeSpace> for BikeOutputAsync<T> wh
|
||||
}
|
||||
}
|
||||
|
||||
struct Strip {
|
||||
start: usize,
|
||||
length: usize,
|
||||
}
|
||||
|
||||
const STRIP_MAP: [Strip; 6] = [
|
||||
Strip { start: 0, length: 28 },
|
||||
Strip { start: 28, length: 17 },
|
||||
Strip { start: 45, length: 14 },
|
||||
Strip { start: 59, length: 17 },
|
||||
Strip { start: 76, length: 14 },
|
||||
Strip { start: 90, length: 88 }
|
||||
];
|
||||
|
||||
#[derive(Clone, Copy, Default, Debug)]
|
||||
pub struct BikeSpace {}
|
||||
impl CoordinateSpace for BikeSpace {
|
||||
pub struct SegmentSpace {}
|
||||
impl CoordinateSpace for SegmentSpace {
|
||||
type Data = usize;
|
||||
}
|
||||
|
||||
pub struct BikeIter<'a, P: LinearPixbuf + Debug> {
|
||||
pixbuf: &'a mut P,
|
||||
cur: Coordinates<BikeSpace>,
|
||||
end: Coordinates<BikeSpace>
|
||||
pub struct SegmentIter<'a, Format: PixelFormat> {
|
||||
pixbuf: &'a mut [Format; 178],
|
||||
cur: Coordinates<SegmentSpace>,
|
||||
end: Coordinates<SegmentSpace>,
|
||||
}
|
||||
|
||||
impl<'a, P: LinearPixbuf + Debug> Debug for BikeIter<'a, P> {
|
||||
impl<'a, Format: PixelFormat + Debug> Debug for SegmentIter<'a, Format> {
|
||||
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
|
||||
f.debug_struct("BikeIter").field("cur", &self.cur).field("end", &self.end).finish()
|
||||
}
|
||||
}
|
||||
|
||||
impl<'a, P: LinearPixbuf + Debug> Iterator for BikeIter<'a, P> {
|
||||
type Item = (Coordinates<BikeSpace>, &'a mut P::Format);
|
||||
impl<'a, Format: PixelFormat> Iterator for SegmentIter<'a, Format> {
|
||||
type Item = (Coordinates<SegmentSpace>, &'a mut Format);
|
||||
|
||||
fn next(&mut self) -> Option<Self::Item> {
|
||||
if self.cur.y > self.end.y || self.cur.y >= STRIP_MAP.len() {
|
||||
@@ -190,9 +127,15 @@ impl<'a, P: LinearPixbuf + Debug> Iterator for BikeIter<'a, P> {
|
||||
}
|
||||
|
||||
let bufref = unsafe {
|
||||
&mut *(self.pixbuf as *mut P)
|
||||
&mut *(self.pixbuf as *mut [Format; 178])
|
||||
};
|
||||
|
||||
Some((pixel_coords, &mut bufref[offset]))
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Default, Debug)]
|
||||
pub struct Uniforms {
|
||||
pub frame: usize,
|
||||
pub primary_color: Hsv
|
||||
}
|
||||
264
src/ego/engine.rs
Normal file
264
src/ego/engine.rs
Normal file
@@ -0,0 +1,264 @@
|
||||
use embassy_sync::channel::DynamicSender;
|
||||
use embassy_time::{Duration, Instant};
|
||||
use nalgebra::{Rotation3, Vector2, Vector3};
|
||||
use log::*;
|
||||
//use micromath::F32Ext;
|
||||
use nalgebra::{ComplexField, RealField};
|
||||
|
||||
use core::fmt::Debug;
|
||||
|
||||
use crate::{ego::{heading::HeadingEstimator, kalman::Ekf2D, orientation::OrientationEstimator}, events::{Notification, Prediction}, Breaker, CircularBuffer, idle::IdleClock};
|
||||
|
||||
#[derive(PartialEq, Debug, Default, Clone, Copy)]
|
||||
pub enum MotionState {
|
||||
#[default]
|
||||
Stationary,
|
||||
Steady,
|
||||
Accelerating,
|
||||
Decelerating
|
||||
}
|
||||
|
||||
pub struct BikeStates {
|
||||
/// Prediction models
|
||||
orientation: OrientationEstimator,
|
||||
heading: HeadingEstimator,
|
||||
speedo: CircularBuffer<f32, 300>,
|
||||
kf: Ekf2D,
|
||||
|
||||
last_stamp: Instant,
|
||||
last_fix: Vector2<f64>, // The most recent GPS value
|
||||
reference_fix: Option<Vector2<f64>>, // The first GPS value, which is used to make sense out of the EKF output which is in meters offset from this point
|
||||
|
||||
// State switches
|
||||
is_calibrated: Breaker<bool>,
|
||||
motion_state: Breaker<MotionState>,
|
||||
// FIXME: pub
|
||||
pub has_gps_fix: Breaker<bool>,
|
||||
predicted_velocity: Breaker<f32>,
|
||||
reported_velocity: Breaker<f32>,
|
||||
predicted_location: Breaker<Vector2<f64>>,
|
||||
wake_requested: Breaker<bool>,
|
||||
steady_timer: IdleClock
|
||||
}
|
||||
|
||||
impl Debug for BikeStates {
|
||||
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
|
||||
f.debug_struct("BikeStates")
|
||||
.field("has_down", &self.orientation.has_down())
|
||||
.field("has_orientation", &self.orientation.is_ready())
|
||||
.field("heading", &self.heading.heading())
|
||||
.field("motion_state", &self.motion_state)
|
||||
.field("predicted_location", &self.predicted_location)
|
||||
.field("predicted_velocity", &self.predicted_velocity)
|
||||
.field("wake_requested", &self.wake_requested)
|
||||
.finish()
|
||||
}
|
||||
}
|
||||
|
||||
impl BikeStates {
|
||||
pub fn insert_gps(&mut self, gps_pos: Vector2<f64>) {
|
||||
match self.reference_fix {
|
||||
None => {
|
||||
self.reference_fix = Some(gps_pos);
|
||||
self.last_fix = gps_pos;
|
||||
self.has_gps_fix.set(true);
|
||||
},
|
||||
Some(coords) => {
|
||||
if self.last_fix != coords {
|
||||
let gps_heading = self.last_fix.angle(&coords);
|
||||
self.heading.correct(gps_heading as f32, 0.9);
|
||||
}
|
||||
|
||||
let delta = gps_to_local_meters_haversine(&coords, &gps_pos);
|
||||
|
||||
self.kf.update_gps(delta.x, delta.y);
|
||||
|
||||
self.last_fix = coords;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn insert_imu(&mut self, accel: Vector3<f32>, gyro: Vector3<f32>) {
|
||||
self.orientation.insert(accel);
|
||||
|
||||
if self.orientation.has_down() {
|
||||
let body_accel = self.orientation.apply(accel);
|
||||
let body_gyro = self.orientation.apply(gyro);
|
||||
|
||||
let cur_stamp = Instant::now();
|
||||
let dt = (cur_stamp - self.last_stamp).as_millis() as f32 / 1000.0;
|
||||
self.last_stamp = cur_stamp;
|
||||
|
||||
self.heading.update(body_gyro.z, dt);
|
||||
let heading_rotation = Rotation3::from_axis_angle(&Vector3::z_axis(), self.heading.heading());
|
||||
|
||||
let enu_rotated = heading_rotation * body_accel;
|
||||
if accel.xy().magnitude() >= 0.8 {
|
||||
self.kf.predict(enu_rotated.xy(), body_gyro.z, dt);
|
||||
} else {
|
||||
// Otherwise, we are standing stationary and should insert accel=0 data into the model
|
||||
self.kf.update_zupt();
|
||||
}
|
||||
|
||||
self.is_calibrated.set(true);
|
||||
}
|
||||
}
|
||||
|
||||
pub async fn commit(&mut self, predictions: &DynamicSender<'static, Prediction>, notifications: &DynamicSender<'static, Notification>) {
|
||||
if let Some(true) = self.is_calibrated.read_tripped() {
|
||||
notifications.send(Notification::SensorOnline(crate::events::SensorSource::IMU)).await
|
||||
}
|
||||
|
||||
match self.has_gps_fix.read_tripped() {
|
||||
None => (),
|
||||
Some(true) => notifications.send(Notification::SensorOnline(crate::events::SensorSource::GPS)).await,
|
||||
Some(false) => notifications.send(Notification::SensorOffline(crate::events::SensorSource::GPS)).await,
|
||||
}
|
||||
|
||||
let est = self.kf.x;
|
||||
|
||||
let position = self.reference_fix.as_ref().map(|start_coords| local_to_gps(*start_coords, Vector2::new(est.x as f64, est.y as f64)));
|
||||
let velocity = Vector2::new(est[2], est[3]);
|
||||
|
||||
// If we have a new location, update the predictions
|
||||
if let Some(pos) = position {
|
||||
self.predicted_location.set(pos);
|
||||
if let Some(pos) = self.predicted_location.read_tripped() {
|
||||
predictions.send(Prediction::Location(pos)).await;
|
||||
}
|
||||
}
|
||||
|
||||
// If we have a new velocity, update the acceleration status
|
||||
self.predicted_velocity.set(velocity.norm());
|
||||
if let Some(v) = self.predicted_velocity.read_tripped() {
|
||||
self.wake_requested.set(true);
|
||||
if self.wake_requested.read_tripped().is_some() {
|
||||
predictions.send(Prediction::WakeRequested).await;
|
||||
}
|
||||
self.speedo.insert(v);
|
||||
if self.speedo.is_filled() {
|
||||
let threshold = 1.0;
|
||||
let trend = self.speedo.data().windows(2).map(|n| {
|
||||
n[1] - n[0]
|
||||
}).sum::<f32>();
|
||||
let mean = self.speedo.mean();
|
||||
|
||||
// Reported velocity is kept only to the first decimal
|
||||
self.reported_velocity.set((mean *10.0).round() / 10.0);
|
||||
if let Some(v) = self.reported_velocity.read_tripped() {
|
||||
predictions.send(Prediction::Velocity(v)).await;
|
||||
}
|
||||
|
||||
// If the total slope is more upwards than not, we are accelerating.
|
||||
if trend >= threshold {
|
||||
self.motion_state.set(MotionState::Accelerating);
|
||||
self.steady_timer.wake();
|
||||
} else if trend <= -threshold {
|
||||
self.steady_timer.wake();
|
||||
self.motion_state.set(MotionState::Decelerating);
|
||||
} else if self.steady_timer.check() && mean > 1.0 {
|
||||
// If we haven't changed our acceleration for a while, and we still have speed, we are moving at a steady pace
|
||||
self.motion_state.set(MotionState::Steady);
|
||||
} else if v <= 1.0 && mean <= 1.0 {
|
||||
// If the average and instantaneous speed is rather low, we are probably stationary!
|
||||
self.motion_state.set(MotionState::Stationary);
|
||||
}
|
||||
|
||||
// And if the motion status has changed, send it out
|
||||
if let Some(state) = self.motion_state.read_tripped() {
|
||||
debug!("state={state:?} trend={trend} mean={mean} v={v}");
|
||||
predictions.send(Prediction::Motion(state)).await
|
||||
}
|
||||
}
|
||||
} else {
|
||||
self.wake_requested.set(false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Default for BikeStates {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
motion_state: Default::default(),
|
||||
orientation: Default::default(),
|
||||
last_stamp: Instant::now(),
|
||||
speedo: Default::default(),
|
||||
heading: Default::default(),
|
||||
is_calibrated: Default::default(),
|
||||
kf: Default::default(),
|
||||
steady_timer: IdleClock::new(Duration::from_secs(3)),
|
||||
last_fix: Default::default(),
|
||||
reference_fix: Default::default(),
|
||||
has_gps_fix: Default::default(),
|
||||
predicted_location: Default::default(),
|
||||
predicted_velocity: Default::default(),
|
||||
reported_velocity: Default::default(),
|
||||
wake_requested: Default::default()
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Compute displacement vector (east, north) in meters between two GPS coordinates
|
||||
/// Input: Vector2::new(lat_deg, lon_deg)
|
||||
pub fn gps_to_local_meters_haversine(a: &Vector2<f64>, b: &Vector2<f64>) -> Vector2<f32> {
|
||||
let r = 6_371_000.0; // Earth radius in meters
|
||||
|
||||
// Convert to radians
|
||||
let lat1 = a.x.to_radians();
|
||||
let lon1 = a.y.to_radians();
|
||||
let lat2 = b.x.to_radians();
|
||||
let lon2 = b.y.to_radians();
|
||||
|
||||
// Differences
|
||||
let dlat = lat2 - lat1;
|
||||
let dlon = lon2 - lon1;
|
||||
|
||||
// Haversine formula for great-circle distance
|
||||
let hav = (dlat / 2.0).sin().powi(2)
|
||||
+ lat1.cos() * lat2.cos() * (dlon / 2.0).sin().powi(2);
|
||||
let c = 2.0 * hav.sqrt().atan2((1.0 - hav).sqrt());
|
||||
let distance = r * c;
|
||||
|
||||
// Bearing from point A to B
|
||||
let y = dlon.sin() * lat2.cos();
|
||||
let x = lat1.cos() * lat2.sin() - lat1.sin() * lat2.cos() * dlon.cos();
|
||||
let bearing = y.atan2(x); // radians, from north, clockwise
|
||||
|
||||
// Convert to local east/north vector
|
||||
let dx = distance * bearing.sin(); // east
|
||||
let dy = distance * bearing.cos(); // north
|
||||
|
||||
Vector2::new(dx as f32, dy as f32)
|
||||
}
|
||||
|
||||
/// Convert a local displacement (east, north) in meters back to GPS coordinates.
|
||||
/// Input:
|
||||
/// ref_gps = Vector2::new(lat_deg, lon_deg)
|
||||
/// disp = Vector2::new(dx_east_m, dy_north_m)
|
||||
/// Output:
|
||||
/// Vector2::new(lat_deg, lon_deg)
|
||||
pub fn local_to_gps(ref_gps: Vector2<f64>, disp: Vector2<f64>) -> Vector2<f64> {
|
||||
let r = 6_371_000.0; // Earth radius in meters
|
||||
|
||||
let lat0 = ref_gps.x.to_radians();
|
||||
let lon0 = ref_gps.y.to_radians();
|
||||
|
||||
// Displacement in meters
|
||||
let dx = disp.x; // east
|
||||
let dy = disp.y; // north
|
||||
|
||||
// Distance and bearing
|
||||
let distance = (dx.powi(2) + dy.powi(2)).sqrt();
|
||||
let bearing = dx.atan2(dy); // atan2(east, north)
|
||||
|
||||
// Destination point on sphere
|
||||
let lat = (lat0.sin() * (distance / r).cos()
|
||||
+ lat0.cos() * (distance / r).sin() * bearing.cos())
|
||||
.asin();
|
||||
|
||||
let lon = lon0
|
||||
+ (bearing.sin() * (distance / r).sin() * lat0.cos())
|
||||
.atan2((distance / r).cos() - lat0.sin() * lat.sin());
|
||||
|
||||
Vector2::new(lat.to_degrees(), lon.to_degrees())
|
||||
}
|
||||
@@ -9,16 +9,12 @@ fn wrap_angle(angle: f32) -> f32 {
|
||||
}
|
||||
|
||||
/// Heading estimator using gyro yaw rate
|
||||
#[derive(Debug, Clone, Copy)]
|
||||
#[derive(Debug, Clone, Copy, Default)]
|
||||
pub struct HeadingEstimator {
|
||||
heading: f32, // radians
|
||||
}
|
||||
|
||||
impl HeadingEstimator {
|
||||
pub fn new(initial_heading: f32) -> Self {
|
||||
Self { heading: wrap_angle(initial_heading) }
|
||||
}
|
||||
|
||||
/// Integrate yaw rate (rad/s) over dt (s)
|
||||
pub fn update(&mut self, yaw_rate: f32, dt: f32) {
|
||||
self.heading = wrap_angle(self.heading + yaw_rate * dt);
|
||||
|
||||
@@ -1,87 +1,189 @@
|
||||
#![allow(non_snake_case)] // TODO, because most of this code came from chatgpt and I'm still not sure if it actually works or is completely wrong
|
||||
use nalgebra::{SMatrix, SVector};
|
||||
#![allow(non_snake_case)]
|
||||
use nalgebra::{Matrix2, Matrix5, Vector2, Vector5, Matrix2x5};
|
||||
use core::f32::consts::PI;
|
||||
use nalgebra::ComplexField;
|
||||
|
||||
/// State: [px, py, vx, vy]
|
||||
pub type State = SVector<f32, 6>;
|
||||
pub type Control = SVector<f32, 3>; // dx, dx, yaw
|
||||
pub type GpsMeasurement = SVector<f32, 2>; // gps x, gps y
|
||||
|
||||
pub struct Kalman {
|
||||
x: State, // state estimate
|
||||
P: SMatrix<f32, 6, 6>, // covariance
|
||||
F: SMatrix<f32, 6, 6>, // state transition
|
||||
B: SMatrix<f32, 6, 3>, // control matrix
|
||||
H: SMatrix<f32, 2, 6>, // measurement matrix
|
||||
Q: SMatrix<f32, 6, 6>, // process noise
|
||||
R: SMatrix<f32, 2, 2>, // measurement noise
|
||||
/// 5-state EKF for planar vehicle: [px, py, vx, vy, theta]
|
||||
pub struct Ekf2D {
|
||||
pub x: Vector5<f32>, // state
|
||||
pub P: Matrix5<f32>, // covariance
|
||||
pub Q: Matrix5<f32>, // process noise
|
||||
pub R_gps: Matrix2<f32>, // GPS measurement noise (px,py)
|
||||
}
|
||||
|
||||
impl Kalman {
|
||||
pub fn new(dt: f32) -> Self {
|
||||
let mut kf = Kalman {
|
||||
x: State::zeros(),
|
||||
P: SMatrix::identity(),
|
||||
F: SMatrix::identity(),
|
||||
B: SMatrix::zeros(),
|
||||
H: SMatrix::zeros(),
|
||||
Q: SMatrix::identity() * 0.1,
|
||||
R: SMatrix::identity() * 3.0, // GPS noise
|
||||
};
|
||||
|
||||
// F (state transition)
|
||||
kf.F[(0,2)] = dt;
|
||||
kf.F[(1,3)] = dt;
|
||||
kf.F[(4,5)] = dt;
|
||||
|
||||
// B (control matrix)
|
||||
kf.B[(0,0)] = 0.5 * dt * dt;
|
||||
kf.B[(1,1)] = 0.5 * dt * dt;
|
||||
kf.B[(2,0)] = dt;
|
||||
kf.B[(3,1)] = dt;
|
||||
kf.B[(5,2)] = dt;
|
||||
|
||||
// H (GPS: positions only)
|
||||
kf.H[(0,0)] = 1.0;
|
||||
kf.H[(1,1)] = 1.0;
|
||||
|
||||
kf
|
||||
impl Ekf2D {
|
||||
/// Small helper: rotation matrix R(theta)
|
||||
fn rotation(theta: f32) -> Matrix2<f32> {
|
||||
let c = theta.cos();
|
||||
let s = theta.sin();
|
||||
Matrix2::new(c, -s, s, c)
|
||||
}
|
||||
|
||||
pub fn predict(&mut self, u: &Control) {
|
||||
self.x = self.F * self.x + self.B * u;
|
||||
self.P = self.F * self.P * self.F.transpose() + self.Q;
|
||||
/// Derivative dR/dtheta
|
||||
fn rotation_derivative(theta: f32) -> Matrix2<f32> {
|
||||
let c = theta.cos();
|
||||
let s = theta.sin();
|
||||
// derivative of [[c, -s],[s, c]] = [[-s, -c],[c, -s]]
|
||||
Matrix2::new(-s, -c, c, -s)
|
||||
}
|
||||
|
||||
pub fn update(&mut self, z: &GpsMeasurement) {
|
||||
let y = z - self.H * self.x; // innovation
|
||||
let S = self.H * self.P * self.H.transpose() + self.R;
|
||||
if let Some(s_inv) = S.try_inverse() {
|
||||
let K = self.P * self.H.transpose() * s_inv;
|
||||
self.x += K * y;
|
||||
self.P = (SMatrix::<f32,6,6>::identity() - K * self.H) * self.P;
|
||||
}
|
||||
/// Predict step using IMU readings in body frame:
|
||||
/// - accel_b: Vector2 (ax, ay) in body frame (m/s^2)
|
||||
/// - gyro_z: yaw rate (rad/s)
|
||||
/// - dt: seconds
|
||||
pub fn predict(&mut self, accel_b: Vector2<f32>, gyro_z: f32, dt: f32) {
|
||||
// Unpack state
|
||||
let px = self.x[0];
|
||||
let py = self.x[1];
|
||||
let vx = self.x[2];
|
||||
let vy = self.x[3];
|
||||
let theta = self.x[4];
|
||||
|
||||
// rotation and rotated acceleration
|
||||
let R = Self::rotation(theta);
|
||||
let a_w = R * accel_b; // world-frame acceleration
|
||||
|
||||
// Nonlinear state propagation (discrete)
|
||||
let p_new = Vector2::new(px, py) + Vector2::new(vx, vy) * dt + 0.5 * a_w * dt * dt;
|
||||
let v_new = Vector2::new(vx, vy) + a_w * dt;
|
||||
let theta_new = theta + gyro_z * dt;
|
||||
|
||||
// Updated state
|
||||
self.x[0] = p_new[0];
|
||||
self.x[1] = p_new[1];
|
||||
self.x[2] = v_new[0];
|
||||
self.x[3] = v_new[1];
|
||||
self.x[4] = theta_new;
|
||||
|
||||
// Build Jacobian F = df/dx (5x5)
|
||||
// Using ordering [px, py, vx, vy, theta]
|
||||
let mut F = Matrix5::identity();
|
||||
|
||||
// ∂p'/∂v = dt * I2
|
||||
F[(0,2)] = dt; F[(1,3)] = dt;
|
||||
|
||||
// ∂p'/∂theta = 0.5 * dt^2 * dR/dtheta * a_b (2x1 into columns)
|
||||
let dR_dth = Self::rotation_derivative(theta);
|
||||
let dp_dth = 0.5 * dt * dt * (dR_dth * accel_b);
|
||||
F[(0,4)] = dp_dth[0];
|
||||
F[(1,4)] = dp_dth[1];
|
||||
|
||||
// ∂v'/∂theta = dt * dR/dtheta * a_b
|
||||
let dv_dth = dt * (dR_dth * accel_b);
|
||||
F[(2,4)] = dv_dth[0];
|
||||
F[(3,4)] = dv_dth[1];
|
||||
|
||||
// ∂v'/∂v = I2 (already identity on diagonal)
|
||||
// ∂p'/∂p = I2 (already identity)
|
||||
|
||||
// ∂theta'/∂theta = 1 (identity)
|
||||
|
||||
// Covariance propagation: P = F P Fᵀ + Q (we might scale Q by dt if desired)
|
||||
self.P = F * self.P * F.transpose() + self.Q;
|
||||
}
|
||||
|
||||
// Used to indicate stationary behavior and therefore velocities should be zero
|
||||
/// GPS update: provide measured position (px,py) in world frame
|
||||
pub fn update_gps(&mut self, z_px: f32, z_py: f32) {
|
||||
// measurement matrix H (2x5) picks px,py
|
||||
let mut H = Matrix2x5::zeros();
|
||||
H[(0,0)] = 1.0;
|
||||
H[(1,1)] = 1.0;
|
||||
|
||||
// y = z - H x
|
||||
let z = Vector2::new(z_px, z_py);
|
||||
let hx = Vector2::new(self.x[0], self.x[1]);
|
||||
let y = z - hx;
|
||||
|
||||
// S = H P Hᵀ + R
|
||||
let S = H * self.P * H.transpose() + self.R_gps;
|
||||
|
||||
// K = P Hᵀ S⁻¹ (5x2)
|
||||
let K = self.P * H.transpose() * S.try_inverse().unwrap_or_else(|| {
|
||||
// fallback to small-regularized inverse if S is singular
|
||||
let mut Sreg = S;
|
||||
Sreg[(0,0)] += 1e-6;
|
||||
Sreg[(1,1)] += 1e-6;
|
||||
Sreg.try_inverse().unwrap()
|
||||
});
|
||||
|
||||
// x = x + K y
|
||||
let dx = K * y;
|
||||
self.x += dx;
|
||||
|
||||
// P = (I - K H) P
|
||||
let I5 = Matrix5::identity();
|
||||
let KH = K * H;
|
||||
self.P = (I5 - KH) * self.P;
|
||||
}
|
||||
|
||||
/// Optional: set a more informed initial pose when first GPS arrives.
|
||||
/// This can reduce large initial uncertainty in position and optionally set heading.
|
||||
pub fn initialize_with_gps(&mut self, px: f32, py: f32, pos_variance: f32) {
|
||||
self.x[0] = px;
|
||||
self.x[1] = py;
|
||||
self.P[(0,0)] = pos_variance;
|
||||
self.P[(1,1)] = pos_variance;
|
||||
// leave velocity and heading as-is (or set to small uncertainty if you know them)
|
||||
}
|
||||
|
||||
/// Zero-velocity update (ZUPT). Call this when you know the vehicle is stationary.
|
||||
pub fn update_zupt(&mut self) {
|
||||
let z = SVector::<f32, 2>::zeros(); // velocity = 0
|
||||
let mut H = SMatrix::<f32, 2, 6>::zeros();
|
||||
H[(0,2)] = 1.0; // vx
|
||||
H[(1,3)] = 1.0; // vy
|
||||
// Measurement matrix H (2x5): extracts vx, vy
|
||||
let mut H = Matrix2x5::zeros();
|
||||
H[(0,2)] = 1.0;
|
||||
H[(1,3)] = 1.0;
|
||||
|
||||
let R = SMatrix::<f32, 2, 2>::identity() * 0.01; // very confident
|
||||
// Expected measurement: current velocity estimate
|
||||
let hx = Vector2::new(self.x[2], self.x[3]);
|
||||
|
||||
let y = z - H * self.x;
|
||||
// "Observed" velocity is zero
|
||||
let z = Vector2::zeros();
|
||||
let y = z - hx;
|
||||
|
||||
// Choose a small measurement noise R (you can tune this)
|
||||
let mut R = Matrix2::zeros();
|
||||
R[(0,0)] = 0.01;
|
||||
R[(1,1)] = 0.01;
|
||||
|
||||
// S = H P Hᵀ + R
|
||||
let S = H * self.P * H.transpose() + R;
|
||||
|
||||
if let Some(s_inv) = S.try_inverse() {
|
||||
let k = self.P * H.transpose() * s_inv;
|
||||
self.x += k * y;
|
||||
self.P = (SMatrix::<f32,6,6>::identity() - k * H) * self.P;
|
||||
}
|
||||
}
|
||||
// Kalman gain
|
||||
let K = self.P * H.transpose() * S.try_inverse().unwrap();
|
||||
|
||||
pub fn state(&self) -> &State {
|
||||
&self.x
|
||||
// State correction
|
||||
self.x += K * y;
|
||||
|
||||
// Covariance correction
|
||||
let I5 = Matrix5::identity();
|
||||
self.P = (I5 - K * H) * self.P;
|
||||
}
|
||||
}
|
||||
|
||||
impl Default for Ekf2D {
|
||||
fn default() -> Self {
|
||||
let x = Vector5::zeros();
|
||||
// default: unknown position/vel/heading. We'll set large covariance
|
||||
let mut P = Matrix5::zeros();
|
||||
P[(0,0)] = 1e6; // px
|
||||
P[(1,1)] = 1e6; // py
|
||||
P[(2,2)] = 1e3; // vx
|
||||
P[(3,3)] = 1e3; // vy
|
||||
P[(4,4)] = (2.0*PI).powi(2); // theta huge uncertainty
|
||||
|
||||
// Process noise - tune to your sensors:
|
||||
// Represents uncertainty in the dynamics (accel noise affects v and p, gyro noise affects theta)
|
||||
let mut Q = Matrix5::zeros();
|
||||
Q[(0,0)] = 0.1;
|
||||
Q[(1,1)] = 0.1;
|
||||
Q[(2,2)] = 1.0;
|
||||
Q[(3,3)] = 1.0;
|
||||
Q[(4,4)] = 0.5; // heading diffusion
|
||||
|
||||
let mut R_gps = Matrix2::zeros();
|
||||
R_gps[(0,0)] = 5.0; // variance in meters^2 (tune to your GPS)
|
||||
R_gps[(1,1)] = 5.0;
|
||||
|
||||
Self { x, P, Q, R_gps }
|
||||
}
|
||||
}
|
||||
@@ -1,4 +1,5 @@
|
||||
pub mod alignment;
|
||||
pub mod tilt;
|
||||
//pub mod tilt;
|
||||
pub mod heading;
|
||||
pub mod kalman;
|
||||
pub mod kalman;
|
||||
pub mod orientation;
|
||||
pub mod engine;
|
||||
83
src/ego/orientation.rs
Normal file
83
src/ego/orientation.rs
Normal file
@@ -0,0 +1,83 @@
|
||||
use nalgebra::{Vector3, Rotation3, Matrix3, Unit};
|
||||
use log::*;
|
||||
|
||||
use crate::CircularBuffer;
|
||||
|
||||
#[derive(Default, Debug, Clone, Copy)]
|
||||
pub struct OrientationEstimator {
|
||||
sensor_bias: Vector3<f32>, // Bias of the sensors at rest, eg, this will include the pull of gravity and needs applied prior to rotating
|
||||
down: Option<Unit<Vector3<f32>>>,
|
||||
forward: Option<Unit<Vector3<f32>>>,
|
||||
rotation_sb: Rotation3<f32>, // sensor -> body
|
||||
accel_history: CircularBuffer<Vector3<f32>, 200>
|
||||
}
|
||||
|
||||
impl OrientationEstimator {
|
||||
pub fn is_ready(&self) -> bool {
|
||||
self.forward.is_some()
|
||||
}
|
||||
|
||||
pub fn has_down(&self) -> bool {
|
||||
self.down.is_some()
|
||||
}
|
||||
|
||||
pub fn bias(&self) -> Vector3<f32> {
|
||||
self.sensor_bias
|
||||
}
|
||||
|
||||
pub fn insert(&mut self, accel: Vector3<f32>) {
|
||||
self.accel_history.insert(accel);
|
||||
match self.down {
|
||||
None => {
|
||||
if self.accel_history.is_filled() {
|
||||
// Estimate down from stationary accel
|
||||
let avg = self.accel_history.data().iter().sum::<Vector3<f32>>()
|
||||
/ (self.accel_history.data().len() as f32);
|
||||
if avg.norm() >= 9.764 && avg.norm() <= 9.834 {
|
||||
self.down = Some(Unit::new_normalize(-avg));
|
||||
self.sensor_bias = avg;
|
||||
info!("Found down={:?} bias={:?}", self.down, self.sensor_bias);
|
||||
}
|
||||
}
|
||||
},
|
||||
Some(down) => {
|
||||
// Project motion into horizontal plane
|
||||
let avg = self.accel_history.data().iter().sum::<Vector3<f32>>();
|
||||
let horiz = avg - avg.dot(&down) * down.into_inner();
|
||||
|
||||
if horiz.norm() < 5.0 {
|
||||
return; // not enough motion
|
||||
}
|
||||
|
||||
let mut forward = Unit::new_normalize(horiz);
|
||||
|
||||
// If we already had a forward, allow reversal check
|
||||
if let Some(prev) = &self.forward {
|
||||
// Positive dot means similar direction, negative means opposite direction
|
||||
if forward.dot(prev) < -0.7 {
|
||||
// Strong opposite -> flip
|
||||
forward = Unit::new_unchecked(-forward.into_inner());
|
||||
warn!("Forwards is flipped!!! prev={prev:?} forward={forward:?}");
|
||||
}
|
||||
} else {
|
||||
info!("Found forwards: {forward:?}")
|
||||
}
|
||||
|
||||
self.forward = Some(forward);
|
||||
|
||||
// Build body axes
|
||||
let x = forward.into_inner();
|
||||
let z = down.into_inner();
|
||||
let y = z.cross(&x).normalize();
|
||||
let x = y.cross(&z).normalize(); // re-orthogonalize
|
||||
|
||||
let mat = Matrix3::from_columns(&[x, y, z]);
|
||||
self.rotation_sb = Rotation3::from_matrix_unchecked(mat);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub fn apply(&self, v_sensor: Vector3<f32>) -> Vector3<f32> {
|
||||
self.rotation_sb * (v_sensor - self.sensor_bias)
|
||||
}
|
||||
}
|
||||
115
src/events.rs
115
src/events.rs
@@ -2,18 +2,19 @@
|
||||
use core::sync::atomic::{AtomicBool, AtomicU8};
|
||||
|
||||
use embassy_sync::{blocking_mutex::raw::{CriticalSectionRawMutex, NoopRawMutex}, channel::Channel, signal::Signal};
|
||||
use figments::hardware::Brightness;
|
||||
use embassy_time::Duration;
|
||||
use nalgebra::{Vector2, Vector3};
|
||||
use alloc::sync::Arc;
|
||||
|
||||
use crate::ego::engine::MotionState;
|
||||
|
||||
#[derive(Clone, Copy, Default, Debug)]
|
||||
pub enum Scene {
|
||||
#[default]
|
||||
Startup, // Default state when booting up
|
||||
ParkedIdle, // Default state when waking up from long term (overnight, eg) parking, or entered when accelerometers and GPS both show zero motion for ~30 seconds
|
||||
StoplightIdle, // Entered when GPS speed is zero after decelerating
|
||||
Accelerating, // GPS speed is increasing
|
||||
Decelerating, // GPS speed is decreasing
|
||||
ParkedLongTerm, // GPS has not changed location in the last ~5 minutes
|
||||
Ready, // Default state when booting up, but not when coming out of sleep
|
||||
Accelerating,
|
||||
Decelerating,
|
||||
Idle, // Default state when waking up from sleep, or entered when accelerometers and GPS both show zero motion for ~30 seconds
|
||||
}
|
||||
|
||||
#[derive(Clone, Copy, Debug)]
|
||||
@@ -22,52 +23,89 @@ pub enum Measurement {
|
||||
GPS(Option<Vector2<f64>>),
|
||||
// Accelerometer values in body frame where x=forwards
|
||||
IMU { accel: Vector3<f32>, gyro: Vector3<f32> },
|
||||
|
||||
// Hardware status updates
|
||||
SensorOnline(SensorSource),
|
||||
SensorOffline(SensorSource),
|
||||
|
||||
// Simulation metadata updates
|
||||
SimulationProgress(SensorSource, Duration, f32)
|
||||
}
|
||||
|
||||
#[derive(Clone, Copy, Debug)]
|
||||
pub enum Prediction {
|
||||
Location(Vector2<f32>),
|
||||
Velocity(Vector2<f32>)
|
||||
}
|
||||
|
||||
#[derive(Clone, Copy, Debug)]
|
||||
pub enum Telemetry {
|
||||
Prediction(Prediction),
|
||||
Measurement(Measurement),
|
||||
Notification(Notification)
|
||||
Motion(MotionState),
|
||||
Velocity(f32),
|
||||
Location(Vector2<f64>),
|
||||
WakeRequested
|
||||
}
|
||||
|
||||
#[derive(Clone, Copy, Debug)]
|
||||
pub enum Notification {
|
||||
// The prediction engine has decided the UI should switch to another scene
|
||||
SceneChange(Scene),
|
||||
WifiConnected,
|
||||
WifiDisconnected,
|
||||
GPSLost,
|
||||
GPSAcquired,
|
||||
CalibrationComplete,
|
||||
|
||||
// States of external connections to the world
|
||||
SensorOnline(SensorSource),
|
||||
SensorOffline(SensorSource),
|
||||
|
||||
// TODO: Should be emitted by the prediction engine after it doesn't get any sensor data for some time. Perhaps the threads are somehow deadlocked and a reboot is needed if it doesn't recover
|
||||
SensorsOffline,
|
||||
|
||||
// The prediction engine has decided that the system should be woken up and begin running again
|
||||
WakeUp,
|
||||
// The prediction engine has decided that the system is inactive enough and it should go to low-power sleep
|
||||
Sleep,
|
||||
|
||||
// FIXME: Sor safety purposes, we probably want these two events to be combined and act atomic; if the safety lights are ever on, they should both be on.
|
||||
SetHeadlight(bool),
|
||||
SetBrakelight(bool)
|
||||
SetBrakelight(bool),
|
||||
|
||||
// TODO: BPM detection via bluetooth
|
||||
Beat
|
||||
}
|
||||
|
||||
#[derive(Clone, Copy, Debug)]
|
||||
pub enum SensorSource {
|
||||
IMU,
|
||||
GPS
|
||||
}
|
||||
|
||||
// TODO: Make this clone() able, so multiple threads can point to the same underlying atomics for the hardware controls
|
||||
// FIXME: We only ever hold this behind an Arc and therefore end up storing a Signal inside of an Arc<>... which defeats the whole purpose and can introduce a deadlock
|
||||
pub struct DisplayControls {
|
||||
pub on: AtomicBool,
|
||||
pub brightness: AtomicU8,
|
||||
pub render_is_running: Signal<CriticalSectionRawMutex, bool>
|
||||
on: AtomicBool,
|
||||
brightness: AtomicU8,
|
||||
// FIXME: This should get turned into a embassy_sync::Watch sender, so multiple tasks can wait on the renderer to be running.
|
||||
pub render_is_running: Signal<CriticalSectionRawMutex, bool>,
|
||||
render_pause: Signal<CriticalSectionRawMutex, bool>
|
||||
}
|
||||
|
||||
impl Brightness for DisplayControls {
|
||||
fn set_brightness(&mut self, brightness: u8) {
|
||||
impl DisplayControls {
|
||||
pub fn is_on(&self) -> bool {
|
||||
self.on.load(core::sync::atomic::Ordering::Relaxed)
|
||||
}
|
||||
|
||||
pub fn brightness(&self) -> u8 {
|
||||
self.brightness.load(core::sync::atomic::Ordering::Relaxed)
|
||||
}
|
||||
|
||||
pub fn set_brightness(&self, brightness: u8) {
|
||||
self.brightness.store(brightness, core::sync::atomic::Ordering::Relaxed);
|
||||
}
|
||||
|
||||
fn set_on(&mut self, is_on: bool) {
|
||||
self.on.store(is_on, core::sync::atomic::Ordering::Relaxed);
|
||||
// FIXME: its a bit weird we have a pub function for the renderer's privates to wait while hiding render_pause, but directly expose render_is_running for any task to wait on
|
||||
pub async fn wait_for_on(&self) {
|
||||
self.render_pause.wait().await;
|
||||
}
|
||||
|
||||
// TODO: Split gamma out from brightness controls
|
||||
fn set_gamma(&mut self, _gamma: f32) {
|
||||
unimplemented!()
|
||||
pub fn set_on(&self, is_on: bool) {
|
||||
self.on.store(is_on, core::sync::atomic::Ordering::Relaxed);
|
||||
if is_on {
|
||||
self.render_pause.signal(true);
|
||||
} else {
|
||||
self.render_pause.reset();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -86,7 +124,8 @@ impl Default for DisplayControls {
|
||||
Self {
|
||||
on: AtomicBool::new(true),
|
||||
brightness: AtomicU8::new(255),
|
||||
render_is_running: Signal::new()
|
||||
render_is_running: Signal::new(),
|
||||
render_pause: Signal::new()
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -94,17 +133,17 @@ impl Default for DisplayControls {
|
||||
#[derive(Debug)]
|
||||
pub struct BusGarage {
|
||||
pub motion: Channel<NoopRawMutex, Measurement, 5>,
|
||||
pub scenes: Channel<CriticalSectionRawMutex, Notification, 5>,
|
||||
pub telemetry: Channel<CriticalSectionRawMutex, Telemetry, 15>,
|
||||
pub display: DisplayControls
|
||||
pub notify: Channel<CriticalSectionRawMutex, Notification, 5>,
|
||||
pub predict: Channel<CriticalSectionRawMutex, Prediction, 15>,
|
||||
pub display: Arc<DisplayControls>
|
||||
}
|
||||
|
||||
impl Default for BusGarage {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
motion: Channel::new(),
|
||||
scenes: Channel::new(),
|
||||
telemetry: Channel::new(),
|
||||
notify: Channel::new(),
|
||||
predict: Channel::new(),
|
||||
display: Default::default()
|
||||
}
|
||||
}
|
||||
|
||||
88
src/lib.rs
88
src/lib.rs
@@ -6,49 +6,27 @@ pub mod events;
|
||||
pub mod tasks;
|
||||
pub mod shaders;
|
||||
pub mod ego;
|
||||
pub mod animation;
|
||||
pub mod idle;
|
||||
pub mod logging;
|
||||
|
||||
extern crate alloc;
|
||||
|
||||
use rgb::Rgb;
|
||||
use core::{iter::Sum, ops::{Div, Sub}, fmt::Debug};
|
||||
|
||||
/// Scales the requested brightness to stay within power consumption limits
|
||||
pub fn brightness_for_mw(total_mw : u32, target : u8, max_power: u32) -> u8 {
|
||||
let target32 = target as u32;
|
||||
let requested_mw = (total_mw * target32) / 256;
|
||||
|
||||
if requested_mw > max_power {
|
||||
((target32 * max_power) / requested_mw) as u8
|
||||
} else {
|
||||
target
|
||||
}
|
||||
}
|
||||
|
||||
/// Calculate the estimated power draw of a single pixel, in milliwatts
|
||||
pub fn as_milliwatts(pixel: &Rgb<u8>) -> u32 {
|
||||
// These values are copied from the original FastLED implementation
|
||||
const RED_MW : u32 = 16 * 5; //< 16mA @ 5v = 80mW
|
||||
const GREEN_MW : u32 = 11 * 5; //< 11mA @ 5v = 55mW
|
||||
const BLUE_MW : u32 = 15 * 5; //< 15mA @ 5v = 75mW
|
||||
const DARK_MW : u32 = 5; //< 1mA @ 5v = 5mW
|
||||
|
||||
let red = (pixel.r as u32 * RED_MW).wrapping_shr(8);
|
||||
let green = (pixel.g as u32 * GREEN_MW).wrapping_shr(8);
|
||||
let blue = (pixel.b as u32 * BLUE_MW).wrapping_shr(8);
|
||||
|
||||
red + green + blue + DARK_MW
|
||||
}
|
||||
|
||||
|
||||
#[derive(Debug)]
|
||||
#[derive(Debug, Clone, Copy)]
|
||||
pub struct CircularBuffer<T, const SIZE: usize = 20> {
|
||||
pub data: [T; SIZE],
|
||||
next_index: usize
|
||||
data: [T; SIZE],
|
||||
next_index: usize,
|
||||
filled: bool
|
||||
}
|
||||
|
||||
impl<T: Default + Copy, const SIZE: usize> Default for CircularBuffer<T, SIZE> {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
data: [Default::default(); SIZE],
|
||||
next_index: 0
|
||||
next_index: 0,
|
||||
filled: false
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -59,6 +37,50 @@ impl<T, const SIZE: usize> CircularBuffer<T, SIZE> {
|
||||
self.next_index += 1;
|
||||
if self.next_index == self.data.len() {
|
||||
self.next_index = 0;
|
||||
self.filled = true;
|
||||
}
|
||||
}
|
||||
|
||||
pub fn data(&self) -> &[T; SIZE] {
|
||||
&self.data
|
||||
}
|
||||
|
||||
pub fn is_filled(&self) -> bool {
|
||||
self.filled
|
||||
}
|
||||
|
||||
pub fn mean(&self) -> T where T: for<'a> Sum<&'a T> + Div<f32, Output = T> {
|
||||
self.data.iter().sum::<T>() / self.data.len() as f32
|
||||
}
|
||||
|
||||
pub fn derivative(&self) -> T where for<'a> &'a T: Sub<&'a T, Output = T>, T: Sum<T> + Div<f32, Output = T> {
|
||||
self.data.windows(2).map(|n| {
|
||||
&n[0] - &n[1]
|
||||
}).sum::<T>().div(self.data.len() as f32)
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Default)]
|
||||
pub struct Breaker<T> {
|
||||
tripped: bool,
|
||||
value: T
|
||||
}
|
||||
|
||||
impl<T: PartialEq + Copy + Debug> Breaker<T> {
|
||||
pub fn set(&mut self, value: T) {
|
||||
if self.value != value {
|
||||
self.value = value;
|
||||
self.tripped = true;
|
||||
}
|
||||
}
|
||||
|
||||
pub fn read_tripped(&mut self) -> Option<T> {
|
||||
if self.tripped {
|
||||
//info!("Tripped: {self:?}");
|
||||
self.tripped = false;
|
||||
Some(self.value)
|
||||
} else {
|
||||
None
|
||||
}
|
||||
}
|
||||
}
|
||||
45
src/logging.rs
Normal file
45
src/logging.rs
Normal file
@@ -0,0 +1,45 @@
|
||||
use esp_println::println;
|
||||
use log::{LevelFilter, Metadata, Record};
|
||||
|
||||
pub struct RenderbugLogger;
|
||||
|
||||
impl RenderbugLogger {
|
||||
pub fn init_logger() {
|
||||
unsafe {
|
||||
log::set_logger_racy(&Self).ok();
|
||||
log::set_max_level_racy(LevelFilter::Debug);
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
impl log::Log for RenderbugLogger {
|
||||
fn enabled(&self, _metadata: &Metadata) -> bool {true}
|
||||
|
||||
fn flush(&self) {}
|
||||
|
||||
fn log(&self, record: &Record) {
|
||||
const RESET: &str = "\u{001B}[0m";
|
||||
const RED: &str = "\u{001B}[31m";
|
||||
const GREEN: &str = "\u{001B}[32m";
|
||||
const YELLOW: &str = "\u{001B}[33m";
|
||||
const BLUE: &str = "\u{001B}[34m";
|
||||
const CYAN: &str = "\u{001B}[35m";
|
||||
const GREY: &str = "\u{001B}[38;5;240m";
|
||||
|
||||
let color = match record.level() {
|
||||
log::Level::Error => RED,
|
||||
log::Level::Warn => YELLOW,
|
||||
log::Level::Info => GREEN,
|
||||
log::Level::Debug => BLUE,
|
||||
log::Level::Trace => CYAN,
|
||||
};
|
||||
|
||||
let filename = record.file().map_or("???", |f| {f});
|
||||
if filename.chars().nth(0).unwrap() == '/' {
|
||||
return;
|
||||
}
|
||||
let prefix = filename.split('/').next().unwrap();
|
||||
let suffix = filename.split('/').next_back().unwrap();
|
||||
println!("{color}{}\t{GREY}{prefix}/{suffix}:{}{RESET}\t{}{RESET}", record.level(), record.line().map_or(0, |f| {f}), record.args())
|
||||
}
|
||||
}
|
||||
@@ -3,7 +3,32 @@ use core::cmp::max;
|
||||
use figments::{liber8tion::{interpolate::{ease_in_out_quad}, noise::inoise8, trig::{cos8, sin8}}, prelude::*};
|
||||
use rgb::Rgba;
|
||||
|
||||
use crate::{display::BikeSpace, tasks::render::Uniforms};
|
||||
use crate::display::{SegmentSpace, Uniforms};
|
||||
|
||||
#[derive(Clone, Copy, Default)]
|
||||
pub struct Movement {
|
||||
reverse: bool
|
||||
}
|
||||
|
||||
impl Movement {
|
||||
pub fn reversed(self) -> Self {
|
||||
Self {
|
||||
reverse: !self.reverse
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Shader<Uniforms, SegmentSpace, Rgba<u8>> for Movement {
|
||||
fn draw(&self, surface_coords: &Coordinates<SegmentSpace>, uniforms: &Uniforms) -> Rgba<u8> {
|
||||
let offset = if self.reverse {
|
||||
uniforms.frame.wrapping_add(surface_coords.x)
|
||||
} else {
|
||||
uniforms.frame.wrapping_sub(surface_coords.x)
|
||||
};
|
||||
let idx = sin8(offset).wrapping_add(uniforms.primary_color.hue);
|
||||
Rgba::new(idx, idx.wrapping_mul(2), idx.wrapping_div(2), 128)
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Clone, Copy, Default)]
|
||||
pub struct Background {
|
||||
@@ -18,8 +43,8 @@ impl Background {
|
||||
}
|
||||
}
|
||||
|
||||
impl Shader<Uniforms, BikeSpace, Rgba<u8>> for Background {
|
||||
fn draw(&self, coords: &Coordinates<BikeSpace>, uniforms: &Uniforms) -> Rgba<u8> {
|
||||
impl Shader<Uniforms, SegmentSpace, Rgba<u8>> for Background {
|
||||
fn draw(&self, coords: &Coordinates<SegmentSpace>, uniforms: &Uniforms) -> Rgba<u8> {
|
||||
let noise_x = sin8(uniforms.frame % 255) as i16;
|
||||
let noise_y = cos8(uniforms.frame % 255) as i16;
|
||||
let brightness = inoise8(noise_x.wrapping_add(coords.x as i16), noise_y.wrapping_add(coords.y as i16));
|
||||
@@ -35,8 +60,8 @@ impl Shader<Uniforms, BikeSpace, Rgba<u8>> for Background {
|
||||
|
||||
#[derive(Clone, Copy, Default)]
|
||||
pub struct Tail {}
|
||||
impl Shader<Uniforms, BikeSpace, Rgba<u8>> for Tail {
|
||||
fn draw(&self, coords: &Coordinates<BikeSpace>, uniforms: &Uniforms) -> Rgba<u8> {
|
||||
impl Shader<Uniforms, SegmentSpace, Rgba<u8>> for Tail {
|
||||
fn draw(&self, coords: &Coordinates<SegmentSpace>, uniforms: &Uniforms) -> Rgba<u8> {
|
||||
let hue_offset: u8 = 32.scale8(sin8(uniforms.frame.wrapping_sub(coords.x)));
|
||||
let value = max(30, inoise8(coords.x.wrapping_add(uniforms.frame) as i16, coords.y.wrapping_add(uniforms.frame) as i16));
|
||||
Hsv::new(uniforms.primary_color.hue.wrapping_sub(hue_offset), max(210, sin8(uniforms.frame.wrapping_add(coords.x))), value).into()
|
||||
@@ -59,15 +84,15 @@ impl Brakelight {
|
||||
15
|
||||
}
|
||||
|
||||
pub const fn rectangle() -> Rectangle<BikeSpace> {
|
||||
pub const fn rectangle() -> Rectangle<SegmentSpace> {
|
||||
Rectangle::new_from_coordinates(Self::end() - Self::length(), 5, Self::end(), 5)
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
impl Shader<Uniforms, BikeSpace, Rgba<u8>> for Brakelight {
|
||||
impl Shader<Uniforms, SegmentSpace, Rgba<u8>> for Brakelight {
|
||||
|
||||
fn draw(&self, coords: &Coordinates<BikeSpace>, uniforms: &Uniforms) -> Rgba<u8> {
|
||||
fn draw(&self, coords: &Coordinates<SegmentSpace>, uniforms: &Uniforms) -> Rgba<u8> {
|
||||
let distance_from_end = Self::end() - coords.x;
|
||||
|
||||
if distance_from_end < Self::safety_length() {
|
||||
@@ -81,16 +106,16 @@ impl Shader<Uniforms, BikeSpace, Rgba<u8>> for Brakelight {
|
||||
|
||||
#[derive(Clone, Copy, Default)]
|
||||
pub struct Headlight {}
|
||||
impl Shader<Uniforms, BikeSpace, Rgba<u8>> for Headlight {
|
||||
fn draw(&self, coords: &Coordinates<BikeSpace>, uniforms: &Uniforms) -> Rgba<u8> {
|
||||
Hsv::new(0, 0, max(100, ease_in_out_quad(sin8(uniforms.frame.wrapping_sub(coords.x))))).into()
|
||||
impl Shader<Uniforms, SegmentSpace, Rgba<u8>> for Headlight {
|
||||
fn draw(&self, coords: &Coordinates<SegmentSpace>, uniforms: &Uniforms) -> Rgba<u8> {
|
||||
Hsv::new(0, 0, max(130, ease_in_out_quad(sin8(uniforms.frame.wrapping_sub(coords.x))))).into()
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Clone, Copy, Default)]
|
||||
pub struct Panel {}
|
||||
impl Shader<Uniforms, BikeSpace, Rgba<u8>> for Panel {
|
||||
fn draw(&self, coords: &Coordinates<BikeSpace>, uniforms: &Uniforms) -> Rgba<u8> {
|
||||
impl Shader<Uniforms, SegmentSpace, Rgba<u8>> for Panel {
|
||||
fn draw(&self, coords: &Coordinates<SegmentSpace>, uniforms: &Uniforms) -> Rgba<u8> {
|
||||
let noise_offset = max(180, inoise8(coords.x.wrapping_add(uniforms.frame) as i16, coords.y.wrapping_add(uniforms.frame) as i16));
|
||||
let pct = (coords.x as f32 / 18f32) * 255f32;
|
||||
let shift = match coords.y {
|
||||
@@ -105,8 +130,8 @@ impl Shader<Uniforms, BikeSpace, Rgba<u8>> for Panel {
|
||||
#[derive(Clone, Copy, Default)]
|
||||
pub struct Thinking {}
|
||||
|
||||
impl Shader<Uniforms, BikeSpace, Rgba<u8>> for Thinking {
|
||||
fn draw(&self, coords: &Coordinates<BikeSpace>, uniforms: &Uniforms) -> Rgba<u8> {
|
||||
impl Shader<Uniforms, SegmentSpace, Rgba<u8>> for Thinking {
|
||||
fn draw(&self, coords: &Coordinates<SegmentSpace>, uniforms: &Uniforms) -> Rgba<u8> {
|
||||
//let noise_x = sin8(sin8((frame % 255) as u8).wrapping_add(coords.x));
|
||||
//let noise_y = cos8(cos8((frame % 255) as u8).wrapping_add(coords.y));
|
||||
let offset_x = sin8(uniforms.frame.wrapping_add(coords.x));
|
||||
@@ -117,8 +142,8 @@ impl Shader<Uniforms, BikeSpace, Rgba<u8>> for Thinking {
|
||||
//let noise_y = coords.y.wrapping_add(offset_y);
|
||||
Hsv::new(
|
||||
inoise8(offset_x as i16, offset_y as i16),
|
||||
128_u8.saturating_add(inoise8(noise_y as i16, noise_x as i16)),
|
||||
128_u8.saturating_add(inoise8(noise_y.into(), noise_x.into())),
|
||||
255
|
||||
).into()
|
||||
}
|
||||
}
|
||||
}
|
||||
18
src/tasks/demo.rs
Normal file
18
src/tasks/demo.rs
Normal file
@@ -0,0 +1,18 @@
|
||||
use embassy_sync::channel::DynamicSender;
|
||||
use embassy_time::Timer;
|
||||
|
||||
use crate::events::{Notification, Scene};
|
||||
|
||||
|
||||
#[embassy_executor::task]
|
||||
pub async fn demo_task(ui: DynamicSender<'static, Notification>) {
|
||||
Timer::after_secs(10).await;
|
||||
ui.send(Notification::SceneChange(Scene::Idle)).await;
|
||||
Timer::after_secs(10).await;
|
||||
loop {
|
||||
for scene in [Scene::Accelerating, Scene::Ready, Scene::Decelerating, Scene::Ready] {
|
||||
Timer::after_secs(8).await;
|
||||
ui.send(Notification::SceneChange(scene)).await
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -10,9 +10,7 @@ use nmea::Nmea;
|
||||
|
||||
use crate::{backoff::Backoff, events::Measurement};
|
||||
|
||||
#[allow(dead_code)] //FIXME: Allow switching to this via configure option
|
||||
const GPS_TEST_DATA: &str = include_str!("../test.nmea");
|
||||
|
||||
// FIXME: We need a way to put the GPS to sleep when the system goes to sleep
|
||||
#[embassy_executor::task]
|
||||
pub async fn gps_task(events: DynamicSender<'static, Measurement>, mut i2c_bus: I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>) {
|
||||
Backoff::from_secs(5).forever().attempt::<_, (), ()>(async || {
|
||||
@@ -40,16 +38,15 @@ pub async fn gps_task(events: DynamicSender<'static, Measurement>, mut i2c_bus:
|
||||
let mut parser = Nmea::default();
|
||||
let mut parsing = false;
|
||||
let mut has_lock = false;
|
||||
//let mut iter = GPS_TEST_DATA.as_bytes().iter();
|
||||
info!("GPS is ready!");
|
||||
loop {
|
||||
let mut buf = [0; 1];
|
||||
i2c_bus.read(0x10, &mut buf).await.map_err(|_| { Err::<(), ()>(()) }).ok();
|
||||
//buf[0] = *(iter.next().unwrap());
|
||||
if (buf[0] as char == '\n' || buf[0] as char == '\r') && !strbuf.is_empty() {
|
||||
if let Ok(result) = parser.parse(&strbuf) {
|
||||
match parser.fix_type {
|
||||
None if has_lock => {
|
||||
// TODO: Send a Measurement::SensorOffline(SensorSource::GPS) here instead
|
||||
events.send(Measurement::GPS(None)).await;
|
||||
has_lock = false
|
||||
},
|
||||
@@ -58,6 +55,7 @@ pub async fn gps_task(events: DynamicSender<'static, Measurement>, mut i2c_bus:
|
||||
if !has_lock {
|
||||
has_lock = true;
|
||||
}
|
||||
// TODO: Send a Measurement::SensorOnline(SensorSource::GPS) here instead
|
||||
|
||||
//TODO: 4 satellites seems to be "Some" fix, 6 is a perfect fix
|
||||
//TODO: Only send updates when we get the correct nmea sentence
|
||||
@@ -66,11 +64,11 @@ pub async fn gps_task(events: DynamicSender<'static, Measurement>, mut i2c_bus:
|
||||
}
|
||||
}
|
||||
}
|
||||
log::info!("nmea={result:?} raw={strbuf:?}");
|
||||
log::debug!("nmea={parser:?}");
|
||||
log::info!("speed={:?} altitude={:?} lat={:?} lng={:?} fix={:?}", parser.speed_over_ground, parser.altitude, parser.latitude, parser.longitude, parser.fix_type);
|
||||
log::trace!("nmea={result:?} raw={strbuf:?}");
|
||||
log::trace!("nmea={parser:?}");
|
||||
log::trace!("speed={:?} altitude={:?} lat={:?} lng={:?} fix={:?}", parser.speed_over_ground, parser.altitude, parser.latitude, parser.longitude, parser.fix_type);
|
||||
for sat in parser.satellites() {
|
||||
info!("\t{} snr={:?} prn={:?}", sat.gnss_type(), sat.snr(), sat.prn())
|
||||
trace!("\t{} snr={:?} prn={:?}", sat.gnss_type(), sat.snr(), sat.prn())
|
||||
}
|
||||
} else {
|
||||
log::warn!("Unhandled NMEA {strbuf:?}");
|
||||
|
||||
@@ -1,7 +1,13 @@
|
||||
#[cfg(feature="mpu")]
|
||||
pub mod mpu;
|
||||
#[cfg(feature="gps")]
|
||||
pub mod gps;
|
||||
pub mod render;
|
||||
pub mod motion;
|
||||
pub mod gps;
|
||||
pub mod ui;
|
||||
#[cfg(feature="radio")]
|
||||
pub mod wifi;
|
||||
#[cfg(feature="simulation")]
|
||||
pub mod simulation;
|
||||
pub mod simulation;
|
||||
pub mod predict;
|
||||
pub mod demo;
|
||||
@@ -1,203 +1,31 @@
|
||||
use embassy_sync::channel::{DynamicReceiver, DynamicSender};
|
||||
use nalgebra::{Rotation3, Vector2, Vector3, ComplexField};
|
||||
|
||||
use crate::{ego::{alignment::{Direction, DirectionEstimator, SensorAlignment}, heading::HeadingEstimator, kalman::{GpsMeasurement, Kalman}}, events::{Measurement, Notification, Prediction, Telemetry}, CircularBuffer};
|
||||
use log::*;
|
||||
|
||||
#[embassy_executor::task]
|
||||
pub async fn motion_task(src: DynamicReceiver<'static, Measurement>, ui_sink: DynamicSender<'static, Notification>, telemetry_sink: DynamicSender<'static, Telemetry>) {
|
||||
//TODO: is this the right value? how do update frequencies work?
|
||||
let dt = 0.01; // 10 Hz
|
||||
let mut kf = Kalman::new(dt);
|
||||
use crate::{ego::engine::BikeStates, events::{Measurement, Notification, Prediction}};
|
||||
|
||||
let mut recent_accel: CircularBuffer<Vector3<f32>, 100> = CircularBuffer::default();
|
||||
let mut gravity_align = None;
|
||||
let mut forwards_align = DirectionEstimator::new(Direction::UpX);
|
||||
let mut last_fix = Vector2::default();
|
||||
let mut reference_fix = None;
|
||||
let mut heading = HeadingEstimator::new(0.0);
|
||||
#[embassy_executor::task]
|
||||
pub async fn motion_task(src: DynamicReceiver<'static, Measurement>, ui_sink: DynamicSender<'static, Notification>, prediction_sink: DynamicSender<'static, Prediction>) {
|
||||
let mut states = BikeStates::default();
|
||||
|
||||
loop {
|
||||
let next_measurement = src.receive().await;
|
||||
let _ = telemetry_sink.try_send(Telemetry::Measurement(next_measurement));
|
||||
debug!("measurement={next_measurement:?}");
|
||||
match next_measurement {
|
||||
Measurement::IMU { accel, gyro } => {
|
||||
recent_accel.insert(accel);
|
||||
// Require a gravity alignment first
|
||||
if gravity_align.is_none() {
|
||||
gravity_align = {
|
||||
let align = SensorAlignment::from_samples(&recent_accel.data, crate::ego::alignment::Direction::DnZ);
|
||||
if align.bias.norm() < 9.764 || align.bias.norm() > 9.834 {
|
||||
None
|
||||
} else {
|
||||
ui_sink.send(Notification::CalibrationComplete).await;
|
||||
Some(align)
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// Minimum motion threshold
|
||||
if accel.xy().magnitude() >= 0.8 {
|
||||
// Try to build a new 'forwards' alignment
|
||||
forwards_align.update(accel);
|
||||
|
||||
// Rotate the measurement into the gravity-down frame
|
||||
let (body_accel, body_gyro) = if let Some(ref adj) = gravity_align {
|
||||
(adj.apply(&accel), adj.apply(&gyro))
|
||||
} else {
|
||||
(accel, gyro)
|
||||
};
|
||||
|
||||
// Then rotate the other two axes to produce the final body frame
|
||||
let forwards_adjusted = forwards_align.apply(&body_accel);
|
||||
heading.update(body_gyro.z, 0.1); // TODO: need to correctly use time values for dt
|
||||
let heading_rotation = Rotation3::from_axis_angle(&Vector3::z_axis(), heading.heading());
|
||||
|
||||
let enu_rotated = heading_rotation * forwards_adjusted;
|
||||
|
||||
// FIXME: the last thing to add here is rotating the acceleration measurements into the ENU frame.
|
||||
let m = Vector3::new(enu_rotated.x, enu_rotated.y, body_gyro.z);
|
||||
kf.predict(&m);
|
||||
} else {
|
||||
// Otherwise, we are standing stationary and should insert accel=0 data into the model
|
||||
kf.update_zupt();
|
||||
}
|
||||
}
|
||||
states.insert_imu(accel, gyro);
|
||||
states.commit(&prediction_sink, &ui_sink).await;
|
||||
},
|
||||
Measurement::GPS(Some(gps_pos)) => {
|
||||
match reference_fix {
|
||||
None => {
|
||||
reference_fix = Some(gps_pos);
|
||||
last_fix = gps_pos;
|
||||
ui_sink.send(Notification::GPSAcquired).await;
|
||||
},
|
||||
Some(coords) => {
|
||||
|
||||
let est = kf.state();
|
||||
info!("pos=\t({},\t{})\tvel=({},\t{})\theading={}\t({})\tforwards={:?}", est[0], est[1], est[2], est[3], est[4], heading.heading(), forwards_align.apply(&Vector3::new(1.0, 0.0, 0.0)));
|
||||
if last_fix != coords {
|
||||
let gps_heading = last_fix.angle(&coords);
|
||||
heading.correct(gps_heading as f32, 0.9);
|
||||
}
|
||||
|
||||
// Convert GPS coordinates into a meter distance away from the known fix location
|
||||
let dx = (gps_pos.x - coords.x) * coords.x.cos() * 6371000.0;
|
||||
let dy = (gps_pos.y - coords.y) * 6371000.0;
|
||||
kf.update(&GpsMeasurement::new(dx as f32, dy as f32));
|
||||
info!("GPS={gps_pos:?} d=({dx}, {dy}) heading={}", heading.heading().to_degrees());
|
||||
|
||||
last_fix = coords;
|
||||
}
|
||||
}
|
||||
states.insert_gps(gps_pos);
|
||||
states.commit(&prediction_sink, &ui_sink).await;
|
||||
},
|
||||
Measurement::GPS(None) => {
|
||||
reference_fix = None;
|
||||
ui_sink.send(Notification::GPSLost).await;
|
||||
}
|
||||
}
|
||||
|
||||
let est = kf.state();
|
||||
if est[2].abs() + est[3].abs() >= 5.0 {
|
||||
//info!("pos=\t({},\t{})\tvel=({},\t{})\theading={}\tforwards={:?}", est[0], est[1], est[2], est[3], est[4], forwards_align.apply(&Vector3::new(1.0, 0.0, 0.0)));
|
||||
}
|
||||
|
||||
let _ = telemetry_sink.try_send(Telemetry::Prediction(Prediction::Location(est.xy())));
|
||||
}
|
||||
}
|
||||
|
||||
/*#[embassy_executor::task]
|
||||
pub async fn _motion_task(src: DynamicReceiver<'static, Measurement>, sink: DynamicSender<'static, Notification>, telemetry_sink: DynamicSender<'static, Measurement>) {
|
||||
//TODO: is this the right value? how do update frequencies work?
|
||||
let dt = 0.01; // 10 Hz
|
||||
let mut kf = Kalman::new(dt);
|
||||
let mut is_moving = false;
|
||||
let mut heading = HeadingEstimator::new(0.0);
|
||||
|
||||
//TODO: Accelerating/decelerating events need thresholds, maybe a PID to detect? PID setpoint can be a running average speed, and if the calculated error is above/below a threshold, it means we are accelerating/decelerating
|
||||
|
||||
let mut last_fix = None;
|
||||
let mut last_idle = Instant::now();
|
||||
let mut is_maybe_idling = false;
|
||||
let mut is_idle = false;
|
||||
let mut is_parked = false;
|
||||
|
||||
loop {
|
||||
match src.receive().await {
|
||||
Measurement::IMU{ accel, gyro } => {
|
||||
//info!("IMU update {imu_accel:?}");
|
||||
//info!("m=\t({:?}, {:?})\tyaw={:?}", imu_accel.x, imu_accel.y, imu_accel.z);
|
||||
//kf.predict(&Control::new(imu_accel.x, imu_accel.y, imu_accel.z));
|
||||
// TODO: Also needs corrected using GPS headings via rotation from body frame into world frame
|
||||
//heading.heading();
|
||||
heading.update(accel.z, 0.01);
|
||||
|
||||
//info!("{}", imu_accel.xy().magnitude());
|
||||
if accel.xy().magnitude() >= 0.8 {
|
||||
//info!("magnitude {imu_accel:?} {}", imu_accel.xy().magnitude());
|
||||
kf.predict(&accel);
|
||||
if is_parked {
|
||||
info!("Wake up!");
|
||||
sink.send(Notification::SceneChange(crate::events::Scene::ParkedIdle)).await;
|
||||
is_maybe_idling = false;
|
||||
is_parked = false;
|
||||
is_idle = false;
|
||||
}
|
||||
} else {
|
||||
//info!("Stationary!!");
|
||||
kf.update_zupt();
|
||||
|
||||
if !is_maybe_idling {
|
||||
is_maybe_idling = true;
|
||||
last_idle = Instant::now();
|
||||
} else if !is_idle && Instant::now() - last_idle >= Duration::from_secs(15) {
|
||||
info!("Parked idle");
|
||||
sink.send(Notification::SceneChange(crate::events::Scene::ParkedIdle)).await;
|
||||
is_idle = true;
|
||||
} else if !is_parked && Instant::now() - last_idle >= Duration::from_secs(60 * 3) {
|
||||
info!("Parked long term!");
|
||||
sink.send(Notification::SceneChange(crate::events::Scene::ParkedLongTerm)).await;
|
||||
is_parked = true;
|
||||
}
|
||||
}
|
||||
// TODO: Rotate the acceleration values into the World frame using GPS heading values
|
||||
|
||||
// X means forwards, Y is left/right slide, Z is vertical movement
|
||||
if accel.x > 1.0 {
|
||||
//sink.send(Notification::SceneChange(crate::events::Scene::Accelerating)).await;
|
||||
} else if accel.x < -1.0 {
|
||||
//sink.send(Notification::SceneChange(crate::events::Scene::Decelerating)).await;
|
||||
}
|
||||
|
||||
let _ = telemetry_sink.try_send(Measurement::IMU { accel, gyro });
|
||||
}
|
||||
Measurement::GPS(gps_pos) => {
|
||||
//info!("GPS update! {gps_pos:?}");
|
||||
match last_fix {
|
||||
None => last_fix = Some(gps_pos),
|
||||
Some(coords) => {
|
||||
let dx = (gps_pos.x - coords.x) * coords.x.cos() * 6371000.0;
|
||||
let dy = (gps_pos.y - coords.y) * 6371000.0;
|
||||
kf.update(&GpsMeasurement::new(dx as f32, dy as f32));
|
||||
}
|
||||
}
|
||||
|
||||
let _ = telemetry_sink.try_send(Measurement::GPS(gps_pos));
|
||||
}
|
||||
}
|
||||
|
||||
let est = kf.state();
|
||||
//let _ = telemetry_sink.try_send(Measurement::Prediction(*est));
|
||||
if est[2].abs() + est[3].abs() >= 0.2 {
|
||||
info!("pos=\t({},\t{})\tvel=({},\t{})\theading=({},\t{})", est[0], est[1], est[2], est[3], est[4], est[5]);
|
||||
}
|
||||
|
||||
if est[2].abs() > 1.0 {
|
||||
if !is_moving {
|
||||
is_moving = true;
|
||||
sink.send(Notification::SceneChange(crate::events::Scene::Accelerating)).await;
|
||||
}
|
||||
} else if is_moving {
|
||||
is_moving = false;
|
||||
sink.send(Notification::SceneChange(crate::events::Scene::StoplightIdle)).await;
|
||||
states.has_gps_fix.set(false);
|
||||
},
|
||||
// FIXME: This needs harmonized with the automatic data timeout from above, somehow?
|
||||
Measurement::SensorOnline(source) => warn!("Sensor {source:?} is online!"),
|
||||
Measurement::SensorOffline(source) => warn!("Sensor {source:?} is offline!"),
|
||||
Measurement::SimulationProgress(source, duration, _pct) => debug!("{source:?} simulation time: {}", duration.as_secs()),
|
||||
}
|
||||
}
|
||||
}*/
|
||||
}
|
||||
@@ -2,14 +2,15 @@ use core::cell::RefCell;
|
||||
|
||||
use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
|
||||
use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::DynamicSender};
|
||||
use embassy_time::{Delay, Timer};
|
||||
use embassy_time::{Delay, Timer, Instant};
|
||||
use esp_hal::{i2c::master::I2c, Async};
|
||||
use log::{info, warn};
|
||||
use log::*;
|
||||
use mpu6050_dmp::{address::Address, calibration::CalibrationParameters, error_async::Error, sensor_async::Mpu6050};
|
||||
use nalgebra::Vector3;
|
||||
use core::f32::consts::PI;
|
||||
use crate::events::SensorSource;
|
||||
|
||||
use crate::{backoff::Backoff, ego::tilt::TiltEstimator, events::Measurement};
|
||||
use crate::{backoff::Backoff, events::Measurement};
|
||||
|
||||
const G: f32 = 9.80665;
|
||||
const SENS_2G: f32 = 16384.0;
|
||||
@@ -27,43 +28,6 @@ fn gyro_raw_to_rads(raw: i16) -> f32 {
|
||||
(raw as f32 / 16.4) * (DEG2RAD)
|
||||
}
|
||||
|
||||
/*/// Rotate body accel into ENU and remove gravity; returns (east, north, up) linear accel in m/s^2
|
||||
pub fn accel_body_to_enu_lin(body_acc: (f32,f32,f32), roll: f32, pitch: f32, yaw: f32) -> (f32,f32,f32) {
|
||||
let (ax, ay, az) = body_acc;
|
||||
|
||||
let (sr, cr) = roll.sin_cos();
|
||||
let (sp, cp) = pitch.sin_cos();
|
||||
let (sy, cy) = yaw.sin_cos();
|
||||
|
||||
// Build R = Rz(yaw) * Ry(pitch) * Rx(roll)
|
||||
// multiply R * a_body
|
||||
let r11 = cy*cp;
|
||||
let r12 = cy*sp*sr - sy*cr;
|
||||
let r13 = cy*sp*cr + sy*sr;
|
||||
|
||||
let r21 = sy*cp;
|
||||
let r22 = sy*sp*sr + cy*cr;
|
||||
let r23 = sy*sp*cr - cy*sr;
|
||||
|
||||
let r31 = -sp;
|
||||
let r32 = cp*sr;
|
||||
let r33 = cp*cr;
|
||||
|
||||
// a_nav = R * a_body
|
||||
let ax_nav = r11*ax + r12*ay + r13*az; // east
|
||||
let ay_nav = r21*ax + r22*ay + r23*az; // north
|
||||
let az_nav = r31*ax + r32*ay + r33*az; // up
|
||||
|
||||
// remove gravity (nav frame gravity points down in NED; in our ENU 'up' axis positive up)
|
||||
// If accel measured gravity as +9.8 on sensor when static and z down convention used above,
|
||||
// then az_nav will include +g in 'up' direction; subtract G.
|
||||
let ax_lin = ax_nav;
|
||||
let ay_lin = ay_nav;
|
||||
let az_lin = az_nav - G; // linear acceleration in up axis
|
||||
|
||||
(ax_lin, ay_lin, az_lin)
|
||||
}*/
|
||||
|
||||
#[embassy_executor::task]
|
||||
pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>) {
|
||||
let backoff = Backoff::from_millis(5);
|
||||
@@ -71,7 +35,7 @@ pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevic
|
||||
|
||||
backoff.forever().attempt::<_, (), ()>(async || {
|
||||
let mut sensor = backoff.forever().attempt(async || {
|
||||
info!("Initializing MPU");
|
||||
warn!("Initializing connection to MPU");
|
||||
match Mpu6050::new(busref.replace(None).unwrap(), Address::default()).await.map_err(|e| { e.i2c }) {
|
||||
Err(i2c) => {
|
||||
busref.replace(Some(i2c));
|
||||
@@ -91,8 +55,7 @@ pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevic
|
||||
|
||||
let sensor_ref = &mut sensor;
|
||||
|
||||
info!("MPU is ready!");
|
||||
let _tilt = TiltEstimator::new(0.3);
|
||||
events.send(Measurement::SensorOnline(SensorSource::IMU)).await;
|
||||
//TODO: Need to read in a constant buffer of accelerometer measurements, which we can then use to determine where "forward" points in the body frame when converting from the sensor frame.
|
||||
// From there, we can rotate the body frame into the world frame using gps headings to generate a compass north
|
||||
fn lowpass(prev: f32, current: f32, alpha: f32) -> f32 {
|
||||
@@ -122,12 +85,6 @@ pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevic
|
||||
);
|
||||
prev_accel = adjusted;
|
||||
|
||||
// Apply current rotation and accel values to rotate our values back into the ENU frame
|
||||
/*tilt.update(0.01, (filtered.x, filtered.y, filtered.z), (adjusted_gyro.x, adjusted_gyro.y, adjusted_gyro.z));
|
||||
let (roll, pitch, yaw) = tilt.angles();
|
||||
let (ax_e, ay_n, az_up) = accel_body_to_enu_lin((filtered.x,filtered.y,filtered.z), roll, pitch, yaw);
|
||||
|
||||
let measured = Vector3::new(ax_e, ay_n, az_up);*/
|
||||
events.send(
|
||||
Measurement::IMU {
|
||||
accel: filtered,
|
||||
@@ -137,7 +94,7 @@ pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevic
|
||||
Timer::after_millis(10).await;
|
||||
},
|
||||
Err(e) => {
|
||||
warn!("Failed to read MPU motion data! {e:?}");
|
||||
error!("Failed to read MPU motion data! {e:?}");
|
||||
busref.replace(Some(sensor.release()));
|
||||
return Err(());
|
||||
}
|
||||
@@ -147,18 +104,28 @@ pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevic
|
||||
}
|
||||
|
||||
async fn mpu_init(sensor: &mut Mpu6050<I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>>) -> Result<(), Error<I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>>> {
|
||||
let mut delay = Delay;
|
||||
let backoff = Backoff::from_millis(10);
|
||||
info!("Initializing MPU");
|
||||
backoff.attempt(async || { sensor.initialize_dmp(&mut delay).await }).await?;
|
||||
backoff.attempt(async || { sensor.set_digital_lowpass_filter(mpu6050_dmp::config::DigitalLowPassFilter::Filter3).await }).await?;
|
||||
info!("Calibrating MPU");
|
||||
backoff.attempt(async || {
|
||||
// TODO: Store calibration data with set_accel_calibration, set_gyro_calibration
|
||||
sensor.calibrate(&mut delay, &CalibrationParameters::new(
|
||||
mpu6050_dmp::accel::AccelFullScale::G2,
|
||||
mpu6050_dmp::gyro::GyroFullScale::Deg2000,
|
||||
mpu6050_dmp::calibration::ReferenceGravity::Zero
|
||||
)).await.map(|_| { Ok(()) })
|
||||
}).await?
|
||||
if cfg!(feature="wokwi") {
|
||||
warn!("Initializing simulated MPU");
|
||||
Ok(())
|
||||
} else {
|
||||
let mut delay = Delay;
|
||||
let backoff = Backoff::from_millis(10);
|
||||
info!("Initializing DMP");
|
||||
let start = Instant::now();
|
||||
backoff.attempt(async || { sensor.initialize_dmp(&mut delay).await }).await?;
|
||||
backoff.attempt(async || { sensor.set_digital_lowpass_filter(mpu6050_dmp::config::DigitalLowPassFilter::Filter1).await }).await?;
|
||||
info!("DMP ready in {}ms", start.as_millis());
|
||||
info!("Calibrating MPU");
|
||||
let start = Instant::now();
|
||||
backoff.attempt(async || {
|
||||
// TODO: Store calibration data with set_accel_calibration, set_gyro_calibration
|
||||
sensor.calibrate(&mut delay, &CalibrationParameters::new(
|
||||
mpu6050_dmp::accel::AccelFullScale::G2,
|
||||
mpu6050_dmp::gyro::GyroFullScale::Deg2000,
|
||||
mpu6050_dmp::calibration::ReferenceGravity::Zero
|
||||
)).await
|
||||
}).await?;
|
||||
info!("MPU calibrated in {}ms", start.as_millis());
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
108
src/tasks/predict.rs
Normal file
108
src/tasks/predict.rs
Normal file
@@ -0,0 +1,108 @@
|
||||
|
||||
use embassy_sync::channel::{DynamicReceiver, DynamicSender};
|
||||
use embassy_time::Duration;
|
||||
use log::*;
|
||||
|
||||
use crate::{ego::engine::{gps_to_local_meters_haversine, MotionState}, events::{Notification, Prediction, Scene}, idle::IdleClock};
|
||||
|
||||
#[embassy_executor::task]
|
||||
pub async fn prediction_task(prediction_src: DynamicReceiver<'static, Prediction>, notify: DynamicSender<'static, Notification>) {
|
||||
let mut last_velocity = 0.0;
|
||||
let mut first_position = None;
|
||||
let mut last_position = Default::default();
|
||||
let mut parking_timer = IdleClock::new(Duration::from_secs(10));
|
||||
let mut sleep_timer = IdleClock::new(Duration::from_secs(30));
|
||||
let mut stationary = true;
|
||||
loop {
|
||||
let d = first_position.map(|x| {
|
||||
gps_to_local_meters_haversine(&x, &last_position).norm()
|
||||
});
|
||||
if let Ok(next_evt) = embassy_time::with_timeout(Duration::from_secs(1), prediction_src.receive()).await {
|
||||
match next_evt {
|
||||
Prediction::WakeRequested => {
|
||||
if sleep_timer.wake() {
|
||||
warn!("Wake requested during sleep");
|
||||
notify.send(Notification::WakeUp).await;
|
||||
|
||||
// Also reset the parking timer
|
||||
parking_timer.wake();
|
||||
} else if parking_timer.wake() {
|
||||
info!("Wake requested while parked");
|
||||
// If we weren't asleep but we were parked, then switch back to the Ready state and turn on the lights
|
||||
notify.send(Notification::SetHeadlight(true)).await;
|
||||
notify.send(Notification::SetBrakelight(true)).await;
|
||||
notify.send(Notification::SceneChange(Scene::Ready)).await;
|
||||
}
|
||||
}
|
||||
Prediction::Velocity(v) => {
|
||||
last_velocity = v;
|
||||
|
||||
if v > 5.0 && stationary {
|
||||
notify.send(Notification::Beat).await;
|
||||
}
|
||||
// TODO: Probably makes sense to only print this based on an IdleTimer, so that a long period of slightly variable movement doesn't get lost, but we can still report values to the UI / telemetry outputs
|
||||
//info!("Velocity predict: velocity={v}\tpos={last_position:?}\tdistance={d:?}");
|
||||
},
|
||||
Prediction::Location(loc) => {
|
||||
if first_position.is_none() {
|
||||
info!("Got location={loc:?}");
|
||||
first_position = Some(loc);
|
||||
}
|
||||
last_position = loc;
|
||||
}
|
||||
Prediction::Motion(motion) => {
|
||||
info!("Motion predict:\t{motion:?}\tvelocity={last_velocity}\tpos={last_position:?}\tdistance={d:?}");
|
||||
if sleep_timer.wake() {
|
||||
notify.send(Notification::WakeUp).await;
|
||||
notify.send(Notification::SetHeadlight(true)).await;
|
||||
notify.send(Notification::SetBrakelight(true)).await
|
||||
}
|
||||
|
||||
if parking_timer.wake() {
|
||||
notify.send(Notification::SetHeadlight(true)).await;
|
||||
notify.send(Notification::SetBrakelight(true)).await
|
||||
}
|
||||
match motion {
|
||||
MotionState::Accelerating => {
|
||||
if stationary {
|
||||
// If we are going from standing still to immediately accelerating, first transition to the 'ready' scene
|
||||
notify.send(Notification::SceneChange(Scene::Ready)).await;
|
||||
}
|
||||
notify.send(Notification::SceneChange(Scene::Accelerating)).await;
|
||||
stationary = false;
|
||||
},
|
||||
MotionState::Decelerating => {
|
||||
if stationary {
|
||||
// If we are going from standing still to immediately accelerating, first transition to the 'ready' scene
|
||||
notify.send(Notification::SceneChange(Scene::Ready)).await;
|
||||
}
|
||||
notify.send(Notification::SceneChange(Scene::Decelerating)).await;
|
||||
stationary = false;
|
||||
},
|
||||
MotionState::Steady => {
|
||||
notify.send(Notification::SceneChange(Scene::Ready)).await;
|
||||
stationary = false;
|
||||
},
|
||||
MotionState::Stationary => {
|
||||
notify.send(Notification::SceneChange(Scene::Ready)).await;
|
||||
stationary = true
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
// TODO: Need a way to detect if sensors are dead for some reason. Probably should be done in the motion engine, since it would be a prediction?
|
||||
|
||||
if stationary {
|
||||
if parking_timer.check() {
|
||||
notify.send(Notification::SceneChange(Scene::Idle)).await;
|
||||
notify.send(Notification::SetHeadlight(false)).await;
|
||||
notify.send(Notification::SetBrakelight(false)).await
|
||||
}
|
||||
|
||||
if sleep_timer.check() {
|
||||
notify.send(Notification::Sleep).await;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,23 +1,20 @@
|
||||
use embassy_time::{Duration, Instant, Timer};
|
||||
use esp_hal::{gpio::AnyPin, rmt::Rmt, time::Rate};
|
||||
use esp_hal::{gpio::AnyPin, rmt::Rmt, time::Rate, timer::timg::Wdt};
|
||||
use esp_hal_smartled::{buffer_size_async, SmartLedsAdapterAsync};
|
||||
use figments::{hardware::{Brightness, OutputAsync}, prelude::Hsv, surface::{BufferedSurfacePool, Surfaces}};
|
||||
use figments::{prelude::*, surface::{BufferedSurfacePool, Surfaces}};
|
||||
use figments_render::gamma::GammaCurve;
|
||||
use figments_render::output::{GammaCorrected, OutputAsync};
|
||||
use log::{info, warn};
|
||||
use rgb::Rgba;
|
||||
use nalgebra::ComplexField;
|
||||
use alloc::sync::Arc;
|
||||
|
||||
use crate::{display::{BikeOutputAsync, BikeSpace}, events::DisplayControls};
|
||||
|
||||
#[derive(Default)]
|
||||
pub struct Uniforms {
|
||||
pub frame: usize,
|
||||
pub primary_color: Hsv
|
||||
}
|
||||
use crate::{display::{BikeOutput, SegmentSpace, Uniforms}, events::DisplayControls};
|
||||
|
||||
//TODO: Import the bike surfaces from renderbug-prime, somehow make those surfaces into tasks
|
||||
|
||||
#[embassy_executor::task]
|
||||
pub async fn render(rmt: esp_hal::peripherals::RMT<'static>, gpio: AnyPin<'static>, surfaces: BufferedSurfacePool<Uniforms, BikeSpace, Rgba<u8>>, controls: &'static DisplayControls) {
|
||||
pub async fn render(rmt: esp_hal::peripherals::RMT<'static>, gpio: AnyPin<'static>, surfaces: BufferedSurfacePool<Uniforms, SegmentSpace, Rgba<u8>>, controls: Arc<DisplayControls>, mut wdt: Wdt<esp_hal::peripherals::TIMG0<'static>>) {
|
||||
let frequency: Rate = Rate::from_mhz(80);
|
||||
let rmt = Rmt::new(rmt, frequency)
|
||||
.expect("Failed to initialize RMT").into_async();
|
||||
@@ -33,7 +30,8 @@ pub async fn render(rmt: esp_hal::peripherals::RMT<'static>, gpio: AnyPin<'stati
|
||||
|
||||
// You probably don't need to change these values, unless your LED strip is somehow not 5 volts
|
||||
const POWER_VOLTS : u32 = 5;
|
||||
const MAX_POWER_MW : u32 = POWER_VOLTS * POWER_MA;
|
||||
|
||||
const MAX_POWER_MW : u32 = if cfg!(feature="max-usb-power") { u32::MAX } else { POWER_VOLTS * POWER_MA };
|
||||
|
||||
// This value is used as the 'seed' for rendering each frame, allowing us to do things like run the animation backwards, frames for double FPS, or even use system uptime for more human-paced animations
|
||||
let mut uniforms = Uniforms {
|
||||
@@ -41,29 +39,42 @@ pub async fn render(rmt: esp_hal::peripherals::RMT<'static>, gpio: AnyPin<'stati
|
||||
..Uniforms::default()
|
||||
};
|
||||
|
||||
let mut output = BikeOutputAsync::new(target, MAX_POWER_MW);
|
||||
let mut output = BikeOutput::new(target, MAX_POWER_MW, controls.clone());
|
||||
output.set_gamma(GammaCurve::new(1.3));
|
||||
//#[cfg(not(feature="wokwi"))]
|
||||
//output.set_gamma(GammaCurve::new(2.1));
|
||||
|
||||
info!("Rendering started! {}ms since boot", Instant::now().as_millis());
|
||||
controls.render_is_running.signal(true);
|
||||
|
||||
const FPS: u64 = 60;
|
||||
const FPS: u64 = 80;
|
||||
const RENDER_BUDGET: Duration = Duration::from_millis(1000 / FPS);
|
||||
|
||||
loop {
|
||||
// FIXME: need to put the rendering loop into a deep sleep when the display is off
|
||||
let start = Instant::now();
|
||||
output.blank_async().await.expect("Failed to blank framebuf");
|
||||
|
||||
output.blank();
|
||||
|
||||
surfaces.render_to(&mut output, &uniforms);
|
||||
|
||||
// Apply hardware controls
|
||||
output.set_on(controls.on.load(core::sync::atomic::Ordering::Relaxed));
|
||||
output.set_brightness(controls.brightness.load(core::sync::atomic::Ordering::Relaxed));
|
||||
|
||||
// Finally, write out the rendered frame
|
||||
output.commit_async().await.expect("Failed to commit frame");
|
||||
|
||||
let render_duration = Instant::now() - start;
|
||||
|
||||
if !controls.is_on() {
|
||||
warn!("Renderer is sleeping zzzz");
|
||||
//controls.render_is_running.signal(false);
|
||||
output.blank();
|
||||
wdt.disable();
|
||||
controls.wait_for_on().await;
|
||||
wdt.feed();
|
||||
wdt.enable();
|
||||
warn!("Renderer is awake !!!!");
|
||||
//controls.render_is_running.signal(true);
|
||||
}
|
||||
|
||||
if render_duration < RENDER_BUDGET {
|
||||
let remaining_budget = RENDER_BUDGET - render_duration;
|
||||
uniforms.frame += 1;
|
||||
@@ -74,5 +85,9 @@ pub async fn render(rmt: esp_hal::peripherals::RMT<'static>, gpio: AnyPin<'stati
|
||||
// If we took longer than the budget, we need to drop some frames to catch up
|
||||
uniforms.frame += dropped_count;
|
||||
}
|
||||
|
||||
uniforms.primary_color.hue = uniforms.primary_color.hue.wrapping_add(1);
|
||||
|
||||
wdt.feed();
|
||||
}
|
||||
}
|
||||
@@ -1,160 +1,63 @@
|
||||
use embassy_sync::channel::DynamicSender;
|
||||
use embassy_time::{Duration, Timer};
|
||||
use nalgebra::{Vector2, Vector3};
|
||||
use csv_core::{ReadFieldResult, Reader};
|
||||
use log::*;
|
||||
|
||||
use crate::events::Measurement;
|
||||
|
||||
const ACCELEROMETER_DATA: &str = include_str!("../../test-data/accelerometer.csv");
|
||||
//const GYRO_DATA: &str = include_str!("../../test-data/gyro.csv");
|
||||
const GPS_DATA: &str = include_str!("../../test-data/gps.csv");
|
||||
|
||||
struct SimDataReader<'a> {
|
||||
reader: Reader,
|
||||
buf: &'a str,
|
||||
read_offset: usize
|
||||
}
|
||||
|
||||
impl<'a> SimDataReader<'a> {
|
||||
pub fn new(buf: &'a str) -> Self {
|
||||
Self {
|
||||
buf,
|
||||
read_offset: 0,
|
||||
reader: Reader::new()
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl<'a> Iterator for SimDataReader<'a> {
|
||||
type Item = &'a str;
|
||||
|
||||
fn next(&mut self) -> Option<Self::Item> {
|
||||
let mut out_buf = [0u8; 128];
|
||||
match self.reader.read_field(&self.buf.as_bytes()[self.read_offset..(self.read_offset + 128).min(self.buf.len())], &mut out_buf) {
|
||||
(ReadFieldResult::Field { record_end: _ }, read_size, write_size) => {
|
||||
let start = self.read_offset;
|
||||
self.read_offset += read_size;
|
||||
Some(&self.buf[start..(start + write_size)])
|
||||
},
|
||||
(ReadFieldResult::End, _, _) => None,
|
||||
err => panic!("{:?}", err)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Default, Debug, Clone, Copy)]
|
||||
enum MotionFields {
|
||||
#[default]
|
||||
Time,
|
||||
SecondsElapsed,
|
||||
Z,
|
||||
Y,
|
||||
X,
|
||||
End
|
||||
}
|
||||
|
||||
impl MotionFields {
|
||||
pub fn next(self) -> Self {
|
||||
match self {
|
||||
Self::Time => Self::SecondsElapsed,
|
||||
Self::SecondsElapsed => Self::Z,
|
||||
Self::Z => Self::Y,
|
||||
Self::Y => Self::X,
|
||||
Self::X => Self::End,
|
||||
Self::End => panic!("No more fields")
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#[derive(Default, Debug, Clone, Copy)]
|
||||
enum GpsFields {
|
||||
#[default]
|
||||
Time,
|
||||
SecondsElapsed,
|
||||
BearingAcc,
|
||||
SpeedAcc,
|
||||
VertAcc,
|
||||
HorizAcc,
|
||||
Speed,
|
||||
Bearing,
|
||||
Altitude,
|
||||
Longitude,
|
||||
Latitude,
|
||||
End
|
||||
}
|
||||
|
||||
impl GpsFields {
|
||||
pub fn next(self) -> Self {
|
||||
match self {
|
||||
Self::Time => Self::SecondsElapsed,
|
||||
Self::SecondsElapsed => Self::BearingAcc,
|
||||
Self::BearingAcc => Self::SpeedAcc,
|
||||
Self::SpeedAcc => Self::VertAcc,
|
||||
Self::VertAcc => Self::HorizAcc,
|
||||
Self::HorizAcc => Self::Speed,
|
||||
Self::Speed => Self::Bearing,
|
||||
Self::Bearing => Self::Altitude,
|
||||
Self::Altitude => Self::Longitude,
|
||||
Self::Longitude => Self::Latitude,
|
||||
Self::Latitude => Self::End,
|
||||
Self::End => panic!("No more fields")
|
||||
}
|
||||
}
|
||||
}
|
||||
use crate::events::{Measurement, SensorSource};
|
||||
use crate::Breaker;
|
||||
|
||||
#[embassy_executor::task]
|
||||
pub async fn motion_simulation_task(events: DynamicSender<'static, Measurement>) {
|
||||
let mut accel = Vector3::default();
|
||||
let mut cur_field = MotionFields::default();
|
||||
let mut last_stamp = 0.0;
|
||||
let mut cur_delta = Duration::from_ticks(0);
|
||||
let reader = SimDataReader::new(ACCELEROMETER_DATA);
|
||||
for field in reader {
|
||||
match cur_field {
|
||||
MotionFields::SecondsElapsed => {
|
||||
let secs = field.parse::<f64>().unwrap();
|
||||
cur_delta = Duration::from_millis(((secs - last_stamp) * 1000.0) as u64);
|
||||
last_stamp = secs;
|
||||
}
|
||||
MotionFields::X => accel.x = field.parse::<f32>().unwrap(),
|
||||
MotionFields::Y => accel.y = field.parse().unwrap(),
|
||||
MotionFields::Z => accel.z = field.parse().unwrap(),
|
||||
_ => ()
|
||||
}
|
||||
cur_field = cur_field.next();
|
||||
if let MotionFields::End = cur_field {
|
||||
cur_field = MotionFields::default();
|
||||
events.send(Measurement::IMU{ accel, gyro: Vector3::default() }).await;
|
||||
Timer::after(cur_delta).await;
|
||||
let mut rd = rmp::decode::Bytes::new(include_bytes!("../../test-data/motion.msgpack"));
|
||||
let mut runtime_secs = Breaker::default();
|
||||
let mut runtime = Duration::default();
|
||||
events.send(Measurement::SensorOnline(SensorSource::IMU)).await;
|
||||
while let Ok(size) = rmp::decode::read_array_len(&mut rd) {
|
||||
assert_eq!(size, 7, "Expected 7 fields, but only found {size}");
|
||||
let delay = embassy_time::Duration::from_millis((rmp::decode::read_f64(&mut rd).unwrap() * 1000.0) as u64);
|
||||
let accel = Vector3::new(
|
||||
rmp::decode::read_f64(&mut rd).unwrap() as f32,
|
||||
rmp::decode::read_f64(&mut rd).unwrap() as f32,
|
||||
rmp::decode::read_f64(&mut rd).unwrap() as f32,
|
||||
);
|
||||
let gyro = Vector3::new(
|
||||
rmp::decode::read_f64(&mut rd).unwrap() as f32,
|
||||
rmp::decode::read_f64(&mut rd).unwrap() as f32,
|
||||
rmp::decode::read_f64(&mut rd).unwrap() as f32,
|
||||
);
|
||||
runtime += delay;
|
||||
runtime_secs.set(runtime.as_secs());
|
||||
if runtime_secs.read_tripped().is_some() {
|
||||
events.send(Measurement::SimulationProgress(SensorSource::IMU, runtime, 0.0)).await;
|
||||
}
|
||||
Timer::after(delay).await;
|
||||
events.send(Measurement::IMU{ accel, gyro }).await;
|
||||
}
|
||||
events.send(Measurement::SensorOffline(SensorSource::IMU)).await;
|
||||
warn!("End of motion recording");
|
||||
}
|
||||
|
||||
#[embassy_executor::task]
|
||||
pub async fn location_simulation_task(events: DynamicSender<'static, Measurement>) {
|
||||
let mut coords = Vector2::default();
|
||||
let mut cur_field = GpsFields::default();
|
||||
let mut last_stamp = 0.0;
|
||||
let mut cur_delta = Duration::from_ticks(0);
|
||||
let reader = SimDataReader::new(GPS_DATA);
|
||||
for field in reader {
|
||||
match cur_field {
|
||||
GpsFields::SecondsElapsed => {
|
||||
let secs = field.parse::<f64>().unwrap();
|
||||
cur_delta = Duration::from_millis(((secs - last_stamp) * 1000.0) as u64);
|
||||
last_stamp = secs;
|
||||
}
|
||||
GpsFields::Latitude => coords.x = field.parse::<f64>().unwrap(),
|
||||
GpsFields::Longitude => coords.y = field.parse().unwrap(),
|
||||
_ => ()
|
||||
}
|
||||
|
||||
cur_field = cur_field.next();
|
||||
if let GpsFields::End = cur_field {
|
||||
cur_field = GpsFields::default();
|
||||
Timer::after(cur_delta).await;
|
||||
events.send(Measurement::GPS(Some(coords))).await;
|
||||
let mut rd = rmp::decode::Bytes::new(include_bytes!("../../test-data/gps.msgpack"));
|
||||
let mut runtime_secs = Breaker::default();
|
||||
let mut runtime = Duration::default();
|
||||
events.send(Measurement::SensorOnline(SensorSource::GPS)).await;
|
||||
while let Ok(size) = rmp::decode::read_array_len(&mut rd) {
|
||||
assert_eq!(size, 3, "Expected 3 fields, but only found {size}");
|
||||
let delay = embassy_time::Duration::from_millis((rmp::decode::read_f64(&mut rd).unwrap() * 1000.0) as u64);
|
||||
let coords = Vector2::new(
|
||||
rmp::decode::read_f64(&mut rd).unwrap(),
|
||||
rmp::decode::read_f64(&mut rd).unwrap()
|
||||
);
|
||||
runtime += delay;
|
||||
runtime_secs.set(runtime.as_secs());
|
||||
if runtime_secs.read_tripped().is_some() {
|
||||
events.send(Measurement::SimulationProgress(SensorSource::GPS, runtime, 0.0)).await;
|
||||
}
|
||||
Timer::after(delay).await;
|
||||
events.send(Measurement::GPS(Some(coords))).await;
|
||||
}
|
||||
events.send(Measurement::SensorOffline(SensorSource::GPS)).await;
|
||||
warn!("End of GPS recording");
|
||||
}
|
||||
369
src/tasks/ui.rs
369
src/tasks/ui.rs
@@ -1,272 +1,243 @@
|
||||
use embassy_sync::channel::DynamicReceiver;
|
||||
use embassy_time::{Duration, Instant, Timer};
|
||||
use figments::{liber8tion::interpolate::Fract8, prelude::*};
|
||||
use embassy_time::Duration;
|
||||
use figments::prelude::*;
|
||||
use rgb::{Rgb, Rgba};
|
||||
use log::*;
|
||||
use nalgebra::ComplexField;
|
||||
|
||||
use crate::{display::BikeSpace, events::{DisplayControls, Notification, Scene}, shaders::*, tasks::render::Uniforms};
|
||||
use alloc::sync::Arc;
|
||||
use core::fmt::Debug;
|
||||
use futures::join;
|
||||
|
||||
use crate::{animation::{AnimDisplay, AnimatedSurface, Animation}, display::{SegmentSpace, Uniforms}, events::{DisplayControls, Notification, Scene, SensorSource}, shaders::*};
|
||||
|
||||
pub struct Ui<S: Surface> {
|
||||
// Background layer provides an always-running background for everything to draw on
|
||||
background: S,
|
||||
background: AnimatedSurface<S>,
|
||||
|
||||
// Tail and panels provide content
|
||||
tail: S,
|
||||
panels: S,
|
||||
tail: AnimatedSurface<S>,
|
||||
panels: AnimatedSurface<S>,
|
||||
|
||||
motion: AnimatedSurface<S>,
|
||||
|
||||
// Notification layer sits on top of the content, and is used for transient event notifications (gps lost, wifi found, etc)
|
||||
notification:S,
|
||||
notification: AnimatedSurface<S>,
|
||||
|
||||
// Headlight and brakelight layers can only be overpainted by the bootsplash overlay layer
|
||||
headlight: S,
|
||||
brakelight: S,
|
||||
headlight: AnimatedSurface<S>,
|
||||
brakelight: AnimatedSurface<S>,
|
||||
|
||||
// The overlay covers everything and is used to implement a power-on and power-off animation.
|
||||
overlay:S,
|
||||
headlight_on: bool,
|
||||
brakelight_on: bool,
|
||||
display: &'static DisplayControls
|
||||
overlay: AnimatedSurface<S>,
|
||||
display: Arc<DisplayControls>
|
||||
}
|
||||
|
||||
impl<S: Surface<Uniforms = Uniforms, CoordinateSpace = BikeSpace, Pixel = Rgba<u8>>> Ui<S> {
|
||||
pub fn new(surfaces: &mut impl Surfaces<S::CoordinateSpace, Surface = S>, display: &'static DisplayControls) -> Self {
|
||||
impl<S: Debug + Surface<Uniforms = Uniforms, CoordinateSpace = SegmentSpace, Pixel = Rgba<u8>>> Ui<S> {
|
||||
pub fn new<SS: Surfaces<SegmentSpace, Surface = S>>(surfaces: &mut SS, display: Arc<DisplayControls>) -> Self where SS::Error: Debug {
|
||||
Self {
|
||||
background: SurfaceBuilder::build(surfaces)
|
||||
.rect(Rectangle::everything())
|
||||
.shader(Background::default())
|
||||
.opacity(32)
|
||||
.visible(false)
|
||||
.finish().unwrap(),
|
||||
.finish().unwrap().into(),
|
||||
tail: SurfaceBuilder::build(surfaces)
|
||||
.rect(Rectangle::new_from_coordinates(0, 5, 255, 5))
|
||||
.opacity(96)
|
||||
.shader(Tail::default())
|
||||
.visible(false)
|
||||
.finish().unwrap(),
|
||||
.finish().unwrap().into(),
|
||||
panels: SurfaceBuilder::build(surfaces)
|
||||
.rect(Rectangle::new_from_coordinates(0, 1, 255, 4))
|
||||
.opacity(128)
|
||||
.shader(Panel::default())
|
||||
.visible(false)
|
||||
.finish().unwrap(),
|
||||
.finish().unwrap().into(),
|
||||
motion: SurfaceBuilder::build(surfaces)
|
||||
.rect(Rectangle::everything())
|
||||
.shader(Movement::default())
|
||||
.visible(false)
|
||||
.finish().unwrap().into(),
|
||||
notification: SurfaceBuilder::build(surfaces)
|
||||
.rect(Rectangle::everything())
|
||||
.shader(Background::default())
|
||||
.visible(false)
|
||||
.finish().unwrap(),
|
||||
.finish().unwrap().into(),
|
||||
// FIXME: Headlight, brakelight, and the overlay should all be handled as part of the safety UI
|
||||
headlight: SurfaceBuilder::build(surfaces)
|
||||
.rect(Rectangle::new_from_coordinates(0, 0, 255, 0))
|
||||
.shader(Headlight::default())
|
||||
.visible(false)
|
||||
.finish().unwrap(),
|
||||
.finish().unwrap().into(),
|
||||
brakelight: SurfaceBuilder::build(surfaces)
|
||||
.rect(Brakelight::rectangle())
|
||||
.shader(Brakelight::default())
|
||||
.visible(false)
|
||||
.finish().unwrap(),
|
||||
.finish().unwrap().into(),
|
||||
overlay: SurfaceBuilder::build(surfaces)
|
||||
.rect(Rectangle::everything())
|
||||
.visible(true)
|
||||
.opacity(0)
|
||||
.shader(Thinking::default())
|
||||
.finish().unwrap(),
|
||||
headlight_on: false,
|
||||
brakelight_on: false,
|
||||
.visible(false)
|
||||
.finish().unwrap().into(),
|
||||
display
|
||||
}
|
||||
}
|
||||
|
||||
pub async fn flash_notification_color(&mut self, color: Rgb<u8>) {
|
||||
self.notification.set_shader(Background::from_color(color));
|
||||
self.notification.set_opacity(0);
|
||||
let fade_in = Animation::default().from(0).to(255).duration(Duration::from_millis(30));
|
||||
let pulse_out = Animation::default().from(255).to(60).duration(Duration::from_millis(100));
|
||||
let pulse_in = Animation::default().from(0).to(255).duration(Duration::from_millis(100));
|
||||
let fade_out = Animation::default().from(255).to(0).duration(Duration::from_secs(2));
|
||||
info!("Flashing notification {color}");
|
||||
|
||||
self.notification.set_visible(true);
|
||||
// Fade in to full on over 1s
|
||||
Self::animate_duration(Duration::from_secs(1), |pct| {
|
||||
self.notification.set_opacity(pct);
|
||||
}).await;
|
||||
|
||||
// Pulse quickly 3 times
|
||||
for _ in 0..3 {
|
||||
// Brief dimming back to half brightness for half a sec
|
||||
Self::animate_duration(Duration::from_millis(100), |pct| {
|
||||
self.notification.set_opacity(100 + pct/2);
|
||||
}).await;
|
||||
self.notification.set_shader(Background::from_color(color));
|
||||
|
||||
// Back to full
|
||||
Self::animate_duration(Duration::from_millis(100), |pct| {
|
||||
self.notification.set_opacity(100 + (255 - pct) / 2);
|
||||
}).await;
|
||||
fade_in.apply(&mut self.notification).await;
|
||||
|
||||
// Pulse quickly 5 times
|
||||
for _ in 0..5 {
|
||||
pulse_out.apply(&mut self.notification).await;
|
||||
pulse_in.apply(&mut self.notification).await;
|
||||
}
|
||||
|
||||
// Back to off
|
||||
Self::animate_duration(Duration::from_secs(3), |pct| {
|
||||
self.notification.set_opacity(255 - pct);
|
||||
}).await;
|
||||
fade_out.apply(&mut self.notification).await;
|
||||
self.notification.set_visible(false);
|
||||
}
|
||||
|
||||
async fn animate_duration<F: FnMut(Fract8)>(duration: Duration, mut f: F) {
|
||||
let start = Instant::now();
|
||||
let end = start + duration;
|
||||
let full_duration = duration.as_millis() as f64;
|
||||
loop {
|
||||
let now = Instant::now();
|
||||
if now > end {
|
||||
break
|
||||
} else {
|
||||
let pct = (now - start).as_millis() as f64 / full_duration;
|
||||
let frac_pct = (255.0 * pct) as u8;
|
||||
f(frac_pct);
|
||||
Timer::after_millis(5).await;
|
||||
}
|
||||
}
|
||||
pub async fn sleep(&mut self) {
|
||||
info!("Running sleep sequence");
|
||||
let fade_out = Animation::default().duration(Duration::from_secs(1)).from(255).to(0);
|
||||
let mut disp_anim = AnimDisplay(&self.display);
|
||||
fade_out.apply(&mut disp_anim).await;
|
||||
|
||||
// Reset layers to the initial state now that the display should be off
|
||||
[
|
||||
&mut *self.tail,
|
||||
&mut *self.panels,
|
||||
&mut *self.background,
|
||||
&mut *self.motion
|
||||
].set_opacity(0);
|
||||
[
|
||||
&mut *self.tail,
|
||||
&mut *self.panels,
|
||||
&mut *self.background,
|
||||
&mut *self.motion
|
||||
].set_visible(false);
|
||||
|
||||
info!("Turning off display");
|
||||
self.display.set_on(false);
|
||||
// Wait for the display hardware to actually turn off, before we return to process the next event, which could cause funky behaviors.
|
||||
// FIXME: also deadlocks :(
|
||||
//self.display.render_is_running.wait().await;
|
||||
info!("Display is now sleeping.");
|
||||
}
|
||||
|
||||
pub fn expo_in(t: f32) -> f32 {
|
||||
if t <= 0.0 {
|
||||
0.0
|
||||
} else {
|
||||
2f32.powf(10.0 * t - 10.0)
|
||||
}
|
||||
}
|
||||
|
||||
pub fn expo_out(t: f32) -> f32 {
|
||||
if 1.0 <= t {
|
||||
1.0
|
||||
} else {
|
||||
1.0 - 2f32.powf(-10.0 * t)
|
||||
}
|
||||
}
|
||||
|
||||
pub async fn startup_fade_sequence(&mut self) {
|
||||
pub async fn wakeup(&mut self) {
|
||||
info!("Running startup fade sequence");
|
||||
// Start with a completely transparent overlay, which then fades in over 1 second
|
||||
self.overlay.set_opacity(0);
|
||||
|
||||
info!("Turning on display");
|
||||
// Turn on the display hardware
|
||||
self.display.set_brightness(0);
|
||||
self.display.set_on(true);
|
||||
// Wait for the renderer to start running again
|
||||
// FIXME: This deadlocks :(
|
||||
//self.display.render_is_running.wait().await;
|
||||
|
||||
let fade_in = Animation::default().duration(Duration::from_secs(3)).from(0).to(255);
|
||||
let fade_out = Animation::default().duration(Duration::from_secs(1)).from(255).to(0);
|
||||
let mut disp_anim = AnimDisplay(&self.display);
|
||||
info!("Fade in overlay");
|
||||
self.overlay.set_visible(true);
|
||||
Self::animate_duration(Duration::from_secs(2), |pct| {
|
||||
self.overlay.set_opacity((Self::expo_in(pct as f32 / 255.0) * 255.0) as u8);
|
||||
}).await;
|
||||
// When the overlay is fully opaque and all lower layers will be hidden, turn them on
|
||||
self.apply_scene(Scene::Startup).await;
|
||||
Timer::after_secs(1).await;
|
||||
// Then fade out the overlay over 3 seconds, allowing the base animations to be shown
|
||||
Self::animate_duration(Duration::from_secs(1), |pct| {
|
||||
self.overlay.set_opacity((Self::expo_in((255 - pct) as f32 / 255.0) * 255.0) as u8);
|
||||
}).await;
|
||||
join!(
|
||||
fade_in.apply(&mut self.overlay),
|
||||
fade_in.apply(&mut disp_anim)
|
||||
);
|
||||
|
||||
info!("Flipping on surfaces");
|
||||
// Flip on all the layers
|
||||
[
|
||||
&mut *self.panels,
|
||||
&mut *self.tail,
|
||||
&mut *self.background,
|
||||
&mut *self.motion
|
||||
].set_visible(true);
|
||||
|
||||
// Enter the startup scene
|
||||
self.apply_scene(Scene::Ready).await;
|
||||
|
||||
info!("Fade out overlay");
|
||||
fade_out.apply(&mut self.overlay).await;
|
||||
self.overlay.set_visible(false);
|
||||
info!("Fade sequence completed");
|
||||
}
|
||||
|
||||
pub async fn set_headlight_on(&mut self, is_on: bool) {
|
||||
if self.headlight_on != is_on {
|
||||
self.brakelight_on = is_on;
|
||||
if is_on {
|
||||
info!("Turning on headlight");
|
||||
self.headlight.set_opacity(0);
|
||||
self.headlight.set_visible(true);
|
||||
Self::animate_duration(Duration::from_secs(1), |pct| {
|
||||
self.headlight.set_opacity(pct);
|
||||
}).await;
|
||||
self.headlight.set_opacity(255);
|
||||
} else {
|
||||
info!("Turning off headlight");
|
||||
Self::animate_duration(Duration::from_secs(1), |pct| {
|
||||
self.headlight.set_opacity(255 - pct);
|
||||
}).await;
|
||||
self.headlight.set_visible(false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub async fn set_brakelight_on(&mut self, is_on: bool) {
|
||||
if self.brakelight_on != is_on {
|
||||
self.brakelight_on = is_on;
|
||||
if is_on {
|
||||
info!("Turning on brakelight");
|
||||
self.brakelight.set_opacity(0);
|
||||
self.brakelight.set_visible(true);
|
||||
Self::animate_duration(Duration::from_millis(300), |pct| {
|
||||
self.brakelight.set_opacity(pct);
|
||||
}).await;
|
||||
self.brakelight.set_opacity(255);
|
||||
} else {
|
||||
info!("Turning off brakelight");
|
||||
Self::animate_duration(Duration::from_millis(300), |pct| {
|
||||
self.brakelight.set_opacity(255 - pct);
|
||||
}).await;
|
||||
self.brakelight.set_visible(false);
|
||||
}
|
||||
}
|
||||
info!("Wakeup complete!");
|
||||
}
|
||||
|
||||
// TODO: Brakelight should only be toggled when actually braking or stationary
|
||||
pub async fn apply_scene(&mut self, next_scene: Scene) {
|
||||
info!("Activating scene {next_scene:?}");
|
||||
match next_scene {
|
||||
Scene::Startup => {
|
||||
self.display.on.store(true, core::sync::atomic::Ordering::Relaxed);
|
||||
self.display.brightness.store(255, core::sync::atomic::Ordering::Relaxed);
|
||||
self.background.set_visible(true);
|
||||
self.tail.set_visible(true);
|
||||
self.panels.set_visible(true);
|
||||
Scene::Ready => {
|
||||
let tail = Animation::default().duration(Duration::from_millis(300)).to(96);
|
||||
let panels = Animation::default().duration(Duration::from_millis(300)).to(128);
|
||||
let bg = Animation::default().duration(Duration::from_millis(300)).to(32);
|
||||
let motion = Animation::default().duration(Duration::from_secs(1)).to(0);
|
||||
join!(
|
||||
tail.apply(&mut self.tail),
|
||||
panels.apply(&mut self.panels),
|
||||
bg.apply(&mut self.background),
|
||||
motion.apply(&mut self.motion)
|
||||
);
|
||||
self.background.set_shader(Background::default());
|
||||
},
|
||||
Scene::ParkedIdle => {
|
||||
// Ensure the display is on
|
||||
self.display.on.store(true, core::sync::atomic::Ordering::Relaxed);
|
||||
Self::animate_duration(Duration::from_secs(1), |pct| {
|
||||
self.display.brightness.store(128.scale8(255 - pct), core::sync::atomic::Ordering::Relaxed);
|
||||
}).await;
|
||||
// Hide the content; only notifications will remain
|
||||
self.background.set_visible(true);
|
||||
self.tail.set_visible(false);
|
||||
self.panels.set_visible(false);
|
||||
},
|
||||
Scene::ParkedLongTerm => {
|
||||
// For long-term parking, we turn off the safety lights
|
||||
self.set_headlight_on(false).await;
|
||||
self.set_brakelight_on(false).await;
|
||||
// Then we turn the display off completely
|
||||
Self::animate_duration(Duration::from_secs(1), |pct| {
|
||||
self.display.brightness.store(255 - pct, core::sync::atomic::Ordering::Relaxed);
|
||||
}).await;
|
||||
self.display.on.store(false, core::sync::atomic::Ordering::Relaxed);
|
||||
Scene::Idle => {
|
||||
// FIXME: The safety UI task should handle setting the display brightness to 50% here
|
||||
self.background.set_shader(Thinking::default());
|
||||
let fg_fade = Animation::default().duration(Duration::from_millis(300)).to(0);
|
||||
let bg_fade = Animation::default().duration(Duration::from_millis(300)).to(128);
|
||||
|
||||
// FIXME: The scenes shouldn't be touching the brake/headlights at all here. In fact, they should be dealt with in a whole separate task from the main UI, maybe running on the motion prediction executor
|
||||
join!(
|
||||
fg_fade.apply(&mut self.tail),
|
||||
fg_fade.apply(&mut self.panels),
|
||||
bg_fade.apply(&mut self.background),
|
||||
fg_fade.apply(&mut self.motion)
|
||||
);
|
||||
},
|
||||
Scene::Accelerating => {
|
||||
self.display.on.store(true, core::sync::atomic::Ordering::Relaxed);
|
||||
self.display.brightness.store(255, core::sync::atomic::Ordering::Relaxed);
|
||||
self.tail.set_shader(Thinking::default());
|
||||
self.panels.set_shader(Panel::default());
|
||||
self.motion.set_shader(Movement::default());
|
||||
Animation::default().duration(Duration::from_secs(1)).to(255).apply(&mut self.motion).await;
|
||||
},
|
||||
Scene::Decelerating => {
|
||||
self.display.on.store(true, core::sync::atomic::Ordering::Relaxed);
|
||||
self.display.brightness.store(255, core::sync::atomic::Ordering::Relaxed);
|
||||
self.panels.set_shader(Thinking::default());
|
||||
self.tail.set_shader(Tail::default());
|
||||
}
|
||||
_ => {
|
||||
warn!("Unimplemented scene {next_scene:?}!");
|
||||
self.motion.set_shader(Movement::default().reversed());
|
||||
Animation::default().duration(Duration::from_secs(1)).to(255).apply(&mut self.motion).await;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub async fn on_event(&mut self, event: Notification) {
|
||||
info!("UI Event: {event:?}");
|
||||
match event {
|
||||
// Apply the scene when it is changed
|
||||
Notification::SceneChange(_) => (), // We already log this inside apply_scene()
|
||||
evt => info!("UI event: {evt:?}")
|
||||
}
|
||||
match event {
|
||||
// TODO: We probably also want some events to indicate when the ESP has no calibration data or otherwise needs re-calibrated and is waiting for the bike to stand still
|
||||
Notification::SensorOnline(SensorSource::IMU) => self.flash_notification_color(Rgb::new(0, 255, 0)).await,
|
||||
Notification::SensorOffline(SensorSource::GPS) => self.flash_notification_color(Rgb::new(255, 0, 0)).await,
|
||||
Notification::SensorOnline(SensorSource::GPS) => self.flash_notification_color(Rgb::new(0, 255, 255)).await,
|
||||
|
||||
// Scene change
|
||||
Notification::SceneChange(scene) => self.apply_scene(scene).await,
|
||||
|
||||
// TODO: We probably also want some events to indicate when the ESP has no calibration data or otherwise needs re-calibrated and is waiting for the bike to stand still
|
||||
Notification::CalibrationComplete => self.flash_notification_color(Rgb::new(0, 255, 0)).await,
|
||||
Notification::GPSLost => self.flash_notification_color(Rgb::new(255, 0, 0)).await,
|
||||
Notification::GPSAcquired => self.flash_notification_color(Rgb::new(0, 255, 255)).await,
|
||||
Notification::WifiConnected => self.flash_notification_color(Rgb::new(0, 0, 255)).await,
|
||||
Notification::WifiDisconnected => self.flash_notification_color(Rgb::new(128, 0, 255)).await,
|
||||
|
||||
// Toggling head and brake lights
|
||||
Notification::SetBrakelight(is_on) => self.set_brakelight_on(is_on).await,
|
||||
Notification::SetHeadlight(is_on) => self.set_headlight_on(is_on).await
|
||||
Notification::SetBrakelight(is_on) => self.brakelight.set_on(is_on).await,
|
||||
Notification::SetHeadlight(is_on) => self.headlight.set_on(is_on).await,
|
||||
|
||||
Notification::Sleep => self.sleep().await,
|
||||
Notification::WakeUp => self.wakeup().await,
|
||||
|
||||
Notification::SensorsOffline => {
|
||||
self.flash_notification_color(Rgb::new(255, 0, 0)).await;
|
||||
self.flash_notification_color(Rgb::new(0, 255, 0)).await;
|
||||
self.flash_notification_color(Rgb::new(0, 0, 255)).await;
|
||||
}
|
||||
_ => ()
|
||||
|
||||
// Other event ideas:
|
||||
// - Bike has crashed, or is laid down
|
||||
@@ -281,23 +252,23 @@ impl<S: Surface<Uniforms = Uniforms, CoordinateSpace = BikeSpace, Pixel = Rgba<u
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(feature="headless")]
|
||||
pub type UiSurfacePool = NullBufferPool<NullSurface<Uniforms, SegmentSpace, Rgba<u8>>>;
|
||||
#[cfg(not(feature="headless"))]
|
||||
pub type UiSurfacePool = BufferedSurfacePool<Uniforms, SegmentSpace, Rgba<u8>>;
|
||||
|
||||
#[embassy_executor::task]
|
||||
pub async fn ui_main(events: DynamicReceiver<'static, Notification>, mut ui: Ui<BufferedSurface<Uniforms, BikeSpace, Rgba<u8>>>) {
|
||||
pub async fn ui_main(events: DynamicReceiver<'static, Notification>, mut ui: Ui<<UiSurfacePool as Surfaces<SegmentSpace>>::Surface>) {
|
||||
// Wait for the renderer to start running
|
||||
ui.display.render_is_running.wait().await;
|
||||
//ui.display.render_is_running.wait().await;
|
||||
|
||||
// Run the fade sequence
|
||||
ui.startup_fade_sequence().await;
|
||||
// Run the wake sequence, and turn on the lights
|
||||
ui.wakeup().await;
|
||||
// FIXME: Moving the safety lights into another task will let us turn them both on in parallel with the wakeup sequence
|
||||
ui.headlight.set_on(true).await;
|
||||
ui.brakelight.set_on(true).await;
|
||||
|
||||
// Briefly turn on the brakelight while the headlight turns on
|
||||
ui.set_brakelight_on(true).await;
|
||||
// Turn on the headlight when we are finished
|
||||
ui.set_headlight_on(true).await;
|
||||
// Turn off the brakelight
|
||||
ui.set_brakelight_on(false).await;
|
||||
|
||||
// Flash white to indicate we are ready
|
||||
ui.flash_notification_color(Rgb::new(255, 255, 255)).await;
|
||||
// Enter the event loop
|
||||
loop {
|
||||
ui.on_event(events.receive().await).await;
|
||||
}
|
||||
|
||||
147
src/tasks/wifi.rs
Normal file
147
src/tasks/wifi.rs
Normal file
@@ -0,0 +1,147 @@
|
||||
use bleps::{ad_structure::{create_advertising_data, AdStructure, BR_EDR_NOT_SUPPORTED, LE_GENERAL_DISCOVERABLE}, attribute_server::{AttributeServer, NotificationData}, gatt, Ble, HciConnector};
|
||||
use embassy_sync::channel::DynamicReceiver;
|
||||
use embassy_time::Timer;
|
||||
use esp_hal::timer::AnyTimer;
|
||||
use esp_wifi::ble::controller::BleConnector;
|
||||
use log::*;
|
||||
|
||||
use crate::events::Notification;
|
||||
|
||||
pub async fn ble_task(notify: DynamicReceiver<'static, Notification>, wifi_init: &esp_wifi::EspWifiController<'_>, bluetooth_device: esp_hal::peripherals::BT<'static>) {
|
||||
info!("Setting up BLE stack");
|
||||
let connector = BleConnector::new(wifi_init, bluetooth_device);
|
||||
let get_millis = || esp_hal::time::Instant::now().duration_since_epoch().as_millis();
|
||||
let hci = HciConnector::new(connector, get_millis);
|
||||
let mut ble = Ble::new(&hci);
|
||||
ble.init().unwrap();
|
||||
ble.cmd_set_le_advertising_parameters().unwrap();
|
||||
ble.cmd_set_le_advertising_data(
|
||||
create_advertising_data(&[
|
||||
AdStructure::Flags(LE_GENERAL_DISCOVERABLE | BR_EDR_NOT_SUPPORTED),
|
||||
AdStructure::ServiceUuids16(&[Uuid::Uuid16(0x0001)]),
|
||||
AdStructure::CompleteLocalName("Renderbug!")
|
||||
])
|
||||
.unwrap()
|
||||
).unwrap();
|
||||
ble.cmd_set_le_advertise_enable(true).unwrap();
|
||||
|
||||
let mut wf1 = |_offset: usize, data: &[u8]| {
|
||||
info!("Read serial data! {data:?}");
|
||||
};
|
||||
|
||||
// Other useful characteristics:
|
||||
// 0x2A67 - Location and speed
|
||||
// 0x2A00 - Device name
|
||||
// 0x2B90 - Device time
|
||||
// Permitted characteristics:
|
||||
// Acceleration
|
||||
// Force
|
||||
// Length
|
||||
// Linear position
|
||||
// Rotational speed
|
||||
// Temperature
|
||||
// Torque
|
||||
// Useful app that logs data: https://github.com/a2ruan/ArduNetApp?tab=readme-ov-file
|
||||
// Requires service 4fafc201-1fb5-459e-8fcc-c5c9c331914b, characteristic beb5483e-36e1-4688-b7f5-ea07361b26a8
|
||||
let s = &b""[..];
|
||||
gatt!([service {
|
||||
uuid: "6E400001-B5A3-F393-E0A9-E50E24DCCA9E", // Nordic UART
|
||||
characteristics: [
|
||||
characteristic {
|
||||
uuid: "6E400003-B5A3-F393-E0A9-E50E24DCCA9E", // TX from device, everything is sent as notifications
|
||||
notify: true,
|
||||
name: "tx",
|
||||
value: s
|
||||
},
|
||||
characteristic {
|
||||
uuid: "6E400002-B5A3-F393-E0A9-E50E24DCCA9E", // RX from phone
|
||||
write: wf1
|
||||
},
|
||||
]
|
||||
}]);
|
||||
let mut rng = bleps::no_rng::NoRng;
|
||||
let mut srv = AttributeServer::new(&mut ble, &mut gatt_attributes, &mut rng);
|
||||
|
||||
info!("BLE running!");
|
||||
// TODO: Somehow need to recreate the attributeserver after disconnecting?
|
||||
loop {
|
||||
let notification = match notify.try_receive() {
|
||||
Err(_) => None,
|
||||
Ok(Notification::Beat) => Some("beat"),
|
||||
//TODO: Should make Telemetry values serde-encodable
|
||||
/*Ok(Telemetry::Measurement(Measurement::IMU { accel, gyro })) => {
|
||||
let json_data = json!({
|
||||
"x": accel.x,
|
||||
"y": accel.y,
|
||||
"z": accel.z,
|
||||
"gx": gyro.x,
|
||||
"gy": gyro.y,
|
||||
"gz": gyro.z
|
||||
});
|
||||
let json_buf = serde_json::to_string(&json_data).unwrap();
|
||||
Some(json_buf)
|
||||
},
|
||||
Ok(Telemetry::Measurement(Measurement::GPS(Some(measurement)))) => {
|
||||
info!("gps telemetry");
|
||||
let json_data = json!({
|
||||
"lat": measurement.x,
|
||||
"lng": measurement.y
|
||||
});
|
||||
let json_buf = serde_json::to_string(&json_data).unwrap();
|
||||
Some(json_buf)
|
||||
},*/
|
||||
_ => None
|
||||
};
|
||||
|
||||
match notification {
|
||||
None => {
|
||||
srv.do_work().unwrap();
|
||||
Timer::after_millis(5).await;
|
||||
},
|
||||
Some(serial_data) => {
|
||||
for chunk in serial_data.as_bytes().chunks(20) {
|
||||
srv.do_work_with_notification(Some(NotificationData::new(tx_handle, chunk))).unwrap();
|
||||
}
|
||||
srv.do_work_with_notification(Some(NotificationData::new(tx_handle, &b"\n"[..]))).unwrap();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// TODO: Wifi task needs to know when there is data to upload, so it only connects when needed.
|
||||
#[embassy_executor::task]
|
||||
pub async fn wireless_task(notify: DynamicReceiver<'static, Notification>, timer: AnyTimer<'static>, rng: esp_hal::peripherals::RNG<'static>, _wifi_device: esp_hal::peripherals::WIFI<'static>, bluetooth_device: esp_hal::peripherals::BT<'static>) {
|
||||
let rng = esp_hal::rng::Rng::new(rng);
|
||||
let wifi_init =
|
||||
esp_wifi::init(timer, rng).expect("Failed to initialize WIFI/BLE controller");
|
||||
|
||||
ble_task(notify, &wifi_init, bluetooth_device).await;
|
||||
|
||||
/*
|
||||
loop {
|
||||
let (mut wifi, _interfaces) = esp_wifi::wifi::new(&wifi_init, wifi_device)
|
||||
.expect("Failed to initialize WIFI controller"); }
|
||||
|
||||
loop {
|
||||
//let results = wifi.scan_n_async(16).await.unwrap();
|
||||
wifi.set_configuration(&esp_wifi::wifi::Configuration::Client(
|
||||
ClientConfiguration {
|
||||
ssid: "The Frequency".to_string(),
|
||||
auth_method: esp_wifi::wifi::AuthMethod::WPA2Personal,
|
||||
password: "thepasswordkenneth".to_string(),
|
||||
..Default::default()
|
||||
}
|
||||
)).unwrap();
|
||||
|
||||
if wifi.connect_async().await.is_ok() {
|
||||
info!("Connected to wifi!");
|
||||
while wifi.is_connected().unwrap() {
|
||||
Timer::after_secs(60).await;
|
||||
}
|
||||
info!("Disconnected.");
|
||||
}
|
||||
Timer::after_secs(30).await;
|
||||
}
|
||||
}
|
||||
*/
|
||||
}
|
||||
4368
src/test.nmea
4368
src/test.nmea
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user