tdfischer/renderbug-bike
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

src: implement simulation data sources

Browse Source
This commit is contained in:
Victoria Fischer
2025-09-24 08:32:34 +02:00
parent 19875f6ae5
commit 0cd2cc94b9
32 changed files with 389785 additions and 284 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
src/backoff.rs
View File

@@ -56,14 +56,14 @@ impl Backoff {
'outer: loop {
for attempt in self.attempts {
match &latest {
Err(_) => {
Err(err) => {
match attempt {
Attempts::Finite(1) => {
warn!("Operation failed on final attempt.");
warn!("Operation failed on final attempt: {err:?}");
break 'outer
}
attempt => {
warn!("Operation failed. Retrying attempt {attempt:?} after {delay}");
warn!("Operation failed: {err:?} Retrying attempt {attempt:?} after {delay}");
Timer::after(delay).await;
delay *= 2;
latest = f().await

212
src/bin/main.rs
View File

@@ -7,32 +7,44 @@
)]
#![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_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
use embassy_executor::Spawner;
use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
use embassy_sync::channel::DynamicReceiver;
use embassy_sync::mutex::Mutex;
use embassy_time::{Instant, Timer};
use esp_backtrace as _;
use esp_hal::i2c::master::{Config, I2c};
use esp_hal::gpio::Pin;
use esp_hal::i2c::master::{Config, I2c};
use esp_hal::interrupt::software::SoftwareInterruptControl;
use esp_hal::system::{AppCoreGuard, CpuControl, Stack};
use esp_hal::timer::AnyTimer;
use esp_hal::Async;
use esp_hal::clock::CpuClock;
use esp_hal::rmt::Rmt;
use esp_hal::time::Rate;
use esp_hal::timer::systimer::SystemTimer;
use esp_hal::timer::timg::TimerGroup;
use esp_hal_embassy::{Executor, InterruptExecutor};
use esp_wifi::ble::controller::BleConnector;
use figments::surface::BufferedSurfacePool;
use log::info;
use renderbug_embassy::events::BusGarage;
use renderbug_embassy::events::{BusGarage, Measurement, Telemetry};
use renderbug_embassy::tasks::simulation::{motion_simulation_task, location_simulation_task};
use renderbug_embassy::tasks::ui::{Ui, ui_main};
use renderbug_embassy::tasks::gps::gps_task;
use renderbug_embassy::tasks::motion::motion_task;
use serde_json::json;
use static_cell::StaticCell;
use renderbug_embassy::{
tasks::mpu::*,
tasks::render::render
};
extern crate alloc;
// This creates a default app-descriptor required by the esp-idf bootloader.
@@ -41,6 +53,8 @@ esp_bootloader_esp_idf::esp_app_desc!();
static I2C_BUS: StaticCell<Mutex<CriticalSectionRawMutex, I2c<'static, Async>>> = StaticCell::new();
static BUS_GARAGE: StaticCell<BusGarage> = StaticCell::new();
static mut CORE2_STACK: Stack<8192> = Stack::new();
static CORE_HANDLE: StaticCell<AppCoreGuard> = StaticCell::new();
#[esp_hal_embassy::main]
async fn main(spawner: Spawner) {
@@ -49,7 +63,7 @@ async fn main(spawner: Spawner) {
let config = esp_hal::Config::default().with_cpu_clock(CpuClock::max());
let peripherals = esp_hal::init(config);
esp_alloc::heap_allocator!(size: 64 * 1024);
esp_alloc::heap_allocator!(size: 128 * 1024);
let timer0 = SystemTimer::new(peripherals.SYSTIMER);
esp_hal_embassy::init([timer0.alarm0, timer0.alarm1, timer0.alarm2]);
@@ -57,31 +71,181 @@ async fn main(spawner: Spawner) {
let garage = BUS_GARAGE.init(Default::default());
info!("Setting up wifi");
let rng = esp_hal::rng::Rng::new(peripherals.RNG);
let timer1 = TimerGroup::new(peripherals.TIMG0);
let wifi_init =
esp_wifi::init(timer1.timer0, rng).expect("Failed to initialize WIFI/BLE controller");
let (mut _wifi_controller, _interfaces) = esp_wifi::wifi::new(&wifi_init, peripherals.WIFI)
.expect("Failed to initialize WIFI controller");
info!("Setting up rendering pipeline");
let mut surfaces = BufferedSurfacePool::default();
let ui = Ui::new(&mut surfaces, &garage.display);
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));
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 i2c_bus = I2C_BUS.init(Mutex::new(i2c));
spawner.must_spawn(mpu_task(garage.motion.sender(), garage.scenes.dyn_sender(), I2cDevice::new(i2c_bus)));
spawner.must_spawn(gps_task(garage.motion.sender(), garage.scenes.dyn_sender(), I2cDevice::new(i2c_bus)));
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)));
// TODO: Only run the motion sim sources with cfg(simulation)
spawner.must_spawn(motion_simulation_task(garage.motion.dyn_sender()));
spawner.must_spawn(location_simulation_task(garage.motion.dyn_sender()));
info!("Launching motion engine");
spawner.must_spawn(motion_task(garage.motion.receiver(), garage.scenes.dyn_sender()));
spawner.must_spawn(motion_task(garage.motion.dyn_receiver(), garage.scenes.dyn_sender(), garage.telemetry.dyn_sender()));
info!("Setting up rendering pipeline");
let mut surfaces = BufferedSurfacePool::default();
let ui = Ui::new(&mut surfaces, &garage.display);
let frequency: Rate = Rate::from_mhz(80);
let rmt = Rmt::new(peripherals.RMT, frequency)
.expect("Failed to initialize RMT").into_async();
let rmt_channel = rmt.channel0;
spawner.must_spawn(render(rmt_channel, peripherals.GPIO5.degrade(), surfaces, &garage.display));
spawner.must_spawn(ui_main(garage.scenes.dyn_receiver(), ui));
info!("Starting communications and UI on 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));
});
}).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?
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;
}
*/
}
}

