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

src: implement ego tracking models, and port shaders from renderbug into here

Browse Source
This commit is contained in:
Victoria Fischer
2025-09-22 13:16:39 +02:00
parent 29ba78d5b2
commit 19875f6ae5
18 changed files with 1191 additions and 184 deletions
Download Patch File Download Diff File Expand all files Collapse all files

96
src/tasks/gps.rs Normal file
View File

@@ -0,0 +1,96 @@
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_time::Timer;
use embedded_hal_async::i2c::I2c as _;
use esp_hal::{i2c::master::I2c, Async};
use log::*;
use nalgebra::Vector2;
use nmea::Nmea;
use crate::{backoff::Backoff, events::{Measurement, Notification}};
#[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>>) {
Backoff::from_secs(5).forever().attempt::<_, (), ()>(async || {
Backoff::from_secs(5).forever().attempt(async || {
info!("Initializing GPS");
// Enable a bunch of data? idk
let bytes = "$PMTK314,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n";
i2c_bus.write(0x10, bytes.as_bytes()).await?;
// 1hz updates
let bytes = "$PMTK220,1000*1F\r\n";
i2c_bus.write(0x10, bytes.as_bytes()).await?;
// 1hz position fix
let bytes = "$PMTK300,1000,0,0,0,0*1C\r\n";
i2c_bus.write(0x10, bytes.as_bytes()).await?;
// Antenna updates
let bytes = "$PGCMD,33,1*6C\r\n";
i2c_bus.write(0x10, bytes.as_bytes()).await
}).await.unwrap();
let mut strbuf = String::new();
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 => {
sink.send(Notification::GPSLost).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;
}
}
}
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);
for sat in parser.satellites() {
info!("\t{} snr={:?} prn={:?}", sat.gnss_type(), sat.snr(), sat.prn())
}
} else {
log::warn!("Unhandled NMEA {strbuf:?}");
}
strbuf = String::new();
parsing = false;
// Update frequency is 1hz, so we should never get an update faster than once per second
Timer::after_secs(1).await;
} else if strbuf.is_empty() && (buf[0] as char == '$' || buf[0] as char == '!') {
parsing = true;
strbuf.push(buf[0] as char);
Timer::after_millis(10).await;
} else if parsing {
strbuf.push(buf[0] as char);
Timer::after_millis(10).await;
} else {
// If there is no data ready for some reason, wait 500ms, which should place us at least somewhere after the next data frame is ready to read.
Timer::after_millis(500).await;
}
}
}).await.ok();
}

136
src/tasks/i2c.rs
View File

