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

bump a lot of big changes I dont want to break down into individual commits

Browse Source
This commit is contained in:
Victoria Fischer
2025-10-11 16:34:09 +02:00
parent 0e9e0c1b13
commit 8280f38185
48 changed files with 1275467 additions and 394056 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
src/tasks/demo.rs Normal file
View File

@@ -0,0 +1,18 @@
use embassy_sync::channel::DynamicSender;
use embassy_time::Timer;
use crate::events::{Notification, Scene};
#[embassy_executor::task]
pub async fn demo_task(ui: DynamicSender<'static, Notification>) {
Timer::after_secs(10).await;
ui.send(Notification::SceneChange(Scene::Idle)).await;
Timer::after_secs(10).await;
loop {
for scene in [Scene::Accelerating, Scene::Ready, Scene::Decelerating, Scene::Ready] {
Timer::after_secs(8).await;
ui.send(Notification::SceneChange(scene)).await
};
}
}

16
src/tasks/gps.rs
View File

@@ -10,9 +10,7 @@ use nmea::Nmea;
use crate::{backoff::Backoff, events::Measurement};
#[allow(dead_code)] //FIXME: Allow switching to this via configure option
const GPS_TEST_DATA: &str = include_str!("../test.nmea");
// FIXME: We need a way to put the GPS to sleep when the system goes to sleep
#[embassy_executor::task]
pub async fn gps_task(events: DynamicSender<'static, Measurement>, mut i2c_bus: I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>) {
Backoff::from_secs(5).forever().attempt::<_, (), ()>(async || {
@@ -40,16 +38,15 @@ pub async fn gps_task(events: DynamicSender<'static, Measurement>, mut i2c_bus:
let mut parser = Nmea::default();
let mut parsing = false;
let mut has_lock = false;
//let mut iter = GPS_TEST_DATA.as_bytes().iter();
info!("GPS is ready!");
loop {
let mut buf = [0; 1];
i2c_bus.read(0x10, &mut buf).await.map_err(|_| { Err::<(), ()>(()) }).ok();
//buf[0] = *(iter.next().unwrap());
if (buf[0] as char == '\n' || buf[0] as char == '\r') && !strbuf.is_empty() {
if let Ok(result) = parser.parse(&strbuf) {
match parser.fix_type {
None if has_lock => {
// TODO: Send a Measurement::SensorOffline(SensorSource::GPS) here instead
events.send(Measurement::GPS(None)).await;
has_lock = false
},
@@ -58,6 +55,7 @@ pub async fn gps_task(events: DynamicSender<'static, Measurement>, mut i2c_bus:
if !has_lock {
has_lock = true;
}
// TODO: Send a Measurement::SensorOnline(SensorSource::GPS) here instead
//TODO: 4 satellites seems to be "Some" fix, 6 is a perfect fix
//TODO: Only send updates when we get the correct nmea sentence
@@ -66,11 +64,11 @@ pub async fn gps_task(events: DynamicSender<'static, Measurement>, mut i2c_bus:
}
}
}
log::info!("nmea={result:?} raw={strbuf:?}");
log::debug!("nmea={parser:?}");
log::info!("speed={:?} altitude={:?} lat={:?} lng={:?} fix={:?}", parser.speed_over_ground, parser.altitude, parser.latitude, parser.longitude, parser.fix_type);
log::trace!("nmea={result:?} raw={strbuf:?}");
log::trace!("nmea={parser:?}");
log::trace!("speed={:?} altitude={:?} lat={:?} lng={:?} fix={:?}", parser.speed_over_ground, parser.altitude, parser.latitude, parser.longitude, parser.fix_type);
for sat in parser.satellites() {
info!("\t{} snr={:?} prn={:?}", sat.gnss_type(), sat.snr(), sat.prn())
trace!("\t{} snr={:?} prn={:?}", sat.gnss_type(), sat.snr(), sat.prn())
}
} else {
log::warn!("Unhandled NMEA {strbuf:?}");

10
src/tasks/mod.rs
View File

@@ -1,7 +1,13 @@
#[cfg(feature="mpu")]
pub mod mpu;
#[cfg(feature="gps")]
pub mod gps;
pub mod render;
pub mod motion;
pub mod gps;
pub mod ui;
#[cfg(feature="radio")]
pub mod wifi;
#[cfg(feature="simulation")]
pub mod simulation;
pub mod simulation;
pub mod predict;
pub mod demo;

204
src/tasks/motion.rs
View File

@@ -1,203 +1,31 @@
use embassy_sync::channel::{DynamicReceiver, DynamicSender};
use nalgebra::{Rotation3, Vector2, Vector3, ComplexField};
use crate::{ego::{alignment::{Direction, DirectionEstimator, SensorAlignment}, heading::HeadingEstimator, kalman::{GpsMeasurement, Kalman}}, events::{Measurement, Notification, Prediction, Telemetry}, CircularBuffer};
use log::*;
#[embassy_executor::task]
pub async fn motion_task(src: DynamicReceiver<'static, Measurement>, ui_sink: DynamicSender<'static, Notification>, telemetry_sink: DynamicSender<'static, Telemetry>) {
//TODO: is this the right value? how do update frequencies work?
let dt = 0.01; // 10 Hz
let mut kf = Kalman::new(dt);
use crate::{ego::engine::BikeStates, events::{Measurement, Notification, Prediction}};
let mut recent_accel: CircularBuffer<Vector3<f32>, 100> = CircularBuffer::default();
let mut gravity_align = None;
let mut forwards_align = DirectionEstimator::new(Direction::UpX);
let mut last_fix = Vector2::default();
let mut reference_fix = None;
let mut heading = HeadingEstimator::new(0.0);
#[embassy_executor::task]
pub async fn motion_task(src: DynamicReceiver<'static, Measurement>, ui_sink: DynamicSender<'static, Notification>, prediction_sink: DynamicSender<'static, Prediction>) {
let mut states = BikeStates::default();
loop {
let next_measurement = src.receive().await;
let _ = telemetry_sink.try_send(Telemetry::Measurement(next_measurement));
debug!("measurement={next_measurement:?}");
match next_measurement {
Measurement::IMU { accel, gyro } => {
recent_accel.insert(accel);
// Require a gravity alignment first
if gravity_align.is_none() {
gravity_align = {
let align = SensorAlignment::from_samples(&recent_accel.data, crate::ego::alignment::Direction::DnZ);
if align.bias.norm() < 9.764 || align.bias.norm() > 9.834 {
None
} else {
ui_sink.send(Notification::CalibrationComplete).await;
Some(align)
}
}
} else {
// Minimum motion threshold
if accel.xy().magnitude() >= 0.8 {
// Try to build a new 'forwards' alignment
forwards_align.update(accel);
// Rotate the measurement into the gravity-down frame
let (body_accel, body_gyro) = if let Some(ref adj) = gravity_align {
(adj.apply(&accel), adj.apply(&gyro))
} else {
(accel, gyro)
};
// Then rotate the other two axes to produce the final body frame
let forwards_adjusted = forwards_align.apply(&body_accel);
heading.update(body_gyro.z, 0.1); // TODO: need to correctly use time values for dt
let heading_rotation = Rotation3::from_axis_angle(&Vector3::z_axis(), heading.heading());
let enu_rotated = heading_rotation * forwards_adjusted;
// FIXME: the last thing to add here is rotating the acceleration measurements into the ENU frame.
let m = Vector3::new(enu_rotated.x, enu_rotated.y, body_gyro.z);
kf.predict(&m);
} else {
// Otherwise, we are standing stationary and should insert accel=0 data into the model
kf.update_zupt();
}
}
states.insert_imu(accel, gyro);
states.commit(&prediction_sink, &ui_sink).await;
},
Measurement::GPS(Some(gps_pos)) => {
match reference_fix {
None => {
reference_fix = Some(gps_pos);
last_fix = gps_pos;
ui_sink.send(Notification::GPSAcquired).await;
},
Some(coords) => {
let est = kf.state();
info!("pos=\t({},\t{})\tvel=({},\t{})\theading={}\t({})\tforwards={:?}", est[0], est[1], est[2], est[3], est[4], heading.heading(), forwards_align.apply(&Vector3::new(1.0, 0.0, 0.0)));
if last_fix != coords {
let gps_heading = last_fix.angle(&coords);
heading.correct(gps_heading as f32, 0.9);
}
// Convert GPS coordinates into a meter distance away from the known fix location
let dx = (gps_pos.x - coords.x) * coords.x.cos() * 6371000.0;
let dy = (gps_pos.y - coords.y) * 6371000.0;
kf.update(&GpsMeasurement::new(dx as f32, dy as f32));
info!("GPS={gps_pos:?} d=({dx}, {dy}) heading={}", heading.heading().to_degrees());
last_fix = coords;
}
}
states.insert_gps(gps_pos);
states.commit(&prediction_sink, &ui_sink).await;
},
Measurement::GPS(None) => {
reference_fix = None;
ui_sink.send(Notification::GPSLost).await;
}
}
let est = kf.state();
if est[2].abs() + est[3].abs() >= 5.0 {
//info!("pos=\t({},\t{})\tvel=({},\t{})\theading={}\tforwards={:?}", est[0], est[1], est[2], est[3], est[4], forwards_align.apply(&Vector3::new(1.0, 0.0, 0.0)));
}
let _ = telemetry_sink.try_send(Telemetry::Prediction(Prediction::Location(est.xy())));
}
}
/*#[embassy_executor::task]
pub async fn _motion_task(src: DynamicReceiver<'static, Measurement>, sink: DynamicSender<'static, Notification>, telemetry_sink: DynamicSender<'static, Measurement>) {
//TODO: is this the right value? how do update frequencies work?
let dt = 0.01; // 10 Hz
let mut kf = Kalman::new(dt);
let mut is_moving = false;
let mut heading = HeadingEstimator::new(0.0);
//TODO: Accelerating/decelerating events need thresholds, maybe a PID to detect? PID setpoint can be a running average speed, and if the calculated error is above/below a threshold, it means we are accelerating/decelerating
let mut last_fix = None;
let mut last_idle = Instant::now();
let mut is_maybe_idling = false;
let mut is_idle = false;
let mut is_parked = false;
loop {
match src.receive().await {
Measurement::IMU{ accel, gyro } => {
//info!("IMU update {imu_accel:?}");
//info!("m=\t({:?}, {:?})\tyaw={:?}", imu_accel.x, imu_accel.y, imu_accel.z);
//kf.predict(&Control::new(imu_accel.x, imu_accel.y, imu_accel.z));
// TODO: Also needs corrected using GPS headings via rotation from body frame into world frame
//heading.heading();
heading.update(accel.z, 0.01);
//info!("{}", imu_accel.xy().magnitude());
if accel.xy().magnitude() >= 0.8 {
//info!("magnitude {imu_accel:?} {}", imu_accel.xy().magnitude());
kf.predict(&accel);
if is_parked {
info!("Wake up!");
sink.send(Notification::SceneChange(crate::events::Scene::ParkedIdle)).await;
is_maybe_idling = false;
is_parked = false;
is_idle = false;
}
} else {
//info!("Stationary!!");
kf.update_zupt();
if !is_maybe_idling {
is_maybe_idling = true;
last_idle = Instant::now();
} else if !is_idle && Instant::now() - last_idle >= Duration::from_secs(15) {
info!("Parked idle");
sink.send(Notification::SceneChange(crate::events::Scene::ParkedIdle)).await;
is_idle = true;
} else if !is_parked && Instant::now() - last_idle >= Duration::from_secs(60 * 3) {
info!("Parked long term!");
sink.send(Notification::SceneChange(crate::events::Scene::ParkedLongTerm)).await;
is_parked = true;
}
}
// TODO: Rotate the acceleration values into the World frame using GPS heading values
// X means forwards, Y is left/right slide, Z is vertical movement
if accel.x > 1.0 {
//sink.send(Notification::SceneChange(crate::events::Scene::Accelerating)).await;
} else if accel.x < -1.0 {
//sink.send(Notification::SceneChange(crate::events::Scene::Decelerating)).await;
}
let _ = telemetry_sink.try_send(Measurement::IMU { accel, gyro });
}
Measurement::GPS(gps_pos) => {
//info!("GPS update! {gps_pos:?}");
match last_fix {
None => last_fix = Some(gps_pos),
Some(coords) => {
let dx = (gps_pos.x - coords.x) * coords.x.cos() * 6371000.0;
let dy = (gps_pos.y - coords.y) * 6371000.0;
kf.update(&GpsMeasurement::new(dx as f32, dy as f32));
}
}
let _ = telemetry_sink.try_send(Measurement::GPS(gps_pos));
}
}
let est = kf.state();
//let _ = telemetry_sink.try_send(Measurement::Prediction(*est));
if est[2].abs() + est[3].abs() >= 0.2 {
info!("pos=\t({},\t{})\tvel=({},\t{})\theading=({},\t{})", est[0], est[1], est[2], est[3], est[4], est[5]);
}
if est[2].abs() > 1.0 {
if !is_moving {
is_moving = true;
sink.send(Notification::SceneChange(crate::events::Scene::Accelerating)).await;
}
} else if is_moving {
is_moving = false;
sink.send(Notification::SceneChange(crate::events::Scene::StoplightIdle)).await;
states.has_gps_fix.set(false);
},
// FIXME: This needs harmonized with the automatic data timeout from above, somehow?
Measurement::SensorOnline(source) => warn!("Sensor {source:?} is online!"),
Measurement::SensorOffline(source) => warn!("Sensor {source:?} is offline!"),
Measurement::SimulationProgress(source, duration, _pct) => debug!("{source:?} simulation time: {}", duration.as_secs()),
}
}
}*/
}

95
src/tasks/mpu.rs
View File

@@ -2,14 +2,15 @@ use core::cell::RefCell;
use embassy_embedded_hal::shared_bus::asynch::i2c::I2cDevice;
use embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, channel::DynamicSender};
use embassy_time::{Delay, Timer};
use embassy_time::{Delay, Timer, Instant};
use esp_hal::{i2c::master::I2c, Async};
use log::{info, warn};
use log::*;
use mpu6050_dmp::{address::Address, calibration::CalibrationParameters, error_async::Error, sensor_async::Mpu6050};
use nalgebra::Vector3;
use core::f32::consts::PI;
use crate::events::SensorSource;
use crate::{backoff::Backoff, ego::tilt::TiltEstimator, events::Measurement};
use crate::{backoff::Backoff, events::Measurement};
const G: f32 = 9.80665;
const SENS_2G: f32 = 16384.0;
@@ -27,43 +28,6 @@ fn gyro_raw_to_rads(raw: i16) -> f32 {
(raw as f32 / 16.4) * (DEG2RAD)
}
/*/// Rotate body accel into ENU and remove gravity; returns (east, north, up) linear accel in m/s^2
pub fn accel_body_to_enu_lin(body_acc: (f32,f32,f32), roll: f32, pitch: f32, yaw: f32) -> (f32,f32,f32) {
let (ax, ay, az) = body_acc;
let (sr, cr) = roll.sin_cos();
let (sp, cp) = pitch.sin_cos();
let (sy, cy) = yaw.sin_cos();
// Build R = Rz(yaw) * Ry(pitch) * Rx(roll)
// multiply R * a_body
let r11 = cy*cp;
let r12 = cy*sp*sr - sy*cr;
let r13 = cy*sp*cr + sy*sr;
let r21 = sy*cp;
let r22 = sy*sp*sr + cy*cr;
let r23 = sy*sp*cr - cy*sr;
let r31 = -sp;
let r32 = cp*sr;
let r33 = cp*cr;
// a_nav = R * a_body
let ax_nav = r11*ax + r12*ay + r13*az; // east
let ay_nav = r21*ax + r22*ay + r23*az; // north
let az_nav = r31*ax + r32*ay + r33*az; // up
// remove gravity (nav frame gravity points down in NED; in our ENU 'up' axis positive up)
// If accel measured gravity as +9.8 on sensor when static and z down convention used above,
// then az_nav will include +g in 'up' direction; subtract G.
let ax_lin = ax_nav;
let ay_lin = ay_nav;
let az_lin = az_nav - G; // linear acceleration in up axis
(ax_lin, ay_lin, az_lin)
}*/
#[embassy_executor::task]
pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>) {
let backoff = Backoff::from_millis(5);
@@ -71,7 +35,7 @@ pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevic
backoff.forever().attempt::<_, (), ()>(async || {
let mut sensor = backoff.forever().attempt(async || {
info!("Initializing MPU");
warn!("Initializing connection to MPU");
match Mpu6050::new(busref.replace(None).unwrap(), Address::default()).await.map_err(|e| { e.i2c }) {
Err(i2c) => {
busref.replace(Some(i2c));
@@ -91,8 +55,7 @@ pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevic
let sensor_ref = &mut sensor;
info!("MPU is ready!");
let _tilt = TiltEstimator::new(0.3);
events.send(Measurement::SensorOnline(SensorSource::IMU)).await;
//TODO: Need to read in a constant buffer of accelerometer measurements, which we can then use to determine where "forward" points in the body frame when converting from the sensor frame.
// From there, we can rotate the body frame into the world frame using gps headings to generate a compass north
fn lowpass(prev: f32, current: f32, alpha: f32) -> f32 {
@@ -122,12 +85,6 @@ pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevic
);
prev_accel = adjusted;
// Apply current rotation and accel values to rotate our values back into the ENU frame
/*tilt.update(0.01, (filtered.x, filtered.y, filtered.z), (adjusted_gyro.x, adjusted_gyro.y, adjusted_gyro.z));
let (roll, pitch, yaw) = tilt.angles();
let (ax_e, ay_n, az_up) = accel_body_to_enu_lin((filtered.x,filtered.y,filtered.z), roll, pitch, yaw);
let measured = Vector3::new(ax_e, ay_n, az_up);*/
events.send(
Measurement::IMU {
accel: filtered,
@@ -137,7 +94,7 @@ pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevic
Timer::after_millis(10).await;
},
Err(e) => {
warn!("Failed to read MPU motion data! {e:?}");
error!("Failed to read MPU motion data! {e:?}");
busref.replace(Some(sensor.release()));
return Err(());
}
@@ -147,18 +104,28 @@ pub async fn mpu_task(events: DynamicSender<'static, Measurement>, bus: I2cDevic
}
async fn mpu_init(sensor: &mut Mpu6050<I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>>) -> Result<(), Error<I2cDevice<'static, CriticalSectionRawMutex, I2c<'static, Async>>>> {
let mut delay = Delay;
let backoff = Backoff::from_millis(10);
info!("Initializing MPU");
backoff.attempt(async || { sensor.initialize_dmp(&mut delay).await }).await?;
backoff.attempt(async || { sensor.set_digital_lowpass_filter(mpu6050_dmp::config::DigitalLowPassFilter::Filter3).await }).await?;
info!("Calibrating MPU");
backoff.attempt(async || {
// TODO: Store calibration data with set_accel_calibration, set_gyro_calibration
sensor.calibrate(&mut delay, &CalibrationParameters::new(
mpu6050_dmp::accel::AccelFullScale::G2,
mpu6050_dmp::gyro::GyroFullScale::Deg2000,
mpu6050_dmp::calibration::ReferenceGravity::Zero
)).await.map(|_| { Ok(()) })
}).await?
if cfg!(feature="wokwi") {
warn!("Initializing simulated MPU");
Ok(())
} else {
let mut delay = Delay;
let backoff = Backoff::from_millis(10);
info!("Initializing DMP");
let start = Instant::now();
backoff.attempt(async || { sensor.initialize_dmp(&mut delay).await }).await?;
backoff.attempt(async || { sensor.set_digital_lowpass_filter(mpu6050_dmp::config::DigitalLowPassFilter::Filter1).await }).await?;
info!("DMP ready in {}ms", start.as_millis());
info!("Calibrating MPU");
let start = Instant::now();
backoff.attempt(async || {
// TODO: Store calibration data with set_accel_calibration, set_gyro_calibration
sensor.calibrate(&mut delay, &CalibrationParameters::new(
mpu6050_dmp::accel::AccelFullScale::G2,
mpu6050_dmp::gyro::GyroFullScale::Deg2000,
mpu6050_dmp::calibration::ReferenceGravity::Zero
)).await
}).await?;
info!("MPU calibrated in {}ms", start.as_millis());
Ok(())
}
}

108
src/tasks/predict.rs Normal file
View File

@@ -0,0 +1,108 @@
use embassy_sync::channel::{DynamicReceiver, DynamicSender};
use embassy_time::Duration;
use log::*;
use crate::{ego::engine::{gps_to_local_meters_haversine, MotionState}, events::{Notification, Prediction, Scene}, idle::IdleClock};
#[embassy_executor::task]
pub async fn prediction_task(prediction_src: DynamicReceiver<'static, Prediction>, notify: DynamicSender<'static, Notification>) {
let mut last_velocity = 0.0;
let mut first_position = None;
let mut last_position = Default::default();
let mut parking_timer = IdleClock::new(Duration::from_secs(10));
let mut sleep_timer = IdleClock::new(Duration::from_secs(30));
let mut stationary = true;
loop {
let d = first_position.map(|x| {
gps_to_local_meters_haversine(&x, &last_position).norm()
});
if let Ok(next_evt) = embassy_time::with_timeout(Duration::from_secs(1), prediction_src.receive()).await {
match next_evt {
Prediction::WakeRequested => {
if sleep_timer.wake() {
warn!("Wake requested during sleep");
notify.send(Notification::WakeUp).await;
// Also reset the parking timer
parking_timer.wake();
} else if parking_timer.wake() {
info!("Wake requested while parked");
// If we weren't asleep but we were parked, then switch back to the Ready state and turn on the lights
notify.send(Notification::SetHeadlight(true)).await;
notify.send(Notification::SetBrakelight(true)).await;
notify.send(Notification::SceneChange(Scene::Ready)).await;
}
}
Prediction::Velocity(v) => {
last_velocity = v;
if v > 5.0 && stationary {
notify.send(Notification::Beat).await;
}
// TODO: Probably makes sense to only print this based on an IdleTimer, so that a long period of slightly variable movement doesn't get lost, but we can still report values to the UI / telemetry outputs
//info!("Velocity predict: velocity={v}\tpos={last_position:?}\tdistance={d:?}");
},
Prediction::Location(loc) => {
if first_position.is_none() {
info!("Got location={loc:?}");
first_position = Some(loc);
}
last_position = loc;
}
Prediction::Motion(motion) => {
info!("Motion predict:\t{motion:?}\tvelocity={last_velocity}\tpos={last_position:?}\tdistance={d:?}");
if sleep_timer.wake() {
notify.send(Notification::WakeUp).await;
notify.send(Notification::SetHeadlight(true)).await;
notify.send(Notification::SetBrakelight(true)).await
}
if parking_timer.wake() {
notify.send(Notification::SetHeadlight(true)).await;
notify.send(Notification::SetBrakelight(true)).await
}
match motion {
MotionState::Accelerating => {
if stationary {
// If we are going from standing still to immediately accelerating, first transition to the 'ready' scene
notify.send(Notification::SceneChange(Scene::Ready)).await;
}
notify.send(Notification::SceneChange(Scene::Accelerating)).await;
stationary = false;
},
MotionState::Decelerating => {
if stationary {
// If we are going from standing still to immediately accelerating, first transition to the 'ready' scene
notify.send(Notification::SceneChange(Scene::Ready)).await;
}
notify.send(Notification::SceneChange(Scene::Decelerating)).await;
stationary = false;
},
MotionState::Steady => {
notify.send(Notification::SceneChange(Scene::Ready)).await;
stationary = false;
},
MotionState::Stationary => {
notify.send(Notification::SceneChange(Scene::Ready)).await;
stationary = true
}
}
}
}
}
// TODO: Need a way to detect if sensors are dead for some reason. Probably should be done in the motion engine, since it would be a prediction?
if stationary {
if parking_timer.check() {
notify.send(Notification::SceneChange(Scene::Idle)).await;
notify.send(Notification::SetHeadlight(false)).await;
notify.send(Notification::SetBrakelight(false)).await
}
if sleep_timer.check() {
notify.send(Notification::Sleep).await;
}
}
}
}

51
src/tasks/render.rs
View File

@@ -1,23 +1,20 @@
use embassy_time::{Duration, Instant, Timer};
use esp_hal::{gpio::AnyPin, rmt::Rmt, time::Rate};
use esp_hal::{gpio::AnyPin, rmt::Rmt, time::Rate, timer::timg::Wdt};
use esp_hal_smartled::{buffer_size_async, SmartLedsAdapterAsync};
use figments::{hardware::{Brightness, OutputAsync}, prelude::Hsv, surface::{BufferedSurfacePool, Surfaces}};
use figments::{prelude::*, surface::{BufferedSurfacePool, Surfaces}};
use figments_render::gamma::GammaCurve;
use figments_render::output::{GammaCorrected, OutputAsync};
use log::{info, warn};
use rgb::Rgba;
use nalgebra::ComplexField;
use alloc::sync::Arc;
use crate::{display::{BikeOutputAsync, BikeSpace}, events::DisplayControls};
#[derive(Default)]
pub struct Uniforms {
pub frame: usize,
pub primary_color: Hsv
}
use crate::{display::{BikeOutput, SegmentSpace, Uniforms}, events::DisplayControls};
//TODO: Import the bike surfaces from renderbug-prime, somehow make those surfaces into tasks
#[embassy_executor::task]
pub async fn render(rmt: esp_hal::peripherals::RMT<'static>, gpio: AnyPin<'static>, surfaces: BufferedSurfacePool<Uniforms, BikeSpace, Rgba<u8>>, controls: &'static DisplayControls) {
pub async fn render(rmt: esp_hal::peripherals::RMT<'static>, gpio: AnyPin<'static>, surfaces: BufferedSurfacePool<Uniforms, SegmentSpace, Rgba<u8>>, controls: Arc<DisplayControls>, mut wdt: Wdt<esp_hal::peripherals::TIMG0<'static>>) {
let frequency: Rate = Rate::from_mhz(80);
let rmt = Rmt::new(rmt, frequency)
.expect("Failed to initialize RMT").into_async();
@@ -33,7 +30,8 @@ pub async fn render(rmt: esp_hal::peripherals::RMT<'static>, gpio: AnyPin<'stati
// You probably don't need to change these values, unless your LED strip is somehow not 5 volts
const POWER_VOLTS : u32 = 5;
const MAX_POWER_MW : u32 = POWER_VOLTS * POWER_MA;
const MAX_POWER_MW : u32 = if cfg!(feature="max-usb-power") { u32::MAX } else { POWER_VOLTS * POWER_MA };
// This value is used as the 'seed' for rendering each frame, allowing us to do things like run the animation backwards, frames for double FPS, or even use system uptime for more human-paced animations
let mut uniforms = Uniforms {
@@ -41,29 +39,42 @@ pub async fn render(rmt: esp_hal::peripherals::RMT<'static>, gpio: AnyPin<'stati
..Uniforms::default()
};
let mut output = BikeOutputAsync::new(target, MAX_POWER_MW);
let mut output = BikeOutput::new(target, MAX_POWER_MW, controls.clone());
output.set_gamma(GammaCurve::new(1.3));
//#[cfg(not(feature="wokwi"))]
//output.set_gamma(GammaCurve::new(2.1));
info!("Rendering started! {}ms since boot", Instant::now().as_millis());
controls.render_is_running.signal(true);
const FPS: u64 = 60;
const FPS: u64 = 80;
const RENDER_BUDGET: Duration = Duration::from_millis(1000 / FPS);
loop {
// FIXME: need to put the rendering loop into a deep sleep when the display is off
let start = Instant::now();
output.blank_async().await.expect("Failed to blank framebuf");
output.blank();
surfaces.render_to(&mut output, &uniforms);
// Apply hardware controls
output.set_on(controls.on.load(core::sync::atomic::Ordering::Relaxed));
output.set_brightness(controls.brightness.load(core::sync::atomic::Ordering::Relaxed));
// Finally, write out the rendered frame
output.commit_async().await.expect("Failed to commit frame");
let render_duration = Instant::now() - start;
if !controls.is_on() {
warn!("Renderer is sleeping zzzz");
//controls.render_is_running.signal(false);
output.blank();
wdt.disable();
controls.wait_for_on().await;
wdt.feed();
wdt.enable();
warn!("Renderer is awake !!!!");
//controls.render_is_running.signal(true);
}
if render_duration < RENDER_BUDGET {
let remaining_budget = RENDER_BUDGET - render_duration;
uniforms.frame += 1;
@@ -74,5 +85,9 @@ pub async fn render(rmt: esp_hal::peripherals::RMT<'static>, gpio: AnyPin<'stati
// If we took longer than the budget, we need to drop some frames to catch up
uniforms.frame += dropped_count;
}
uniforms.primary_color.hue = uniforms.primary_color.hue.wrapping_add(1);
wdt.feed();
}
}

191
src/tasks/simulation.rs
View File

@@ -1,160 +1,63 @@
use embassy_sync::channel::DynamicSender;
use embassy_time::{Duration, Timer};
use nalgebra::{Vector2, Vector3};
use csv_core::{ReadFieldResult, Reader};
use log::*;
use crate::events::Measurement;
const ACCELEROMETER_DATA: &str = include_str!("../../test-data/accelerometer.csv");
//const GYRO_DATA: &str = include_str!("../../test-data/gyro.csv");
const GPS_DATA: &str = include_str!("../../test-data/gps.csv");
struct SimDataReader<'a> {
reader: Reader,
buf: &'a str,
read_offset: usize
}
impl<'a> SimDataReader<'a> {
pub fn new(buf: &'a str) -> Self {
Self {
buf,
read_offset: 0,
reader: Reader::new()
}
}
}
impl<'a> Iterator for SimDataReader<'a> {
type Item = &'a str;
fn next(&mut self) -> Option<Self::Item> {
let mut out_buf = [0u8; 128];
match self.reader.read_field(&self.buf.as_bytes()[self.read_offset..(self.read_offset + 128).min(self.buf.len())], &mut out_buf) {
(ReadFieldResult::Field { record_end: _ }, read_size, write_size) => {
let start = self.read_offset;
self.read_offset += read_size;
Some(&self.buf[start..(start + write_size)])
},
(ReadFieldResult::End, _, _) => None,
err => panic!("{:?}", err)
}
}
}
#[derive(Default, Debug, Clone, Copy)]
enum MotionFields {
#[default]
Time,
SecondsElapsed,
Z,
Y,
X,
End
}
impl MotionFields {
pub fn next(self) -> Self {
match self {
Self::Time => Self::SecondsElapsed,
Self::SecondsElapsed => Self::Z,
Self::Z => Self::Y,
Self::Y => Self::X,
Self::X => Self::End,
Self::End => panic!("No more fields")
}
}
}
#[derive(Default, Debug, Clone, Copy)]
enum GpsFields {
#[default]
Time,
SecondsElapsed,
BearingAcc,
SpeedAcc,
VertAcc,
HorizAcc,
Speed,
Bearing,
Altitude,
Longitude,
Latitude,
End
}
impl GpsFields {
pub fn next(self) -> Self {
match self {
Self::Time => Self::SecondsElapsed,
Self::SecondsElapsed => Self::BearingAcc,
Self::BearingAcc => Self::SpeedAcc,
Self::SpeedAcc => Self::VertAcc,
Self::VertAcc => Self::HorizAcc,
Self::HorizAcc => Self::Speed,
Self::Speed => Self::Bearing,
Self::Bearing => Self::Altitude,
Self::Altitude => Self::Longitude,
Self::Longitude => Self::Latitude,
Self::Latitude => Self::End,
Self::End => panic!("No more fields")
}
}
}
use crate::events::{Measurement, SensorSource};
use crate::Breaker;
#[embassy_executor::task]
pub async fn motion_simulation_task(events: DynamicSender<'static, Measurement>) {
let mut accel = Vector3::default();
let mut cur_field = MotionFields::default();
let mut last_stamp = 0.0;
let mut cur_delta = Duration::from_ticks(0);
let reader = SimDataReader::new(ACCELEROMETER_DATA);
for field in reader {
match cur_field {
MotionFields::SecondsElapsed => {
let secs = field.parse::<f64>().unwrap();
cur_delta = Duration::from_millis(((secs - last_stamp) * 1000.0) as u64);
last_stamp = secs;
}
MotionFields::X => accel.x = field.parse::<f32>().unwrap(),
MotionFields::Y => accel.y = field.parse().unwrap(),
MotionFields::Z => accel.z = field.parse().unwrap(),
_ => ()
}
cur_field = cur_field.next();
if let MotionFields::End = cur_field {
cur_field = MotionFields::default();
events.send(Measurement::IMU{ accel, gyro: Vector3::default() }).await;
Timer::after(cur_delta).await;
let mut rd = rmp::decode::Bytes::new(include_bytes!("../../test-data/motion.msgpack"));
let mut runtime_secs = Breaker::default();
let mut runtime = Duration::default();
events.send(Measurement::SensorOnline(SensorSource::IMU)).await;
while let Ok(size) = rmp::decode::read_array_len(&mut rd) {
assert_eq!(size, 7, "Expected 7 fields, but only found {size}");
let delay = embassy_time::Duration::from_millis((rmp::decode::read_f64(&mut rd).unwrap() * 1000.0) as u64);
let accel = Vector3::new(
rmp::decode::read_f64(&mut rd).unwrap() as f32,
rmp::decode::read_f64(&mut rd).unwrap() as f32,
rmp::decode::read_f64(&mut rd).unwrap() as f32,
);
let gyro = Vector3::new(
rmp::decode::read_f64(&mut rd).unwrap() as f32,
rmp::decode::read_f64(&mut rd).unwrap() as f32,
rmp::decode::read_f64(&mut rd).unwrap() as f32,
);
runtime += delay;
runtime_secs.set(runtime.as_secs());
if runtime_secs.read_tripped().is_some() {
events.send(Measurement::SimulationProgress(SensorSource::IMU, runtime, 0.0)).await;
}
Timer::after(delay).await;
events.send(Measurement::IMU{ accel, gyro }).await;
}
events.send(Measurement::SensorOffline(SensorSource::IMU)).await;
warn!("End of motion recording");
}
#[embassy_executor::task]
pub async fn location_simulation_task(events: DynamicSender<'static, Measurement>) {
let mut coords = Vector2::default();
let mut cur_field = GpsFields::default();
let mut last_stamp = 0.0;
let mut cur_delta = Duration::from_ticks(0);
let reader = SimDataReader::new(GPS_DATA);
for field in reader {
match cur_field {
GpsFields::SecondsElapsed => {
let secs = field.parse::<f64>().unwrap();
cur_delta = Duration::from_millis(((secs - last_stamp) * 1000.0) as u64);
last_stamp = secs;
}
GpsFields::Latitude => coords.x = field.parse::<f64>().unwrap(),
GpsFields::Longitude => coords.y = field.parse().unwrap(),
_ => ()
}
cur_field = cur_field.next();
if let GpsFields::End = cur_field {
cur_field = GpsFields::default();
Timer::after(cur_delta).await;
events.send(Measurement::GPS(Some(coords))).await;
let mut rd = rmp::decode::Bytes::new(include_bytes!("../../test-data/gps.msgpack"));
let mut runtime_secs = Breaker::default();
let mut runtime = Duration::default();
events.send(Measurement::SensorOnline(SensorSource::GPS)).await;
while let Ok(size) = rmp::decode::read_array_len(&mut rd) {
assert_eq!(size, 3, "Expected 3 fields, but only found {size}");
let delay = embassy_time::Duration::from_millis((rmp::decode::read_f64(&mut rd).unwrap() * 1000.0) as u64);
let coords = Vector2::new(
rmp::decode::read_f64(&mut rd).unwrap(),
rmp::decode::read_f64(&mut rd).unwrap()
);
runtime += delay;
runtime_secs.set(runtime.as_secs());
if runtime_secs.read_tripped().is_some() {
events.send(Measurement::SimulationProgress(SensorSource::GPS, runtime, 0.0)).await;
}
Timer::after(delay).await;
events.send(Measurement::GPS(Some(coords))).await;
}
events.send(Measurement::SensorOffline(SensorSource::GPS)).await;
warn!("End of GPS recording");
}

369
src/tasks/ui.rs
View File

@@ -1,272 +1,243 @@
use embassy_sync::channel::DynamicReceiver;
use embassy_time::{Duration, Instant, Timer};
use figments::{liber8tion::interpolate::Fract8, prelude::*};
use embassy_time::Duration;
use figments::prelude::*;
use rgb::{Rgb, Rgba};
use log::*;
use nalgebra::ComplexField;
use crate::{display::BikeSpace, events::{DisplayControls, Notification, Scene}, shaders::*, tasks::render::Uniforms};
use alloc::sync::Arc;
use core::fmt::Debug;
use futures::join;
use crate::{animation::{AnimDisplay, AnimatedSurface, Animation}, display::{SegmentSpace, Uniforms}, events::{DisplayControls, Notification, Scene, SensorSource}, shaders::*};
pub struct Ui<S: Surface> {
// Background layer provides an always-running background for everything to draw on
background: S,
background: AnimatedSurface<S>,
// Tail and panels provide content
tail: S,
panels: S,
tail: AnimatedSurface<S>,
panels: AnimatedSurface<S>,
motion: AnimatedSurface<S>,
// Notification layer sits on top of the content, and is used for transient event notifications (gps lost, wifi found, etc)
notification:S,
notification: AnimatedSurface<S>,
// Headlight and brakelight layers can only be overpainted by the bootsplash overlay layer
headlight: S,
brakelight: S,
headlight: AnimatedSurface<S>,
brakelight: AnimatedSurface<S>,
// The overlay covers everything and is used to implement a power-on and power-off animation.
overlay:S,
headlight_on: bool,
brakelight_on: bool,
display: &'static DisplayControls
overlay: AnimatedSurface<S>,
display: Arc<DisplayControls>
}
impl<S: Surface<Uniforms = Uniforms, CoordinateSpace = BikeSpace, Pixel = Rgba<u8>>> Ui<S> {
pub fn new(surfaces: &mut impl Surfaces<S::CoordinateSpace, Surface = S>, display: &'static DisplayControls) -> Self {
impl<S: Debug + Surface<Uniforms = Uniforms, CoordinateSpace = SegmentSpace, Pixel = Rgba<u8>>> Ui<S> {
pub fn new<SS: Surfaces<SegmentSpace, Surface = S>>(surfaces: &mut SS, display: Arc<DisplayControls>) -> Self where SS::Error: Debug {
Self {
background: SurfaceBuilder::build(surfaces)
.rect(Rectangle::everything())
.shader(Background::default())
.opacity(32)
.visible(false)
.finish().unwrap(),
.finish().unwrap().into(),
tail: SurfaceBuilder::build(surfaces)
.rect(Rectangle::new_from_coordinates(0, 5, 255, 5))
.opacity(96)
.shader(Tail::default())
.visible(false)
.finish().unwrap(),
.finish().unwrap().into(),
panels: SurfaceBuilder::build(surfaces)
.rect(Rectangle::new_from_coordinates(0, 1, 255, 4))
.opacity(128)
.shader(Panel::default())
.visible(false)
.finish().unwrap(),
.finish().unwrap().into(),
motion: SurfaceBuilder::build(surfaces)
.rect(Rectangle::everything())
.shader(Movement::default())
.visible(false)
.finish().unwrap().into(),
notification: SurfaceBuilder::build(surfaces)
.rect(Rectangle::everything())
.shader(Background::default())
.visible(false)
.finish().unwrap(),
.finish().unwrap().into(),
// FIXME: Headlight, brakelight, and the overlay should all be handled as part of the safety UI
headlight: SurfaceBuilder::build(surfaces)
.rect(Rectangle::new_from_coordinates(0, 0, 255, 0))
.shader(Headlight::default())
.visible(false)
.finish().unwrap(),
.finish().unwrap().into(),
brakelight: SurfaceBuilder::build(surfaces)
.rect(Brakelight::rectangle())
.shader(Brakelight::default())
.visible(false)
.finish().unwrap(),
.finish().unwrap().into(),
overlay: SurfaceBuilder::build(surfaces)
.rect(Rectangle::everything())
.visible(true)
.opacity(0)
.shader(Thinking::default())
.finish().unwrap(),
headlight_on: false,
brakelight_on: false,
.visible(false)
.finish().unwrap().into(),
display
}
}
pub async fn flash_notification_color(&mut self, color: Rgb<u8>) {
self.notification.set_shader(Background::from_color(color));
self.notification.set_opacity(0);
let fade_in = Animation::default().from(0).to(255).duration(Duration::from_millis(30));
let pulse_out = Animation::default().from(255).to(60).duration(Duration::from_millis(100));
let pulse_in = Animation::default().from(0).to(255).duration(Duration::from_millis(100));
let fade_out = Animation::default().from(255).to(0).duration(Duration::from_secs(2));
info!("Flashing notification {color}");
self.notification.set_visible(true);
// Fade in to full on over 1s
Self::animate_duration(Duration::from_secs(1), |pct| {
self.notification.set_opacity(pct);
}).await;
// Pulse quickly 3 times
for _ in 0..3 {
// Brief dimming back to half brightness for half a sec
Self::animate_duration(Duration::from_millis(100), |pct| {
self.notification.set_opacity(100 + pct/2);
}).await;
self.notification.set_shader(Background::from_color(color));
// Back to full
Self::animate_duration(Duration::from_millis(100), |pct| {
self.notification.set_opacity(100 + (255 - pct) / 2);
}).await;
fade_in.apply(&mut self.notification).await;
// Pulse quickly 5 times
for _ in 0..5 {
pulse_out.apply(&mut self.notification).await;
pulse_in.apply(&mut self.notification).await;
}
// Back to off
Self::animate_duration(Duration::from_secs(3), |pct| {
self.notification.set_opacity(255 - pct);
}).await;
fade_out.apply(&mut self.notification).await;
self.notification.set_visible(false);
}
async fn animate_duration<F: FnMut(Fract8)>(duration: Duration, mut f: F) {
let start = Instant::now();
let end = start + duration;
let full_duration = duration.as_millis() as f64;
loop {
let now = Instant::now();
if now > end {
break
} else {
let pct = (now - start).as_millis() as f64 / full_duration;
let frac_pct = (255.0 * pct) as u8;
f(frac_pct);
Timer::after_millis(5).await;
}
}
pub async fn sleep(&mut self) {
info!("Running sleep sequence");
let fade_out = Animation::default().duration(Duration::from_secs(1)).from(255).to(0);
let mut disp_anim = AnimDisplay(&self.display);
fade_out.apply(&mut disp_anim).await;
// Reset layers to the initial state now that the display should be off
[
&mut *self.tail,
&mut *self.panels,
&mut *self.background,
&mut *self.motion
].set_opacity(0);
[
&mut *self.tail,
&mut *self.panels,
&mut *self.background,
&mut *self.motion
].set_visible(false);
info!("Turning off display");
self.display.set_on(false);
// Wait for the display hardware to actually turn off, before we return to process the next event, which could cause funky behaviors.
// FIXME: also deadlocks :(
//self.display.render_is_running.wait().await;
info!("Display is now sleeping.");
}
pub fn expo_in(t: f32) -> f32 {
if t <= 0.0 {
0.0
} else {
2f32.powf(10.0 * t - 10.0)
}
}
pub fn expo_out(t: f32) -> f32 {
if 1.0 <= t {
1.0
} else {
1.0 - 2f32.powf(-10.0 * t)
}
}
pub async fn startup_fade_sequence(&mut self) {
pub async fn wakeup(&mut self) {
info!("Running startup fade sequence");
// Start with a completely transparent overlay, which then fades in over 1 second
self.overlay.set_opacity(0);
info!("Turning on display");
// Turn on the display hardware
self.display.set_brightness(0);
self.display.set_on(true);
// Wait for the renderer to start running again
// FIXME: This deadlocks :(
//self.display.render_is_running.wait().await;
let fade_in = Animation::default().duration(Duration::from_secs(3)).from(0).to(255);
let fade_out = Animation::default().duration(Duration::from_secs(1)).from(255).to(0);
let mut disp_anim = AnimDisplay(&self.display);
info!("Fade in overlay");
self.overlay.set_visible(true);
Self::animate_duration(Duration::from_secs(2), |pct| {
self.overlay.set_opacity((Self::expo_in(pct as f32 / 255.0) * 255.0) as u8);
}).await;
// When the overlay is fully opaque and all lower layers will be hidden, turn them on
self.apply_scene(Scene::Startup).await;
Timer::after_secs(1).await;
// Then fade out the overlay over 3 seconds, allowing the base animations to be shown
Self::animate_duration(Duration::from_secs(1), |pct| {
self.overlay.set_opacity((Self::expo_in((255 - pct) as f32 / 255.0) * 255.0) as u8);
}).await;
join!(
fade_in.apply(&mut self.overlay),
fade_in.apply(&mut disp_anim)
);
info!("Flipping on surfaces");
// Flip on all the layers
[
&mut *self.panels,
&mut *self.tail,
&mut *self.background,
&mut *self.motion
].set_visible(true);
// Enter the startup scene
self.apply_scene(Scene::Ready).await;
info!("Fade out overlay");
fade_out.apply(&mut self.overlay).await;
self.overlay.set_visible(false);
info!("Fade sequence completed");
}
pub async fn set_headlight_on(&mut self, is_on: bool) {
if self.headlight_on != is_on {
self.brakelight_on = is_on;
if is_on {
info!("Turning on headlight");
self.headlight.set_opacity(0);
self.headlight.set_visible(true);
Self::animate_duration(Duration::from_secs(1), |pct| {
self.headlight.set_opacity(pct);
}).await;
self.headlight.set_opacity(255);
} else {
info!("Turning off headlight");
Self::animate_duration(Duration::from_secs(1), |pct| {
self.headlight.set_opacity(255 - pct);
}).await;
self.headlight.set_visible(false);
}
}
}
pub async fn set_brakelight_on(&mut self, is_on: bool) {
if self.brakelight_on != is_on {
self.brakelight_on = is_on;
if is_on {
info!("Turning on brakelight");
self.brakelight.set_opacity(0);
self.brakelight.set_visible(true);
Self::animate_duration(Duration::from_millis(300), |pct| {
self.brakelight.set_opacity(pct);
}).await;
self.brakelight.set_opacity(255);
} else {
info!("Turning off brakelight");
Self::animate_duration(Duration::from_millis(300), |pct| {
self.brakelight.set_opacity(255 - pct);
}).await;
self.brakelight.set_visible(false);
}
}
info!("Wakeup complete!");
}
// TODO: Brakelight should only be toggled when actually braking or stationary
pub async fn apply_scene(&mut self, next_scene: Scene) {
info!("Activating scene {next_scene:?}");
match next_scene {
Scene::Startup => {
self.display.on.store(true, core::sync::atomic::Ordering::Relaxed);
self.display.brightness.store(255, core::sync::atomic::Ordering::Relaxed);
self.background.set_visible(true);
self.tail.set_visible(true);
self.panels.set_visible(true);
Scene::Ready => {
let tail = Animation::default().duration(Duration::from_millis(300)).to(96);
let panels = Animation::default().duration(Duration::from_millis(300)).to(128);
let bg = Animation::default().duration(Duration::from_millis(300)).to(32);
let motion = Animation::default().duration(Duration::from_secs(1)).to(0);
join!(
tail.apply(&mut self.tail),
panels.apply(&mut self.panels),
bg.apply(&mut self.background),
motion.apply(&mut self.motion)
);
self.background.set_shader(Background::default());
},
Scene::ParkedIdle => {
// Ensure the display is on
self.display.on.store(true, core::sync::atomic::Ordering::Relaxed);
Self::animate_duration(Duration::from_secs(1), |pct| {
self.display.brightness.store(128.scale8(255 - pct), core::sync::atomic::Ordering::Relaxed);
}).await;
// Hide the content; only notifications will remain
self.background.set_visible(true);
self.tail.set_visible(false);
self.panels.set_visible(false);
},
Scene::ParkedLongTerm => {
// For long-term parking, we turn off the safety lights
self.set_headlight_on(false).await;
self.set_brakelight_on(false).await;
// Then we turn the display off completely
Self::animate_duration(Duration::from_secs(1), |pct| {
self.display.brightness.store(255 - pct, core::sync::atomic::Ordering::Relaxed);
}).await;
self.display.on.store(false, core::sync::atomic::Ordering::Relaxed);
Scene::Idle => {
// FIXME: The safety UI task should handle setting the display brightness to 50% here
self.background.set_shader(Thinking::default());
let fg_fade = Animation::default().duration(Duration::from_millis(300)).to(0);
let bg_fade = Animation::default().duration(Duration::from_millis(300)).to(128);
// FIXME: The scenes shouldn't be touching the brake/headlights at all here. In fact, they should be dealt with in a whole separate task from the main UI, maybe running on the motion prediction executor
join!(
fg_fade.apply(&mut self.tail),
fg_fade.apply(&mut self.panels),
bg_fade.apply(&mut self.background),
fg_fade.apply(&mut self.motion)
);
},
Scene::Accelerating => {
self.display.on.store(true, core::sync::atomic::Ordering::Relaxed);
self.display.brightness.store(255, core::sync::atomic::Ordering::Relaxed);
self.tail.set_shader(Thinking::default());
self.panels.set_shader(Panel::default());
self.motion.set_shader(Movement::default());
Animation::default().duration(Duration::from_secs(1)).to(255).apply(&mut self.motion).await;
},
Scene::Decelerating => {
self.display.on.store(true, core::sync::atomic::Ordering::Relaxed);
self.display.brightness.store(255, core::sync::atomic::Ordering::Relaxed);
self.panels.set_shader(Thinking::default());
self.tail.set_shader(Tail::default());
}
_ => {
warn!("Unimplemented scene {next_scene:?}!");
self.motion.set_shader(Movement::default().reversed());
Animation::default().duration(Duration::from_secs(1)).to(255).apply(&mut self.motion).await;
}
}
}
pub async fn on_event(&mut self, event: Notification) {
info!("UI Event: {event:?}");
match event {
// Apply the scene when it is changed
Notification::SceneChange(_) => (), // We already log this inside apply_scene()
evt => info!("UI event: {evt:?}")
}
match event {
// TODO: We probably also want some events to indicate when the ESP has no calibration data or otherwise needs re-calibrated and is waiting for the bike to stand still
Notification::SensorOnline(SensorSource::IMU) => self.flash_notification_color(Rgb::new(0, 255, 0)).await,
Notification::SensorOffline(SensorSource::GPS) => self.flash_notification_color(Rgb::new(255, 0, 0)).await,
Notification::SensorOnline(SensorSource::GPS) => self.flash_notification_color(Rgb::new(0, 255, 255)).await,
// Scene change
Notification::SceneChange(scene) => self.apply_scene(scene).await,
// TODO: We probably also want some events to indicate when the ESP has no calibration data or otherwise needs re-calibrated and is waiting for the bike to stand still
Notification::CalibrationComplete => self.flash_notification_color(Rgb::new(0, 255, 0)).await,
Notification::GPSLost => self.flash_notification_color(Rgb::new(255, 0, 0)).await,
Notification::GPSAcquired => self.flash_notification_color(Rgb::new(0, 255, 255)).await,
Notification::WifiConnected => self.flash_notification_color(Rgb::new(0, 0, 255)).await,
Notification::WifiDisconnected => self.flash_notification_color(Rgb::new(128, 0, 255)).await,
// Toggling head and brake lights
Notification::SetBrakelight(is_on) => self.set_brakelight_on(is_on).await,
Notification::SetHeadlight(is_on) => self.set_headlight_on(is_on).await
Notification::SetBrakelight(is_on) => self.brakelight.set_on(is_on).await,
Notification::SetHeadlight(is_on) => self.headlight.set_on(is_on).await,
Notification::Sleep => self.sleep().await,
Notification::WakeUp => self.wakeup().await,
Notification::SensorsOffline => {
self.flash_notification_color(Rgb::new(255, 0, 0)).await;
self.flash_notification_color(Rgb::new(0, 255, 0)).await;
self.flash_notification_color(Rgb::new(0, 0, 255)).await;
}
_ => ()
// Other event ideas:
// - Bike has crashed, or is laid down
@@ -281,23 +252,23 @@ impl<S: Surface<Uniforms = Uniforms, CoordinateSpace = BikeSpace, Pixel = Rgba<u
}
}
#[cfg(feature="headless")]
pub type UiSurfacePool = NullBufferPool<NullSurface<Uniforms, SegmentSpace, Rgba<u8>>>;
#[cfg(not(feature="headless"))]
pub type UiSurfacePool = BufferedSurfacePool<Uniforms, SegmentSpace, Rgba<u8>>;
#[embassy_executor::task]
pub async fn ui_main(events: DynamicReceiver<'static, Notification>, mut ui: Ui<BufferedSurface<Uniforms, BikeSpace, Rgba<u8>>>) {
pub async fn ui_main(events: DynamicReceiver<'static, Notification>, mut ui: Ui<<UiSurfacePool as Surfaces<SegmentSpace>>::Surface>) {
// Wait for the renderer to start running
ui.display.render_is_running.wait().await;
//ui.display.render_is_running.wait().await;
// Run the fade sequence
ui.startup_fade_sequence().await;
// Run the wake sequence, and turn on the lights
ui.wakeup().await;
// FIXME: Moving the safety lights into another task will let us turn them both on in parallel with the wakeup sequence
ui.headlight.set_on(true).await;
ui.brakelight.set_on(true).await;
// Briefly turn on the brakelight while the headlight turns on
ui.set_brakelight_on(true).await;
// Turn on the headlight when we are finished
ui.set_headlight_on(true).await;
// Turn off the brakelight
ui.set_brakelight_on(false).await;
// Flash white to indicate we are ready
ui.flash_notification_color(Rgb::new(255, 255, 255)).await;
// Enter the event loop
loop {
ui.on_event(events.receive().await).await;
}

147
src/tasks/wifi.rs Normal file
View File

@@ -0,0 +1,147 @@
use bleps::{ad_structure::{create_advertising_data, AdStructure, BR_EDR_NOT_SUPPORTED, LE_GENERAL_DISCOVERABLE}, attribute_server::{AttributeServer, NotificationData}, gatt, Ble, HciConnector};
use embassy_sync::channel::DynamicReceiver;
use embassy_time::Timer;
use esp_hal::timer::AnyTimer;
use esp_wifi::ble::controller::BleConnector;
use log::*;
use crate::events::Notification;
pub async fn ble_task(notify: DynamicReceiver<'static, Notification>, wifi_init: &esp_wifi::EspWifiController<'_>, bluetooth_device: esp_hal::peripherals::BT<'static>) {
info!("Setting up BLE stack");
let connector = BleConnector::new(wifi_init, bluetooth_device);
let get_millis = || esp_hal::time::Instant::now().duration_since_epoch().as_millis();
let hci = HciConnector::new(connector, get_millis);
let mut ble = Ble::new(&hci);
ble.init().unwrap();
ble.cmd_set_le_advertising_parameters().unwrap();
ble.cmd_set_le_advertising_data(
create_advertising_data(&[
AdStructure::Flags(LE_GENERAL_DISCOVERABLE | BR_EDR_NOT_SUPPORTED),
AdStructure::ServiceUuids16(&[Uuid::Uuid16(0x0001)]),
AdStructure::CompleteLocalName("Renderbug!")
])
.unwrap()
).unwrap();
ble.cmd_set_le_advertise_enable(true).unwrap();
let mut wf1 = |_offset: usize, data: &[u8]| {
info!("Read serial data! {data:?}");
};
// Other useful characteristics:
// 0x2A67 - Location and speed
// 0x2A00 - Device name
// 0x2B90 - Device time
// Permitted characteristics:
// Acceleration
// Force
// Length
// Linear position
// Rotational speed
// Temperature
// Torque
// Useful app that logs data: https://github.com/a2ruan/ArduNetApp?tab=readme-ov-file
// Requires service 4fafc201-1fb5-459e-8fcc-c5c9c331914b, characteristic beb5483e-36e1-4688-b7f5-ea07361b26a8
let s = &b""[..];
gatt!([service {
uuid: "6E400001-B5A3-F393-E0A9-E50E24DCCA9E", // Nordic UART
characteristics: [
characteristic {
uuid: "6E400003-B5A3-F393-E0A9-E50E24DCCA9E", // TX from device, everything is sent as notifications
notify: true,
name: "tx",
value: s
},
characteristic {
uuid: "6E400002-B5A3-F393-E0A9-E50E24DCCA9E", // RX from phone
write: wf1
},
]
}]);
let mut rng = bleps::no_rng::NoRng;
let mut srv = AttributeServer::new(&mut ble, &mut gatt_attributes, &mut rng);
info!("BLE running!");
// TODO: Somehow need to recreate the attributeserver after disconnecting?
loop {
let notification = match notify.try_receive() {
Err(_) => None,
Ok(Notification::Beat) => Some("beat"),
//TODO: Should make Telemetry values serde-encodable
/*Ok(Telemetry::Measurement(Measurement::IMU { accel, gyro })) => {
let json_data = json!({
"x": accel.x,
"y": accel.y,
"z": accel.z,
"gx": gyro.x,
"gy": gyro.y,
"gz": gyro.z
});
let json_buf = serde_json::to_string(&json_data).unwrap();
Some(json_buf)
},
Ok(Telemetry::Measurement(Measurement::GPS(Some(measurement)))) => {
info!("gps telemetry");
let json_data = json!({
"lat": measurement.x,
"lng": measurement.y
});
let json_buf = serde_json::to_string(&json_data).unwrap();
Some(json_buf)
},*/
_ => None
};
match notification {
None => {
srv.do_work().unwrap();
Timer::after_millis(5).await;
},
Some(serial_data) => {
for chunk in serial_data.as_bytes().chunks(20) {
srv.do_work_with_notification(Some(NotificationData::new(tx_handle, chunk))).unwrap();
}
srv.do_work_with_notification(Some(NotificationData::new(tx_handle, &b"\n"[..]))).unwrap();
}
}
}
}
// TODO: Wifi task needs to know when there is data to upload, so it only connects when needed.
#[embassy_executor::task]
pub async fn wireless_task(notify: DynamicReceiver<'static, Notification>, timer: AnyTimer<'static>, rng: esp_hal::peripherals::RNG<'static>, _wifi_device: esp_hal::peripherals::WIFI<'static>, bluetooth_device: esp_hal::peripherals::BT<'static>) {
let rng = esp_hal::rng::Rng::new(rng);
let wifi_init =
esp_wifi::init(timer, rng).expect("Failed to initialize WIFI/BLE controller");
ble_task(notify, &wifi_init, bluetooth_device).await;
/*
loop {
let (mut wifi, _interfaces) = esp_wifi::wifi::new(&wifi_init, wifi_device)
.expect("Failed to initialize WIFI controller"); }
loop {
//let results = wifi.scan_n_async(16).await.unwrap();
wifi.set_configuration(&esp_wifi::wifi::Configuration::Client(
ClientConfiguration {
ssid: "The Frequency".to_string(),
auth_method: esp_wifi::wifi::AuthMethod::WPA2Personal,
password: "thepasswordkenneth".to_string(),
..Default::default()
}
)).unwrap();
if wifi.connect_async().await.is_ok() {
info!("Connected to wifi!");
while wifi.is_connected().unwrap() {
Timer::after_secs(60).await;
}
info!("Disconnected.");
}
Timer::after_secs(30).await;
}
}
*/
}