85
src/display.rs
View File

@@ -1,26 +1,26 @@
use figments::{hardware::{Brightness, Gamma}, mappings::linear::LinearSpace, power::AsMilliwatts, prelude::*, smart_leds::BrightnessWriter};
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 super::{Output};
use figments::hardware::Output;
use smart_leds::SmartLedsWriteAsync;
use smart_leds::{SmartLedsWrite, 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]> {}
pub struct BikeOutput<T: SmartLedsWriteAsync> {
pub struct BikeOutput<T: SmartLedsWrite> {
pixbuf: [T::Color; 178],
writer: BrightnessWriter<T>
}
impl<T: SmartLedsWriteAsync> Debug for BikeOutput<T> {
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> BikeOutput<T> where T::Color: HardwarePixel + Gamma + Fract8Ops, T::Error: core::fmt::Debug {
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 {
Self {
pixbuf: [Default::default(); 178],
@@ -29,7 +29,7 @@ impl<T: SmartLedsWriteAsync> BikeOutput<T> where T::Color: HardwarePixel + Gamma
}
}
impl<T: SmartLedsWriteAsync> Brightness for BikeOutput<T> where T::Color: HardwarePixel + Gamma + Fract8Ops, T::Error: core::fmt::Debug {
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);
}
@@ -43,13 +43,78 @@ impl<T: SmartLedsWriteAsync> Brightness for BikeOutput<T> where T::Color: Hardwa
}
}
impl<'a, T: SmartLedsWriteAsync + '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 {
async fn blank(&mut self) -> Result<(), Self::Error> {
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(())
}
async fn commit(&mut self) -> Result<(), Self::Error> {
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> {
self.writer.write(&self.pixbuf).await
}
@@ -57,7 +122,7 @@ impl<'a, T: SmartLedsWriteAsync + 'a> Output<'a, BikeSpace> for BikeOutput<T> wh
type Error = T::Error;
}
impl<'a, T: SmartLedsWriteAsync> Sample<'a, BikeSpace> for BikeOutput<T> where T::Color: HardwarePixel + Gamma + Fract8Ops, [T::Color; 178]: AsMilliwatts + 'static {
impl<'a, T: SmartLedsWriteAsync> Sample<'a, BikeSpace> for BikeOutputAsync<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)> {

111
src/ego/alignment.rs
View File

@@ -1,56 +1,95 @@
use log::*;
use nalgebra::{Matrix3, Rotation3, Unit, Vector3};
use nalgebra::{Rotation3, Unit, Vector3, ComplexField};
use crate::CircularBuffer;
type Sample = Vector3<f32>; // [ax, ay, az] in m/s^2
#[derive(Debug, Clone, Copy)]
#[derive(Debug, Clone, Copy, Default)]
pub struct SensorAlignment {
/// 3x3 transform matrix: raw -> body
/// includes permutation, sign, and bias subtraction.
pub transform: Rotation3<f32>,
/// bias vector in body frame
pub accel_bias: Vector3<f32>,
/// bias vector in sensor frame
pub bias: Vector3<f32>
}
pub gyro_bias: Vector3<f32>,
#[derive(Debug, Clone, Copy)]
pub enum Direction {
UpZ,
DnZ,
UpX,
DnX,
UpY,
DnY
}
impl From<Direction> for Unit<Vector3<f32>> {
fn from(value: Direction) -> Self {
Unit::new_normalize(match value {
Direction::UpZ => Vector3::z(),
Direction::DnZ => -Vector3::z(),
Direction::UpX => Vector3::x(),
Direction::DnX => -Vector3::x(),
Direction::UpY => Vector3::y(),
Direction::DnY => -Vector3::y(),
})
}
}
impl SensorAlignment {
pub fn apply(&self, raw: &Sample) -> Vector3<f32> {
self.transform * raw - self.accel_bias
self.transform * (raw - self.bias)
}
pub fn apply_gyro(&self, raw: &Sample) -> Vector3<f32> {
self.transform * raw - self.gyro_bias
}
pub fn new(accel_samples: &[Sample], gyro_samples: &[Sample]) -> SensorAlignment {
let avg: Vector3<f32> = accel_samples.iter().sum::<Vector3<f32>>() / (accel_samples.len() as f32);
let down_body = Unit::new_normalize(avg);
let down_world = Unit::new_normalize(Vector3::new(0.0, 0.0, -1.0));
let rot_down = Rotation3::rotation_between(&down_world, &down_body).unwrap_or_else(Rotation3::identity);
let forward_world = Vector3::x();
let forward_projected = (forward_world - forward_world.dot(&down_world) * down_world.into_inner()).normalize();
// Transform forward into the body frame
let forward_body = rot_down * forward_projected;
// Construct a proper body frame basis
let right_body = down_body.cross(&forward_body).normalize();
let forward_body = right_body.cross(&down_body).normalize();
// Construct rotation matrix from basis vectors
let rot = Matrix3::from_columns(&[forward_body, right_body, down_body.into_inner()]);
let transform = Rotation3::from_matrix_unchecked(rot);
let gyro_avg: Vector3<f32> = gyro_samples.iter().sum::<Vector3<f32>>() / (gyro_samples.len() as f32);
info!("down_body={down_body:?} down_world={down_world:?}");
pub fn from_samples(accel_samples: &[Sample], down_direction: Direction) -> SensorAlignment {
let bias: Vector3<f32> = accel_samples.iter().sum::<Vector3<f32>>() / (accel_samples.len() as f32);
// Figure out the unit vector pointing 'down' at rest
let down_body = Unit::new_normalize(bias);
// Create our 'true' down vector
let down_world: Unit<Vector3<f32>> = down_direction.into();//Unit::new_normalize(Vector3::new(0.0, 0.0, -1.0));
// Determine how the axes are rotated between the two
let axis = down_body.cross(&down_world);
let axis_unit = Unit::new_normalize(axis);
let angle = down_body.dot(&down_world).clamp(-1.0, 1.0).acos();
// Create a rotation matrix from the angles
let transform = Rotation3::from_axis_angle(&axis_unit, angle);
Self {
transform,
accel_bias: transform * avg,
gyro_bias: transform * gyro_avg
bias,
}
}
}
#[derive(Debug)]
pub struct DirectionEstimator {
buf: CircularBuffer<Vector3<f32>>,
direction: Direction,
pub alignment: SensorAlignment
}
impl DirectionEstimator {
pub fn new(direction: Direction) -> Self {
Self {
buf: Default::default(),
alignment: Default::default(),
direction,
}
}
}
impl DirectionEstimator {
pub fn update(&mut self, accel: Vector3<f32>) {
self.buf.insert(accel);
if self.buf.next_index == 0 {
self.alignment = SensorAlignment::from_samples(&self.buf.data, self.direction);
}
}
pub fn apply(&self, vec: &Vector3<f32>) -> Vector3<f32> {
self.alignment.apply(vec)
}
}

87
src/ego/kalman.rs Normal file
View File

@@ -0,0 +1,87 @@
#![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};
/// 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
}
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
}
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;
}
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;
}
}
// Used to indicate stationary behavior and therefore velocities should be zero
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
let R = SMatrix::<f32, 2, 2>::identity() * 0.01; // very confident
let y = z - H * self.x;
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;
}
}
pub fn state(&self) -> &State {
&self.x
}
}