@@ -1,136 +0,0 @@
use core::cell::RefCell;
use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, pubsub::DynPublisher};
use embassy_time::{Delay, Timer};
use embedded_hal_async::i2c::I2c as _;
use esp_hal::{i2c::master::I2c, Async};
use log::{info, warn};
use mpu6050_dmp::{address::Address, error_async::Error, sensor_async::Mpu6050};
use nmea::Nmea;
use alloc::string::String;
use crate::{backoff::Backoff, events::Notification};
#[embassy_executor::task]
pub async fn mpu_task(events: DynPublisher<'static, Notification>, bus: I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>) {
let backoff = Backoff::from_millis(5);
let busref = RefCell::new(Some(bus));
backoff.forever().attempt::<_, (), ()>(async || {
let mut sensor = backoff.attempt(async || {
info!("Initializing MPU");
match Mpu6050::new(busref.replace(None).unwrap(), Address::default()).await.map_err(|e| { e.i2c }) {
Err(i2c) => {
busref.replace(Some(i2c));
Err(())
},
Ok(mut sensor) => {
match backoff.attempt(async || { mpu_init(&mut sensor).await }).await {
Err(_) => {
busref.replace(Some(sensor.release()));
Err(())
},
Ok(_) => Ok(sensor)
}
}
}
}).await?;
info!("MPU is ready!");
let sensor_ref = &mut sensor;
loop {
match backoff.attempt(async || { sensor_ref.motion6().await }).await {
Ok((accel_data, gyro_data)) => {
info!("Accel x={} y={} z={}", accel_data.x() as i32, accel_data.y() as i32, accel_data.z() as i32);
info!("Gyro x={} y={} z={}", gyro_data.x() as i32, gyro_data.y() as i32, gyro_data.z() as i32);
Timer::after_millis(5000).await;
},
Err(e) => {
warn!("Failed to read MPU motion data! {e:?}");
busref.replace(Some(sensor.release()));
return Err(());
}
};
}
}).await.unwrap();
}
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(3);
info!("Resetting MPU");
backoff.attempt(async || {sensor.reset(&mut delay).await}).await?;
info!("Configuring sample rate");
backoff.attempt(async || {sensor.set_sample_rate_divider(255).await}).await
}
const GPS_TEST_DATA: &str = include_str!("../test.nmea");
#[embassy_executor::task]
pub async fn gps_task(events: DynPublisher<'static, Notification>, mut i2c_bus: I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>) {
Backoff::from_secs(5).forever().attempt(async || {
info!("Initializing GPS");
// Enable a bunch of data? idk
let bytes = "$PMTK314,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n";
i2c_bus.write(0x10, bytes.as_bytes()).await?;
// 1hz updates
let bytes = "$PMTK220,1000*1F\r\n";
i2c_bus.write(0x10, bytes.as_bytes()).await?;
// 1hz position fix
let bytes = "$PMTK300,1000,0,0,0,0*1C\r\n";
i2c_bus.write(0x10, bytes.as_bytes()).await?;
// Antenna updates
let bytes = "$PGCMD,33,1*6C\r\n";
i2c_bus.write(0x10, bytes.as_bytes()).await
}).await.unwrap();
let mut strbuf = String::new();
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.unwrap();
//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) {
if parser.fix_type.is_some() != has_lock {
has_lock = parser.fix_type.is_some();
if has_lock {
events.publish(Notification::GPSAcquired).await;
} else {
events.publish(Notification::GPSLost).await;
}
}
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);
for sat in parser.satellites() {
info!("\t{} snr={:?} prn={:?}", sat.gnss_type(), sat.snr(), sat.prn())
}
} else {
log::warn!("Unhandled NMEA {strbuf:?}");
}
strbuf = String::new();
parsing = false;
// Update frequency is 1hz, so we should never get an update faster than once per second
Timer::after_secs(1).await;
} else if strbuf.is_empty() && (buf[0] as char == '$' || buf[0] as char == '!') {
parsing = true;
strbuf.push(buf[0] as char);
Timer::after_millis(3).await;
} else if parsing {
strbuf.push(buf[0] as char);
Timer::after_millis(3).await;
} else {
// If there is no data ready for some reason, wait 500ms, which should place us at least somewhere after the next data frame is ready to read.
Timer::after_millis(500).await;
}
}
}

7
src/tasks/mod.rs
View File

@@ -1,2 +1,5 @@
pub mod i2c;
pub mod render;
pub mod mpu;
pub mod render;
pub mod motion;
pub mod gps;
pub mod ui;

159
src/tasks/motion.rs Normal file
View File

@@ -0,0 +1,159 @@
#![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 crate::{ego::heading::HeadingEstimator, events::{Measurement, Notification}};
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>) {
//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;
loop {
match src.receive().await {
Measurement::IMU(imu_accel) => {
//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);
//info!("{}", imu_accel.xy().magnitude());
if imu_accel.xy().magnitude() >= 0.2 {
kf.predict(&imu_accel);
} else {
kf.update_zupt();
}
// 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;
}
}
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));
//last_fix = Some(gps_pos);
}
}
}
}
let est = kf.state();
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 !is_moving {
is_moving = true;
}
} else if is_moving {
is_moving = false;
sink.send(Notification::SceneChange(crate::events::Scene::StoplightIdle)).await;
}
}
}

180
src/tasks/mpu.rs Normal file
View File

