diff --git a/firmware/.env.template b/firmware/.env.template index c5fe6026..47624783 100644 --- a/firmware/.env.template +++ b/firmware/.env.template @@ -12,5 +12,4 @@ DEFMT_LOG="info" # Provide this if you have a particular board you want to use. Note that relative paths # are resolved from the same folder as `build.rs`. -BOARD="boards/ferrous_slime.toml" - +BOARD="boards/dev.toml" diff --git a/firmware/.gitignore b/firmware/.gitignore index 1429456d..ba2e1e6c 100644 --- a/firmware/.gitignore +++ b/firmware/.gitignore @@ -3,3 +3,4 @@ *.bin *.uf2 \.env +/boards/dev.toml # convenient for testing out settings locally diff --git a/firmware/.vscode/launch.json b/firmware/.vscode/launch.json index 13ab8acd..bc2af7ac 100644 --- a/firmware/.vscode/launch.json +++ b/firmware/.vscode/launch.json @@ -13,15 +13,16 @@ "resetAfterFlashing": true, "haltAfterReset": true, }, - "runtimeExecutable": "/Users/ryan.butler/.cargo/bin/probe-rs-debugger", - "consoleLogLevel": "Debug", + "connectUnderReset": true, + "runtimeExecutable": "/home/ryan/.cargo/bin/probe-rs-debugger", + // "consoleLogLevel": "Trace", "name": "probe_rs Executable Test", - "chip": "nrf52840", //!MODIFY - "wireProtocol": "Swd", + "chip": "esp32c3", //!MODIFY + "wireProtocol": "Jtag", "cwd": "${workspaceFolder}", "coreConfigs": [ { - "programBinary": "${workspaceFolder}/target/thumbv7em-none-eabihf/debug/firmware", //!MODIFY + "programBinary": "${workspaceFolder}/target/riscv32imc-unknown-none-elf/debug/firmware", //!MODIFY "rttEnabled": true, } ] diff --git a/firmware/Cargo.lock b/firmware/Cargo.lock index a1ab5cd7..c58d21a2 100644 --- a/firmware/Cargo.lock +++ b/firmware/Cargo.lock @@ -177,6 +177,14 @@ dependencies = [ "embedded-hal 0.2.7", ] +[[package]] +name = "bno080" +version = "0.1.3" +source = "git+https://github.com/tstellanova/bno080?rev=f95e860#f95e860cd708c5d9a3612b6921bff987188200a3" +dependencies = [ + "embedded-hal 0.2.7", +] + [[package]] name = "bytemuck" version = "1.12.3" @@ -449,6 +457,12 @@ version = "0.15.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "77c90badedccf4105eca100756a0b1289e191f6fcbdadd3cee1d2f614f97da8f" +[[package]] +name = "dotenvy" +version = "0.15.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "03d8c417d7a8cb362e0c37e5d815f5eb7c37f79ff93707329d5a194e42e54ca0" + [[package]] name = "embassy-cortex-m" version = "0.1.0" @@ -906,6 +920,7 @@ dependencies = [ "alloc-cortex-m", "bleps", "bmi160", + "bno080", "cfg_aliases", "color-eyre", "cortex-m", @@ -914,6 +929,7 @@ dependencies = [ "defmt-bbq", "defmt-rtt", "defmt_esp_println", + "dotenvy", "embassy-executor", "embassy-futures 0.1.0 (registry+https://github.com/rust-lang/crates.io-index)", "embassy-nrf", diff --git a/firmware/Cargo.toml b/firmware/Cargo.toml index 5917882b..af95c8de 100644 --- a/firmware/Cargo.toml +++ b/firmware/Cargo.toml @@ -14,7 +14,7 @@ rust-version.workspace = true [features] -default = ["mcu-esp32c3", "imu-mpu6050", "log-rtt", "net-wifi"] +default = ["mcu-esp32c3", "imu-bno080", "log-rtt", "net-wifi"] # default = [ # "mcu-nrf52840", # "imu-stubbed", @@ -67,6 +67,7 @@ net-stubbed = [] # Stubs out network # Supported IMUs imu-bmi160 = ["dep:bmi160"] imu-mpu6050 = ["dep:mpu6050-dmp"] +imu-bno080 = ["dep:bno080"] imu-stubbed = [] # Stubs out the IMU # Supported defmt loggers @@ -193,6 +194,7 @@ defmt-bbq = { version = "0.1", optional = true } # Peripheral drivers mpu6050-dmp = { version = "0.2", optional = true } bmi160 = { version = "0.1", optional = true } +bno080 = { version = "0.1", optional = true } # Other crates static_cell = "1" @@ -205,8 +207,8 @@ fugit = "0.3" firmware_protocol = { path = "../networking/firmware_protocol", features = [ "nalgebra031", ] } -load-dotenv = "0.1" paste = "1.0" +load-dotenv = "0.1" [build-dependencies] feature_utils = "0.0.0" @@ -215,12 +217,14 @@ serde = { version = "1", features = ["derive"] } toml = "0.5" eyre = "0.6" color-eyre = "0.6" - +dotenvy = "0.15" [patch.crates-io] # Enable non-default I2C addresses and InitError mpu6050-dmp = { git = "https://github.com/TheButlah/mpu6050-dmp-rs", rev = "77128a75a51547baadc0f0ef99aba4432e848f10" } bmi160 = { git = "https://github.com/TheButlah/bmi160-rs", rev = "e99802b" } +# has a way to release i2c that isn't published +bno080 = { git = "https://github.com/TheButlah/bno080", rev = "f95e860" } # Emable use of `atomic-polyfill` defmt = { git = "https://github.com/TheButlah/defmt", rev = "b5d5e316ea56250dcb25b3549bcf4bfdb1687796" } diff --git a/firmware/boards/dev.toml b/firmware/boards/dev.toml new file mode 100644 index 00000000..4546c9d7 --- /dev/null +++ b/firmware/boards/dev.toml @@ -0,0 +1,7 @@ +[pins] +scl = "2" +sda = "4" +int0 = "6" +int1 = "7" +tx = "8" +rx = "9" diff --git a/firmware/build.rs b/firmware/build.rs index b839cc82..cf0859ae 100644 --- a/firmware/build.rs +++ b/firmware/build.rs @@ -8,7 +8,7 @@ use std::{ }; mandatory_and_unique!("mcu-esp32", "mcu-esp32c3", "mcu-nrf52832", "mcu-nrf52840"); -mandatory_and_unique!("imu-stubbed", "imu-mpu6050", "imu-bmi160"); +mandatory_and_unique!("imu-stubbed", "imu-bno080", "imu-mpu6050", "imu-bmi160"); mandatory_and_unique!("log-rtt", "log-usb-serial", "log-uart"); mandatory_and_unique!("net-wifi", "net-ble", "net-stubbed"); @@ -33,6 +33,7 @@ macro_rules! memory_x { } fn main() -> Result<()> { + let _ = dotenvy::dotenv(); #[cfg(all(feature = "mcu-nrf52832", feature = "log-usb-serial"))] compile_error!("the nrf52832 doesn't support USB!"); @@ -146,7 +147,8 @@ struct Pins { impl BoardConfig { /// Loads a board config from a file fn from_file(p: &Path) -> Result { - let s = std::fs::read_to_string(p).wrap_err("Failed to read board toml")?; + let s = std::fs::read_to_string(p.clone()) + .wrap_err(format!("Failed to read board toml: {}", p.display()))?; toml::from_str(&s).wrap_err("Failed to deserialize board toml") } /// Gets the path to the board config, or errors if we can't pick one. diff --git a/firmware/rustfmt.toml b/firmware/rustfmt.toml new file mode 120000 index 00000000..39f97b04 --- /dev/null +++ b/firmware/rustfmt.toml @@ -0,0 +1 @@ +../rustfmt.toml \ No newline at end of file diff --git a/firmware/src/aliases.rs b/firmware/src/aliases.rs index 936cb36c..9454f413 100644 --- a/firmware/src/aliases.rs +++ b/firmware/src/aliases.rs @@ -51,14 +51,27 @@ pub mod ඞ { pub trait I2c: embedded_hal::blocking::i2c::Write::Error> + embedded_hal::blocking::i2c::WriteRead::Error> + + embedded_hal::blocking::i2c::Read::Error> { type Error: core::fmt::Debug; } impl< T: embedded_hal::blocking::i2c::Write - + embedded_hal::blocking::i2c::WriteRead, + + embedded_hal::blocking::i2c::WriteRead + + embedded_hal::blocking::i2c::Read, E: core::fmt::Debug, > I2c for T { type Error = E; } + +pub trait Delay: + embedded_hal::blocking::delay::DelayMs + embedded_hal::blocking::delay::DelayMs +{ +} +impl< + T: embedded_hal::blocking::delay::DelayMs + + embedded_hal::blocking::delay::DelayMs, + > Delay for T +{ +} diff --git a/firmware/src/imu/bno080.rs b/firmware/src/imu/bno080.rs new file mode 100644 index 00000000..ca5e0917 --- /dev/null +++ b/firmware/src/imu/bno080.rs @@ -0,0 +1,127 @@ +use crate::aliases::{Delay, I2c}; +use crate::imu::{Imu, Quat}; +use crate::utils; + +use bno080::interface::I2cInterface; +use bno080::interface::SensorInterface; +use defmt::{debug, trace, warn}; +use firmware_protocol::ImuType; + +pub const IMU_REPORT_INTERVAL_MS: u16 = 10; +pub const I2C_ADDR: u8 = ::bno080::interface::i2c::ALTERNATE_ADDRESS; + +pub type DriverError = ::bno080::wrapper::WrapperError< + as SensorInterface>::SensorError, +>; + +struct Bno080 { + driver: ::bno080::wrapper::BNO080>, +} +impl Bno080 { + pub fn new( + i2c: I, + delay: &mut impl crate::aliases::Delay, + ) -> Result> { + let interface = ::bno080::interface::I2cInterface::new(i2c, I2C_ADDR); + let mut driver = ::bno080::wrapper::BNO080::new_with_interface(interface); + + let mut last_err = Ok(()); + for i in 0..4 { + delay.delay_ms(100u32); + if let Err(e) = driver.soft_reset() { + warn!( + "Error resetting bno080 (atmpt {}): {:?}", + i + 1, + defmt::Debug2Format(&e) + ); + last_err = Err(e); + } else { + last_err = Ok(()); + break; + } + } + last_err?; + + let mut last_err = Ok(()); + for i in 0..4 { + delay.delay_ms(500u32); + if let Err(e) = driver.init(delay) { + warn!( + "Error initializing bno080 (atmpt {}): {:?}", + i + 1, + defmt::Debug2Format(&e) + ); + last_err = Err(e); + } else { + last_err = Ok(()); + break; + } + } + last_err?; + + let mut last_err = Ok(()); + for i in 0..4 { + if let Err(e) = driver.enable_rotation_vector(IMU_REPORT_INTERVAL_MS) { + warn!( + "Error enabling rotation (atmpt {}): {:?}", + i + 1, + defmt::Debug2Format(&e) + ); + last_err = Err(e); + } else { + last_err = Ok(()); + break; + } + } + last_err?; + + // utils::retry( + // 4, + // (), + // |_| -> Result<(), ((), DriverError)> { + // // delay.delay_ms(400u32); + // // trace!("Flushing comms"); + // // let _ = i2c.write(I2C_ADDR, &[0]); + // + // // delay.delay_ms(400u32); + // trace!("Constructing IMU"); + // + // // if let Err(e) = driver.init(delay) { + // // return Err((driver.free().free(), e)); + // // } + // debug!("Initialized bno080 driver"); + // // delay.delay_ms(100u32); + // // if let Err(e) = driver.enable_rotation_vector(IMU_REPORT_INTERVAL_MS) { + // // return Err((driver.free().free(), e)); + // // } + // driver + // .enable_rotation_vector(IMU_REPORT_INTERVAL_MS) + // .map_err(|e| ((), e))?; + // debug!("Enabled rotation vector"); + // Ok(()) + // }, + // |i| warn!("Retrying IMU connection (attempts so far: {})", i + 1), + // ) + // .map_err(|(_, e)| panic!("{:?}", e))?; + Ok(Self { driver }) + } +} + +impl Imu for Bno080 { + type Error = DriverError; + + const IMU_TYPE: ImuType = ImuType::Bno080; + + fn quat(&mut self) -> nb::Result { + let [i, j, k, w] = self.driver.rotation_quaternion()?; + let q = nalgebra::Quaternion { + coords: nalgebra::vector![i, j, k, w], + }; + // TODO: This is already normalized, we can use unsafe for performance + Ok(Quat::from_quaternion(q)) + } +} + +pub fn new_imu(i2c: impl I2c, delay: &mut impl Delay) -> impl Imu { + Bno080::new(i2c, delay).expect("Failed to initialize bno080") +} diff --git a/firmware/src/imu/mod.rs b/firmware/src/imu/mod.rs index f5e6a975..a8b1cbf5 100644 --- a/firmware/src/imu/mod.rs +++ b/firmware/src/imu/mod.rs @@ -13,10 +13,13 @@ mod ඞ; #[path = "bmi160/mod.rs"] mod ඞ; +#[cfg(feature = "imu-bno080")] +#[path = "bno080.rs"] +mod ඞ; + use defmt::{debug, info, trace, warn}; use embassy_executor::task; use embassy_futures::yield_now; -use embedded_hal::blocking::delay::DelayMs; use firmware_protocol::ImuType; use crate::{ @@ -49,7 +52,7 @@ pub async fn imu_task( async fn imu_task_inner( quat_signal: &Unreliable, i2c: impl crate::aliases::I2c, - mut delay: impl DelayMs, + mut delay: impl crate::aliases::Delay, ) -> ! { debug!("Imu task"); let mut imu = ඞ::new_imu(i2c, &mut delay); diff --git a/firmware/src/imu/mpu6050.rs b/firmware/src/imu/mpu6050.rs index 4887587e..328dad6f 100644 --- a/firmware/src/imu/mpu6050.rs +++ b/firmware/src/imu/mpu6050.rs @@ -42,7 +42,7 @@ impl Mpu6050 { fifo_buf: [0; 28], }) }, - |i| debug!("Retrying IMU connection (attempts so far: {})", i + 1), + |i| warn!("Retrying IMU connection (attempts so far: {})", i + 1), ) // Map converts from tuple -> struct .map_err(|(i2c, error)| InitError { i2c, error }) diff --git a/firmware/src/main.rs b/firmware/src/main.rs index f89dc5ec..f1dd8971 100644 --- a/firmware/src/main.rs +++ b/firmware/src/main.rs @@ -24,13 +24,8 @@ mod bbq_logger; use defmt::debug; use embassy_executor::Executor; -use embedded_hal::blocking::delay::DelayMs; use static_cell::StaticCell; -use crate::imu::Quat; -use crate::networking::protocol::Packets; -use crate::utils::Unreliable; - #[cfg(cortex_m)] use cortex_m_rt::entry; #[cfg(riscv)] @@ -40,6 +35,11 @@ use xtensa_lx_rt::entry; #[entry] fn main() -> ! { + use crate::imu::Quat; + use crate::networking::protocol::Packets; + use crate::utils::Unreliable; + use embedded_hal::blocking::delay::DelayMs; + #[cfg(bbq)] let bbq = defmt_bbq::init().unwrap(); diff --git a/firmware/src/peripherals/esp32c3.rs b/firmware/src/peripherals/esp32c3.rs index 33ae22a2..6dff0fb6 100644 --- a/firmware/src/peripherals/esp32c3.rs +++ b/firmware/src/peripherals/esp32c3.rs @@ -63,12 +63,13 @@ pub fn get_peripherals() -> Peripherals, DelayConcrete> { } let io = esp32c3_hal::IO::new(p.GPIO, p.IO_MUX); - // let hz = + let sda = map_pin!(io, env!("PIN_SDA")); + let scl = map_pin!(io, env!("PIN_SCL")); let i2c = esp32c3_hal::i2c::I2C::new( p.I2C0, - map_pin!(io, env!("PIN_SDA")), - map_pin!(io, env!("PIN_SCL")), - 400u32.kHz(), + sda, + scl, + 100u32.kHz(), &mut system.peripheral_clock_control, &clocks, );