3
src/ego/mod.rs
View File

@@ -1,3 +1,4 @@
pub mod alignment;
pub mod tilt;
pub mod heading;
pub mod heading;
pub mod kalman;

59
src/events.rs
View File

@@ -1,7 +1,8 @@
use core::sync::atomic::{AtomicBool, AtomicU8};
use embassy_sync::{blocking_mutex::raw::{CriticalSectionRawMutex, NoopRawMutex}, channel::Channel};
use embassy_sync::{blocking_mutex::raw::{CriticalSectionRawMutex, NoopRawMutex}, channel::Channel, signal::Signal};
use figments::hardware::Brightness;
use nalgebra::{Vector2, Vector3};
#[derive(Clone, Copy, Default, Debug)]
@@ -18,9 +19,22 @@ pub enum Scene {
#[derive(Clone, Copy, Debug)]
pub enum Measurement {
// GPS coordinates
GPS(Vector2<f64>),
GPS(Option<Vector2<f64>>),
// Accelerometer values in body frame where x=forwards
IMU(Vector3<f32>)
IMU { accel: Vector3<f32>, gyro: Vector3<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)
}
#[derive(Clone, Copy, Debug)]
@@ -35,25 +49,53 @@ pub enum Notification {
SetBrakelight(bool)
}
#[derive(Debug)]
// TODO: Make this clone() able, so multiple threads can point to the same underlying atomics for the hardware controls
pub struct DisplayControls {
pub on: AtomicBool,
pub brightness: AtomicU8
pub brightness: AtomicU8,
pub render_is_running: Signal<CriticalSectionRawMutex, bool>
}
impl Brightness for DisplayControls {
fn set_brightness(&mut 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);
}
// TODO: Split gamma out from brightness controls
fn set_gamma(&mut self, _gamma: f32) {
unimplemented!()
}
}
impl core::fmt::Debug for DisplayControls {
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> Result<(), core::fmt::Error> {
f.debug_struct("DisplayControls")
.field("on", &self.on)
.field("brightness", &self.brightness)
.field("render_is_running", &self.render_is_running.signaled())
.finish()
}
}
impl Default for DisplayControls {
fn default() -> Self {
Self {
on: AtomicBool::new(true),
brightness: AtomicU8::new(255)
brightness: AtomicU8::new(255),
render_is_running: Signal::new()
}
}
}
#[derive(Debug)]
pub struct BusGarage {
pub motion: Channel<NoopRawMutex, Measurement, 4>,
pub scenes: Channel<CriticalSectionRawMutex, Notification, 4>,
pub motion: Channel<NoopRawMutex, Measurement, 5>,
pub scenes: Channel<CriticalSectionRawMutex, Notification, 5>,
pub telemetry: Channel<CriticalSectionRawMutex, Telemetry, 15>,
pub display: DisplayControls
}
@@ -62,6 +104,7 @@ impl Default for BusGarage {
Self {
motion: Channel::new(),
scenes: Channel::new(),
telemetry: Channel::new(),
display: Default::default()
}
}