@@ -0,0 +1,180 @@
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_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}};
const G: f32 = 9.80665;
const SENS_2G: f32 = 16384.0;
const DEG2RAD: f32 = PI / 180.0;
/// Convert raw accel (i16) to m/s^2
fn accel_raw_to_ms2(raw: i16) -> f32 {
(raw as f32 / SENS_2G) * G
}
/// Convert raw gyro (i16) to rad/s. For MPU6050 typical gyro scale at ±2000 deg/s -> 16.4 LSB/deg/s
/// Adjust if you configured different full-scale range.
fn gyro_raw_to_rads(raw: i16) -> f32 {
// lsb_per_dps e.g. 16.4 for ±2000 dps
(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: Sender<'static, NoopRawMutex, Measurement, 4>, sink: DynamicSender<'static, Notification>, bus: I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>) {
let backoff = Backoff::from_millis(5);
let busref = RefCell::new(Some(bus));
backoff.forever().attempt::<_, (), ()>(async || {
let mut sensor = backoff.forever().attempt(async || {
info!("Initializing MPU");
match Mpu6050::new(busref.replace(None).unwrap(), Address::default()).await.map_err(|e| { e.i2c }) {
Err(i2c) => {
busref.replace(Some(i2c));
Err(())
},
Ok(mut sensor) => {
match backoff.attempt(async || { mpu_init(&mut sensor).await }).await {
Err(_) => {
busref.replace(Some(sensor.release()));
Err(())
},
Ok(_) => Ok(sensor)
}
}
}
}).await?;
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);
//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 {
prev + alpha * (current - prev) // alpha in (0,1), small alpha = heavy smoothing
}
let mut prev_accel = Vector3::default();
loop {
match backoff.attempt(async || { sensor_ref.motion6().await }).await {
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(
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(
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),
lowpass(prev_accel.y, adjusted.y, 0.3),
lowpass(prev_accel.z, adjusted.z, 0.3),
);
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, adjusted_gyro.z);
events.send(
Measurement::IMU(
measured
)
).await;
Timer::after_millis(10).await;
},
Err(e) => {
warn!("Failed to read MPU motion data! {e:?}");
busref.replace(Some(sensor.release()));
return Err(());
}
};
}
}).await.ok();
}
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?
}

41
src/tasks/render.rs
View File

