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

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;
}