26
src/lib.rs
View File

@@ -35,4 +35,30 @@ pub fn as_milliwatts(pixel: &Rgb<u8>) -> u32 {
let blue = (pixel.b as u32 * BLUE_MW).wrapping_shr(8);
red + green + blue + DARK_MW
}
#[derive(Debug)]
pub struct CircularBuffer<T, const SIZE: usize = 20> {
pub data: [T; SIZE],
next_index: usize
}
impl<T: Default + Copy, const SIZE: usize> Default for CircularBuffer<T, SIZE> {
fn default() -> Self {
Self {
data: [Default::default(); SIZE],
next_index: 0
}
}
}
impl<T, const SIZE: usize> CircularBuffer<T, SIZE> {
pub fn insert(&mut self, value: T) {
self.data[self.next_index] = value;
self.next_index += 1;
if self.next_index == self.data.len() {
self.next_index = 0;
}
}
}

5
src/shaders.rs
View File

@@ -37,9 +37,9 @@ impl Shader<Uniforms, BikeSpace, Rgba<u8>> for Background {
pub struct Tail {}
impl Shader<Uniforms, BikeSpace, Rgba<u8>> for Tail {
fn draw(&self, coords: &Coordinates<BikeSpace>, uniforms: &Uniforms) -> Rgba<u8> {
let hue_offset = 0u8.lerp8by8(16, sin8(uniforms.frame.wrapping_sub(coords.x))) as i8 - 8;
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(hue_offset.wrapping_sub(uniforms.primary_color.hue as i8) as u8, max(210, sin8(uniforms.frame.wrapping_add(coords.x))), value).into()
Hsv::new(uniforms.primary_color.hue.wrapping_sub(hue_offset), max(210, sin8(uniforms.frame.wrapping_add(coords.x))), value).into()
}
}
@@ -122,4 +122,3 @@ impl Shader<Uniforms, BikeSpace, Rgba<u8>> for Thinking {
).into()
}
}