@@ -1,23 +1,26 @@
use embassy_sync::{blocking_mutex::raw::NoopRawMutex, pubsub::Subscriber};
use embassy_time::{Duration, Instant, Timer};
use esp_hal::{rmt::ChannelCreator, Async, peripherals::GPIO5};
use esp_hal::{gpio::AnyPin, rmt::ChannelCreator, Async};
use esp_hal_smartled::{buffer_size_async, SmartLedsAdapterAsync};
use figments::{hardware::Output, liber8tion::trig::sin8, prelude::Rectangle, render::Sample};
use figments::{hardware::{Brightness, Output}, prelude::Hsv, surface::{BufferedSurfacePool, Surfaces}};
use log::{info, warn};
use rgb::Rgb;
use rgb::Rgba;
use crate::{display::BikeOutput, events::Notification};
use crate::{display::{BikeOutput, BikeSpace}, events::DisplayControls};
#[derive(Default)]
pub struct Uniforms {
pub frame: usize,
pub primary_color: Hsv
}
//TODO: Import the bike surfaces from renderbug-prime, somehow make those surfaces into tasks
#[embassy_executor::task]
pub async fn render(mut events: Subscriber<'static, NoopRawMutex, Notification, 4, 4, 4>, rmt_channel: ChannelCreator<Async, 0>, gpio: GPIO5<'static>) {
pub async fn render(rmt_channel: ChannelCreator<Async, 0>, gpio: AnyPin<'static>, surfaces: BufferedSurfacePool<Uniforms, BikeSpace, Rgba<u8>>, controls: &'static DisplayControls) {
let rmt_buffer = [0u32; buffer_size_async(178)];
let target = SmartLedsAdapterAsync::new(rmt_channel, gpio, rmt_buffer);
// Change this number to use a different number of LEDs
//let mut pixbuf = [Default::default(); 16];
// 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
const POWER_MA : u32 = 500;
@@ -26,31 +29,25 @@ pub async fn render(mut events: Subscriber<'static, NoopRawMutex, Notification,
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 frame = 0;
let mut uniforms = Uniforms::default();
//let mut target = BrightnessWriter::new(target, MAX_POWER_MW);
let mut output = BikeOutput::new(target, MAX_POWER_MW);
info!("Rendering started!");
loop {
/*if let Some(evt) = events.try_next_message() {
}*/
let start = Instant::now();
//pixbuf.blank();
output.blank().await.expect("Failed to blank framebuf");
// Render the frame to the pixbuf, while also calculating the power consumption as we go
for (coords, pix) in output.sample(&Rectangle::everything()) {
*pix = Rgb::new(sin8(coords.x.wrapping_mul(3).wrapping_add(coords.y.wrapping_mul(3)).wrapping_add(frame)), 0, 0);
}
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
//target.write(pixbuf.iter().cloned()).await.expect("Could not write frame");
//info!("frame");
output.commit().await.expect("Failed to commit frame");
//info!("commit");
let render_duration = Instant::now() - start;
let render_budget = Duration::from_millis(16);
@@ -63,6 +60,6 @@ pub async fn render(mut events: Subscriber<'static, NoopRawMutex, Notification,
}
// Increment the frame counter
frame += 1;
uniforms.frame += 1;
}
}

246
src/tasks/ui.rs Normal file
View File

@@ -0,0 +1,246 @@
use embassy_sync::channel::DynamicReceiver;
use embassy_time::{Duration, Instant, Timer};
use figments::{liber8tion::interpolate::{ease_in_out_quad, Fract8}, prelude::*};
use rgb::{Rgb, Rgba};
use log::*;
use crate::{display::BikeSpace, events::{DisplayControls, Notification, Scene}, shaders::*, tasks::render::Uniforms};
pub struct Ui<S: Surface> {
// Background layer provides an always-running background for everything to draw on
background: S,
// Tail and panels provide content
tail: S,
panels: S,
// Notification layer sits on top of the content, and is used for transient event notifications (gps lost, wifi found, etc)
notification:S,
// Headlight and brakelight layers can only be overpainted by the bootsplash overlay layer
headlight: S,
brakelight: 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
}
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 {
Self {
background: SurfaceBuilder::build(surfaces)
.rect(Rectangle::everything())
.shader(Background::default())
.opacity(32)
.visible(false)
.finish().unwrap(),
tail: SurfaceBuilder::build(surfaces)
.rect(Rectangle::new_from_coordinates(0, 5, 255, 5))
.opacity(96)
.shader(Tail::default())
.visible(false)
.finish().unwrap(),
panels: SurfaceBuilder::build(surfaces)
.rect(Rectangle::new_from_coordinates(0, 1, 255, 4))
.opacity(128)
.shader(Panel::default())
.visible(false)
.finish().unwrap(),
notification: SurfaceBuilder::build(surfaces)
.rect(Rectangle::everything())
.shader(Background::default())
.visible(false)
.finish().unwrap(),
headlight: SurfaceBuilder::build(surfaces)
.rect(Rectangle::new_from_coordinates(0, 0, 255, 0))
.shader(Headlight::default())
.visible(false)
.finish().unwrap(),
brakelight: SurfaceBuilder::build(surfaces)
.rect(Brakelight::rectangle())
.shader(Brakelight::default())
.visible(false)
.finish().unwrap(),
overlay: SurfaceBuilder::build(surfaces)
.rect(Rectangle::everything())
.visible(true)
.opacity(0)
.shader(Thinking::default())
.finish().unwrap(),
headlight_on: false,
brakelight_on: false,
display
}
}
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_visible(true);
Self::animate_duration(Duration::from_secs(3), |pct| {
self.notification.set_opacity(255 * pct);
}).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 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));
}).await;
// When the overlay is fully opaque and all lower layers will be hidden, turn them on
self.apply_scene(Scene::Startup).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));
}).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);
}
}
}
// 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::ParkedIdle => {
// Ensure the display is on
self.display.on.store(true, core::sync::atomic::Ordering::Relaxed);
// 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
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);
}
_ => unimplemented!()
}
}
pub async fn on_event(&mut self, event: Notification) {
match event {
// Apply the scene when it is changed
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
// Other event ideas:
// - Bike has crashed, or is laid down
// - Unstable physics right before crashing?
// - Turning left/right
// - BPM sync with phone app
// - GPS data is being synchronized with nextcloud/whatever
// - A periodic flash when re-initializing MPU and GPS, to indicate there might be a problem?
// - Bluetooth/BLE connect/disconnect events
// - Bike is waking up from stationary?
}
}
}
#[embassy_executor::task]
pub async fn ui_main(events: DynamicReceiver<'static, Notification>, mut ui: Ui<BufferedSurface<Uniforms, BikeSpace, Rgba<u8>>>) {
// Run the fade sequence
ui.startup_fade_sequence().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;
loop {
ui.on_event(events.receive().await).await;
}
}