11
src/tasks/gps.rs
View File

@@ -1,6 +1,6 @@
use alloc::string::String;
use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
use embassy_sync::{blocking_mutex::raw::{CriticalSectionRawMutex, NoopRawMutex}, channel::{DynamicSender, Sender}};
use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::DynamicSender};
use embassy_time::Timer;
use embedded_hal_async::i2c::I2c as _;
use esp_hal::{i2c::master::I2c, Async};
@@ -8,13 +8,13 @@ use log::*;
use nalgebra::Vector2;
use nmea::Nmea;
use crate::{backoff::Backoff, events::{Measurement, Notification}};
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");
#[embassy_executor::task]
pub async fn gps_task(events: Sender<'static, NoopRawMutex, Measurement, 4>, sink: DynamicSender<'static, Notification>, mut i2c_bus: I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>) {
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 || {
Backoff::from_secs(5).forever().attempt(async || {
info!("Initializing GPS");
@@ -50,20 +50,19 @@ pub async fn gps_task(events: Sender<'static, NoopRawMutex, Measurement, 4>, sin
if let Ok(result) = parser.parse(&strbuf) {
match parser.fix_type {
None if has_lock => {
sink.send(Notification::GPSLost).await;
events.send(Measurement::GPS(None)).await;
has_lock = false
},
None => (),
Some(_) => {
if !has_lock {
sink.send(Notification::GPSAcquired).await;
has_lock = true;
}
//TODO: 4 satellites seems to be "Some" fix, 6 is a perfect fix
//TODO: Only send updates when we get the correct nmea sentence
if let (Some(lat), Some(lng)) = (parser.latitude, parser.longitude) {
events.send(Measurement::GPS(Vector2::new(lat, lng))).await;
events.send(Measurement::GPS(Some(Vector2::new(lat, lng)))).await;
}
}
}

3
src/tasks/mod.rs
View File

@@ -2,4 +2,5 @@ pub mod mpu;
pub mod render;
pub mod motion;
pub mod gps;
pub mod ui;
pub mod ui;
pub mod simulation;

250
src/tasks/motion.rs
View File

@@ -1,98 +1,111 @@
#![allow(non_snake_case)] // WIP, because most of this code came from chatgpt and I'm still not sure if it actually works or is completely wrong
use embassy_sync::{blocking_mutex::raw::NoopRawMutex, channel::{DynamicSender, Receiver}};
use nalgebra::{SMatrix, SVector};
use embassy_sync::channel::{DynamicReceiver, DynamicSender};
use nalgebra::{Rotation3, Vector2, Vector3, ComplexField};
use crate::{ego::heading::HeadingEstimator, events::{Measurement, Notification}};
use crate::{ego::{alignment::{Direction, DirectionEstimator, SensorAlignment}, heading::HeadingEstimator, kalman::{GpsMeasurement, Kalman}}, events::{Measurement, Notification, Prediction, Telemetry}, CircularBuffer};
use log::*;
use nalgebra::ComplexField;
/// State: [px, py, vx, vy]
type State = SVector<f32, 6>;
type Control = SVector<f32, 3>; // dx, dx, yaw
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
}
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() * 5.0,
R: SMatrix::identity() * 5.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
}
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;
}
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;
}
}
// Used to indicate stationary behavior and therefore velocities should be zero
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
let R = SMatrix::<f32, 2, 2>::identity() * 0.01; // very confident
let y = z - H * self.x;
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;
}
}
pub fn state(&self) -> &State {
&self.x
}
}
#[embassy_executor::task]
pub async fn motion_task(src: Receiver<'static, NoopRawMutex, Measurement, 4>, sink: DynamicSender<'static, Notification>) {
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);
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);
loop {
let next_measurement = src.receive().await;
let _ = telemetry_sink.try_send(Telemetry::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();
}
}
},
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;
}
}
},
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);
@@ -102,58 +115,89 @@ pub async fn motion_task(src: Receiver<'static, NoopRawMutex, Measurement, 4>, s
//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(imu_accel) => {
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(imu_accel.z, 0.01);
heading.update(accel.z, 0.01);
//info!("{}", imu_accel.xy().magnitude());
if imu_accel.xy().magnitude() >= 0.2 {
kf.predict(&imu_accel);
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 imu_accel.x > 1.0 {
sink.send(Notification::SceneChange(crate::events::Scene::Accelerating)).await;
} else if imu_accel.x < -1.0 {
sink.send(Notification::SceneChange(crate::events::Scene::Decelerating)).await;
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:?}");
//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));
//last_fix = Some(gps_pos);
}
}
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() + est[3].abs() > 0f32 {
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;
}
}
}
}*/

52
src/tasks/mpu.rs
View File

@@ -1,16 +1,15 @@
use core::cell::RefCell;
use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
use embassy_sync::{blocking_mutex::raw::{CriticalSectionRawMutex, NoopRawMutex}, channel::{DynamicSender, Sender}};
use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::DynamicSender};
use embassy_time::{Delay, Timer};
use esp_hal::{i2c::master::I2c, Async};
use log::{info, warn};
use mpu6050_dmp::{address::Address, calibration::CalibrationParameters, error_async::Error, sensor_async::Mpu6050};
use nalgebra::Vector3;
use core::f32::consts::PI;
use nalgebra::ComplexField;
use crate::{backoff::Backoff, ego::{alignment::SensorAlignment, tilt::TiltEstimator}, events::{Measurement, Notification}};
use crate::{backoff::Backoff, ego::tilt::TiltEstimator, events::Measurement};
const G: f32 = 9.80665;
const SENS_2G: f32 = 16384.0;
@@ -28,7 +27,7 @@ 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
/*/// 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;
@@ -63,10 +62,10 @@ pub fn accel_body_to_enu_lin(body_acc: (f32,f32,f32), roll: f32, pitch: f32, yaw
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: Sender<'static, NoopRawMutex, Measurement, 4>, sink: DynamicSender<'static, Notification>, bus: I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>) {
pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>) {
let backoff = Backoff::from_millis(5);
let busref = RefCell::new(Some(bus));
@@ -92,24 +91,8 @@ pub async fn mpu_task(events: Sender<'static, NoopRawMutex, Measurement, 4>, sin
let sensor_ref = &mut sensor;
let alignment = {
info!("Locating earth...");
let mut accel_samples = [Vector3::default(); 100];
let mut gyro_samples = [Vector3::default(); 100];
for (idx, (accel_sample, gyro_sample)) in accel_samples.iter_mut().zip(gyro_samples.iter_mut()).enumerate() {
let (accel, gyro) = backoff.attempt(async || { sensor_ref.motion6().await }).await.unwrap();
*accel_sample = Vector3::new(accel_raw_to_ms2(accel.x()), accel_raw_to_ms2(accel.y()), accel_raw_to_ms2(accel.z()));
*gyro_sample = Vector3::new(gyro_raw_to_rads(gyro.x()), gyro_raw_to_rads(gyro.y()), gyro_raw_to_rads(gyro.z()));
info!("{idx}/200 {accel_sample:?} {gyro_sample:?}");
Timer::after_millis(5).await;
}
SensorAlignment::new(accel_samples.as_slice(), gyro_samples.as_slice())
};
info!("MPU is ready! earth={alignment:?}");
sink.send(Notification::CalibrationComplete).await;
let mut tilt = TiltEstimator::new(0.3);
info!("MPU is ready!");
let _tilt = TiltEstimator::new(0.3);
//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 {
@@ -121,16 +104,16 @@ pub async fn mpu_task(events: Sender<'static, NoopRawMutex, Measurement, 4>, sin
Ok((accel_data, gyro_data)) => {
// Apply the initial alignment correction to put it into the body frame where X is forward
let adjusted = alignment.apply(&Vector3::new(
let adjusted = Vector3::new(
accel_raw_to_ms2(accel_data.x()),
accel_raw_to_ms2(accel_data.y()),
accel_raw_to_ms2(accel_data.z())
));
let adjusted_gyro = alignment.apply_gyro(&Vector3::new(
);
let adjusted_gyro = Vector3::new(
gyro_raw_to_rads(gyro_data.x()),
gyro_raw_to_rads(gyro_data.y()),
gyro_raw_to_rads(gyro_data.z())
));
);
let filtered = Vector3::new(
lowpass(prev_accel.x, adjusted.x, 0.3),
@@ -140,15 +123,16 @@ pub async fn mpu_task(events: Sender<'static, NoopRawMutex, Measurement, 4>, sin
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));
/*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 (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, adjusted_gyro.z);
let measured = Vector3::new(ax_e, ay_n, az_up);*/
events.send(
Measurement::IMU(
measured
)
Measurement::IMU {
accel: filtered,
gyro: adjusted_gyro
}
).await;
Timer::after_millis(10).await;
},

47
src/tasks/render.rs
View File

@@ -1,11 +1,12 @@
use embassy_time::{Duration, Instant, Timer};
use esp_hal::{gpio::AnyPin, rmt::ChannelCreator, Async};
use esp_hal::{gpio::AnyPin, rmt::Rmt, time::Rate};
use esp_hal_smartled::{buffer_size_async, SmartLedsAdapterAsync};
use figments::{hardware::{Brightness, Output}, prelude::Hsv, surface::{BufferedSurfacePool, Surfaces}};
use figments::{hardware::{Brightness, OutputAsync}, prelude::Hsv, surface::{BufferedSurfacePool, Surfaces}};
use log::{info, warn};
use rgb::Rgba;
use nalgebra::ComplexField;
use crate::{display::{BikeOutput, BikeSpace}, events::DisplayControls};
use crate::{display::{BikeOutputAsync, BikeSpace}, events::DisplayControls};
#[derive(Default)]
pub struct Uniforms {
@@ -16,9 +17,15 @@ pub struct Uniforms {
//TODO: Import the bike surfaces from renderbug-prime, somehow make those surfaces into tasks
#[embassy_executor::task]
pub async fn render(rmt_channel: ChannelCreator<Async, 0>, 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, BikeSpace, Rgba<u8>>, controls: &'static DisplayControls) {
let frequency: Rate = Rate::from_mhz(80);
let rmt = Rmt::new(rmt, frequency)
.expect("Failed to initialize RMT").into_async();
let rmt_channel = rmt.channel0;
let rmt_buffer = [0u32; buffer_size_async(178)];
//let target = SmartLedsAdapterAsync::new(rmt_channel, gpio, rmt_buffer);
let target = SmartLedsAdapterAsync::new(rmt_channel, gpio, rmt_buffer);
// Change this to adjust the power available; the USB spec says 500ma is the standard limit, but sometimes you can draw more from a power brick
@@ -29,16 +36,22 @@ pub async fn render(rmt_channel: ChannelCreator<Async, 0>, gpio: AnyPin<'static>
const MAX_POWER_MW : u32 = 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::default();
let mut uniforms = Uniforms {
primary_color: Hsv::new(210, 255, 255),
..Uniforms::default()
};
let mut output = BikeOutput::new(target, MAX_POWER_MW);
let mut output = BikeOutputAsync::new(target, MAX_POWER_MW);
info!("Rendering started!");
info!("Rendering started! {}ms since boot", Instant::now().as_millis());
controls.render_is_running.signal(true);
const FPS: u64 = 60;
const RENDER_BUDGET: Duration = Duration::from_millis(1000 / FPS);
loop {
let start = Instant::now();
//pixbuf.blank();
output.blank().await.expect("Failed to blank framebuf");
output.blank_async().await.expect("Failed to blank framebuf");
surfaces.render_to(&mut output, &uniforms);
@@ -47,19 +60,19 @@ pub async fn render(rmt_channel: ChannelCreator<Async, 0>, gpio: AnyPin<'static>
output.set_brightness(controls.brightness.load(core::sync::atomic::Ordering::Relaxed));
// Finally, write out the rendered frame
output.commit().await.expect("Failed to commit frame");
output.commit_async().await.expect("Failed to commit frame");
let render_duration = Instant::now() - start;
let render_budget = Duration::from_millis(16);
if render_duration < render_budget {
let remaining_budget = render_budget - render_duration;
if render_duration < RENDER_BUDGET {
let remaining_budget = RENDER_BUDGET - render_duration;
uniforms.frame += 1;
Timer::after(remaining_budget).await;
} else {
warn!("Render stall! Frame took {}ms", render_duration.as_millis());
let dropped_count = (render_duration.as_ticks() as f32 / RENDER_BUDGET.as_ticks() as f32).ceil() as usize;
warn!("Render stall! Frame took {}ms, dropping {dropped_count} frame(s)", render_duration.as_millis());
// If we took longer than the budget, we need to drop some frames to catch up
uniforms.frame += dropped_count;
}
// Increment the frame counter
uniforms.frame += 1;
}
}

161
src/tasks/simulation.rs Normal file
View File

@@ -0,0 +1,161 @@
use embassy_sync::channel::DynamicSender;
use csv_core::{ReadFieldResult, Reader};
use embassy_time::{Duration, Timer};
use nalgebra::{Vector2, Vector3};
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")
}
}
}
#[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;
}
}
}
#[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;
}
}
}

80
src/tasks/ui.rs
View File

@@ -1,8 +1,9 @@
use embassy_sync::channel::DynamicReceiver;
use embassy_time::{Duration, Instant, Timer};
use figments::{liber8tion::interpolate::{ease_in_out_quad, Fract8}, prelude::*};
use figments::{liber8tion::interpolate::Fract8, prelude::*};
use rgb::{Rgb, Rgba};
use log::*;
use nalgebra::ComplexField;
use crate::{display::BikeSpace, events::{DisplayControls, Notification, Scene}, shaders::*, tasks::render::Uniforms};
@@ -79,10 +80,29 @@ impl<S: Surface<Uniforms = Uniforms, CoordinateSpace = BikeSpace, Pixel = Rgba<u
pub async fn flash_notification_color(&mut self, color: Rgb<u8>) {
self.notification.set_shader(Background::from_color(color));
self.notification.set_opacity(255);
self.notification.set_opacity(0);
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;
// Back to full
Self::animate_duration(Duration::from_millis(100), |pct| {
self.notification.set_opacity(100 + (255 - pct) / 2);
}).await;
}
// Back to off
Self::animate_duration(Duration::from_secs(3), |pct| {
self.notification.set_opacity(255 * pct);
self.notification.set_opacity(255 - pct);
}).await;
self.notification.set_visible(false);
}
@@ -104,19 +124,36 @@ impl<S: Surface<Uniforms = Uniforms, CoordinateSpace = BikeSpace, Pixel = Rgba<u
}
}
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) {
info!("Running startup fade sequence");
// Start with a completely transparent overlay, which then fades in over 1 second
self.overlay.set_opacity(0);
self.overlay.set_visible(true);
Self::animate_duration(Duration::from_secs(1), |pct| {
self.overlay.set_opacity(ease_in_out_quad(pct));
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(3), |pct| {
self.overlay.set_opacity(ease_in_out_quad(255 - pct));
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;
self.overlay.set_visible(false);
info!("Fade sequence completed");
@@ -178,13 +215,13 @@ impl<S: Surface<Uniforms = Uniforms, CoordinateSpace = BikeSpace, Pixel = Rgba<u
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);
Self::animate_duration(Duration::from_secs(1), |pct| {
self.display.brightness.store(128.scale8(255 - pct), core::sync::atomic::Ordering::Relaxed);
}).await;
},
Scene::ParkedLongTerm => {
// For long-term parking, we turn off the safety lights
@@ -195,12 +232,27 @@ impl<S: Surface<Uniforms = Uniforms, CoordinateSpace = BikeSpace, Pixel = Rgba<u
self.display.brightness.store(255 - pct, core::sync::atomic::Ordering::Relaxed);
}).await;
self.display.on.store(false, core::sync::atomic::Ordering::Relaxed);
},
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());
},
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:?}!");
}
_ => unimplemented!()
}
}
pub async fn on_event(&mut self, event: Notification) {
info!("UI Event: {event:?}");
match event {
// Apply the scene when it is changed
Notification::SceneChange(scene) => self.apply_scene(scene).await,
@@ -231,6 +283,9 @@ impl<S: Surface<Uniforms = Uniforms, CoordinateSpace = BikeSpace, Pixel = Rgba<u
#[embassy_executor::task]
pub async fn ui_main(events: DynamicReceiver<'static, Notification>, mut ui: Ui<BufferedSurface<Uniforms, BikeSpace, Rgba<u8>>>) {
// Wait for the renderer to start running
ui.display.render_is_running.wait().await;
// Run the fade sequence
ui.startup_fade_sequence().await;
@@ -240,6 +295,9 @@ pub async fn ui_main(events: DynamicReceiver<'static, Notification>, mut ui: Ui<
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;
loop {
ui.on_event(events.receive().await).await;
}