diff --git a/Cargo.toml b/Cargo.toml
index 930bb5fad914929b54594dfbbc5948f6350c359f..f1dd0c7157ce4899fcabfff0c570a4a0307ec593 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -1,6 +1,6 @@
[package]
name = "tudelft-quadrupel"
-version = "2.0.1"
+version = "2.0.2"
edition = "2021"
authors = [
"Anne Stijns <anstijns@gmail.com>",
diff --git a/src/barometer.rs b/src/barometer.rs
index 41174bcaaceb43c7ace605a48e3bb90863d0b01f..80d64aafd798bb37ff7957066c57fe7e6def393e 100644
--- a/src/barometer.rs
+++ b/src/barometer.rs
@@ -3,7 +3,6 @@ use crate::once_cell::OnceCell;
use crate::time::Instant;
use crate::twi::TWI;
use core::time::Duration;
-use embedded_hal::prelude::_embedded_hal_blocking_i2c_WriteRead;
const MS5611_ADDR: u8 = 0b0111_0111;
const REG_READ: u8 = 0x0;
@@ -80,29 +79,29 @@ struct Ms5611 {
static BAROMETER: Mutex<OnceCell<Ms5611>> = Mutex::new(OnceCell::uninitialized());
pub(crate) fn initialize() {
- TWI.modify(|twi| {
- let mut prom = [0; 8];
- let mut data = [0u8; 2];
- for c in 0..8 {
- twi.write_read(MS5611_ADDR, &[REG_PROM + 2 * c], &mut data)
- .unwrap();
- prom[c as usize] = u16::from_be_bytes(data);
- }
+ // Safety: The TWI mutex is not accessed in an interrupt
+ let twi = unsafe { TWI.no_critical_section_lock_mut() };
- BAROMETER.modify(|baro| {
- baro.initialize(Ms5611 {
- pressure_sensitivity: prom[1],
- pressure_offset: prom[2],
- temp_coef_pressure_sensitivity: prom[3],
- temp_coef_pressure_offset: prom[4],
- temp_ref: prom[5],
- temp_coef: prom[6],
- over_sampling_ratio: OverSamplingRatio::Opt4096,
- loop_state: Ms5611LoopState::Reset,
- most_recent_pressure: 0,
- most_recent_temperature: 0,
- })
- })
+ let mut prom = [0; 8];
+ let mut data = [0u8; 2];
+ for c in 0..8 {
+ _ = twi.read(MS5611_ADDR, REG_PROM + 2 * c, &mut data);
+ prom[c as usize] = u16::from_be_bytes(data);
+ }
+
+ BAROMETER.modify(|baro| {
+ baro.initialize(Ms5611 {
+ pressure_sensitivity: prom[1],
+ pressure_offset: prom[2],
+ temp_coef_pressure_sensitivity: prom[3],
+ temp_coef_pressure_offset: prom[4],
+ temp_ref: prom[5],
+ temp_coef: prom[6],
+ over_sampling_ratio: OverSamplingRatio::Opt4096,
+ loop_state: Ms5611LoopState::Reset,
+ most_recent_pressure: 0,
+ most_recent_temperature: 0,
+ });
});
}
@@ -118,9 +117,9 @@ fn update() {
//We let the chip know we want to read D1.
twi.write(
MS5611_ADDR,
- &[REG_D1 + baro.over_sampling_ratio.addr_modifier()],
- )
- .unwrap();
+ REG_D1 + baro.over_sampling_ratio.addr_modifier(),
+ &[],
+ );
//Then set loop state for next iteration
baro.loop_state = Ms5611LoopState::ReadD1 {
@@ -135,16 +134,15 @@ fn update() {
//Read D1
let mut buf = [0u8; 4];
- twi.write_read(MS5611_ADDR, &[REG_READ], &mut buf[1..4])
- .unwrap();
+ _ = twi.read(MS5611_ADDR, REG_READ, &mut buf[1..4]);
let d1 = u32::from_be_bytes(buf);
//We let the chip know we want to read D2.
twi.write(
MS5611_ADDR,
- &[REG_D2 + baro.over_sampling_ratio.addr_modifier()],
- )
- .unwrap();
+ REG_D2 + baro.over_sampling_ratio.addr_modifier(),
+ &[],
+ );
//Then set loop state for next iteration
baro.loop_state = Ms5611LoopState::ReadD2 {
@@ -160,8 +158,7 @@ fn update() {
//Read D2
let mut buf = [0u8; 4];
- twi.write_read(MS5611_ADDR, &[REG_READ], &mut buf[1..4])
- .unwrap();
+ _ = twi.read(MS5611_ADDR, REG_READ, &mut buf[1..4]);
let d1 = u64::from(d1);
let d2 = u64::from(u32::from_be_bytes(buf));
diff --git a/src/battery.rs b/src/battery.rs
index b0a0ac24e4877574c38b0df5f58bbcf00a8660cc..90b17e986683f939174b6c03ee21e4199d0d9216 100644
--- a/src/battery.rs
+++ b/src/battery.rs
@@ -50,7 +50,7 @@ unsafe fn ADC() {
adc.adc.events_end.reset();
// Battery voltage = (result*1.2*3/255*2) = RESULT*0.007058824
adc.last_result = adc.adc.result.read().result().bits() * 7;
- })
+ });
}
/// Returns the battery voltage in 10^-2 volt.
diff --git a/src/initialize.rs b/src/initialize.rs
index 429d28d4193b20643772effadcc6dfddc9bbce61..f4d140c9b275412cab0e4152ce1d86ea30daec0b 100644
--- a/src/initialize.rs
+++ b/src/initialize.rs
@@ -6,13 +6,14 @@ use crate::{barometer, battery, flash, led, motor, mpu, time, twi, uart};
use alloc_cortex_m::CortexMHeap;
use core::mem::MaybeUninit;
use nrf51_pac::Peripherals;
+
static INITIALIZED: Mutex<bool> = Mutex::new(false);
#[global_allocator]
static ALLOCATOR: CortexMHeap = CortexMHeap::empty();
/// Initialize the drone board. This should be run at boot.
-///
+//t/
/// `heap_memory` should be a pointer to statically allocated memory.
/// Care should be taken that the mutable reference given here *really* is the
/// only mutable reference to that area of memory. That should of course be guaranteed by
@@ -62,7 +63,12 @@ pub fn initialize(heap_memory: &'static mut [MaybeUninit<u8>], debug: bool) {
if debug {
let _ = send_bytes(b"RTC driver initialized\n");
}
- twi::initialize(nrf51_peripherals.TWI0, gpio.p0_04, gpio.p0_02);
+ twi::initialize(
+ nrf51_peripherals.TWI0,
+ gpio.p0_04,
+ gpio.p0_02,
+ &mut cortex_m_peripherals.NVIC,
+ );
if debug {
let _ = send_bytes(b"TWI initialized\n");
}
@@ -97,7 +103,6 @@ pub fn initialize(heap_memory: &'static mut [MaybeUninit<u8>], debug: bool) {
&mut cortex_m_peripherals.NVIC,
&mut nrf51_peripherals.PPI,
&mut nrf51_peripherals.GPIOTE,
- gpio.p0_20,
);
if debug {
let _ = send_bytes(b"MOTOR driver initialized\n");
diff --git a/src/lib.rs b/src/lib.rs
index d02fbaca8fdfb3f67205b791112d365ab0524c07..9fb9a39525ff686c8e6f15bc54c175715a9215d0 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -1,7 +1,7 @@
#![no_std]
#![feature(strict_provenance)]
#![deny(missing_docs)]
-#![deny(warnings)]
+// #![deny(warnings)]
#![deny(unused_import_braces)]
#![deny(unused_results)]
#![deny(trivial_casts)]
diff --git a/src/motor.rs b/src/motor.rs
index fc1c7dc97772b761aa8ec1ad52b29cf316635f91..2c95bf2bf8cf1fd4def75e88b92b112e8460201b 100644
--- a/src/motor.rs
+++ b/src/motor.rs
@@ -1,9 +1,6 @@
use crate::mutex::Mutex;
-use crate::nrf51_hal::prelude::OutputPin;
use crate::once_cell::OnceCell;
use cortex_m::peripheral::NVIC;
-use nrf51_hal::gpio::p0::P0_20;
-use nrf51_hal::gpio::{Disconnected, Level, Output, PushPull};
use nrf51_pac::{interrupt, Interrupt, GPIOTE, PPI};
struct Motors {
@@ -11,7 +8,6 @@ struct Motors {
motor_max: u16,
timer1: nrf51_pac::TIMER1,
timer2: nrf51_pac::TIMER2,
- pin20: P0_20<Output<PushPull>>,
}
const MOTOR_0_PIN: u8 = 21;
@@ -63,18 +59,13 @@ pub(crate) fn initialize(
nvic: &mut NVIC,
ppi: &mut PPI,
gpiote: &mut GPIOTE,
- pin20: P0_20<Disconnected>,
) {
- // Configure pin20
- let pin20 = pin20.into_push_pull_output(Level::Low);
-
MOTORS.modify(|motors| {
motors.initialize(Motors {
motor_values: [0; 4],
motor_max: 400,
timer1,
timer2,
- pin20,
});
// Configure GPIOTE. GPIOTE is stands for GPIO tasks and events.
@@ -270,11 +261,9 @@ unsafe fn TIMER1() {
motors.timer1.tasks_capture[2].write(|w| w.bits(1));
if motors.timer1.cc[2].read().bits() < 500 {
- motors.pin20.set_high().unwrap();
// Safety: Any time is allowed
motors.timer1.cc[0].write(|w| w.bits(u32::from(1000 + motors.motor_values[0])));
motors.timer1.cc[1].write(|w| w.bits(u32::from(1000 + motors.motor_values[1])));
- motors.pin20.set_low().unwrap();
}
}
}
diff --git a/src/mpu/error.rs b/src/mpu/error.rs
index f1550d0114b3dd299a645d58d05a7578e07f4b0e..98aae0eb19efd437582db57043021eee4dde7de9 100644
--- a/src/mpu/error.rs
+++ b/src/mpu/error.rs
@@ -1,29 +1,2 @@
-use core::fmt::Formatter;
-use embedded_hal::blocking::i2c::{Write, WriteRead};
-
-/// Error for sensor operations.
-pub enum Error<I2c>
-where
- I2c: WriteRead + Write,
- <I2c as WriteRead>::Error: core::fmt::Debug,
- <I2c as Write>::Error: core::fmt::Debug,
-{
- Write(<I2c as Write>::Error),
- WriteRead(<I2c as WriteRead>::Error),
- WrongDevice,
-}
-
-impl<I2c> core::fmt::Debug for Error<I2c>
-where
- I2c: WriteRead + Write,
- <I2c as WriteRead>::Error: core::fmt::Debug,
- <I2c as Write>::Error: core::fmt::Debug,
-{
- fn fmt(&self, f: &mut Formatter<'_>) -> Result<(), core::fmt::Error> {
- match self {
- Error::WriteRead(e) => f.debug_tuple("WriteReadError").field(e).finish(),
- Error::Write(e) => f.debug_tuple("WriteError").field(e).finish(),
- Error::WrongDevice => f.write_str("WrongDevice"),
- }
- }
-}
+#[derive(Debug)]
+pub enum Error {}
diff --git a/src/mpu/firmware_loader.rs b/src/mpu/firmware_loader.rs
index f35fdb9a53dd9c91a5f35699fef6784c36f2f999..3159f9424aa6d430fe9dee9f91ff8be23b8636b0 100644
--- a/src/mpu/firmware_loader.rs
+++ b/src/mpu/firmware_loader.rs
@@ -1,54 +1,44 @@
use crate::mpu::dmp_firmware::FIRMWARE;
-use crate::mpu::error::Error;
use crate::mpu::registers::Register;
use crate::mpu::sensor::Mpu6050;
-use embedded_hal::blocking::i2c::{Write, WriteRead};
+use crate::mpu::I2c;
const BANK_SIZE: usize = 256;
const CHUNK_SIZE: usize = 16;
-impl<I2c> Mpu6050<I2c>
-where
- I2c: Write + WriteRead,
- <I2c as WriteRead>::Error: core::fmt::Debug,
- <I2c as Write>::Error: core::fmt::Debug,
-{
- pub fn load_firmware(&mut self, i2c: &mut I2c) -> Result<(), Error<I2c>> {
+impl Mpu6050 {
+ pub fn load_firmware(&mut self, i2c: &mut I2c) {
self.write_memory(i2c, &FIRMWARE)
}
- pub fn boot_firmware(&mut self, i2c: &mut I2c) -> Result<(), Error<I2c>> {
- self.write(i2c, &[Register::PrgmStart as u8, 0x04, 0x00])
+ pub fn boot_firmware(&mut self, i2c: &mut I2c) {
+ self.write(i2c, Register::PrgmStart as u8, &[0x04, 0x00]);
}
- fn write_memory(&mut self, i2c: &mut I2c, data: &[u8]) -> Result<(), Error<I2c>> {
+ fn write_memory(&mut self, i2c: &mut I2c, data: &[u8]) {
for (bank, chunk) in data.chunks(BANK_SIZE).enumerate() {
- self.write_bank(i2c, bank as u8, chunk)?;
+ self.write_bank(i2c, bank as u8, chunk);
}
- Ok(())
}
- fn write_bank(&mut self, i2c: &mut I2c, bank: u8, data: &[u8]) -> Result<(), Error<I2c>> {
- self.set_bank(i2c, bank)?;
+ fn write_bank(&mut self, i2c: &mut I2c, bank: u8, data: &[u8]) {
+ self.set_bank(i2c, bank);
for (i, chunk) in data.chunks(CHUNK_SIZE).enumerate() {
- let mut prolog_and_chunk: [u8; CHUNK_SIZE + 1] = [0; CHUNK_SIZE + 1];
- prolog_and_chunk[0] = Register::MemRw as u8;
+ let mut prolog_and_chunk: [u8; CHUNK_SIZE] = [0; CHUNK_SIZE];
for (i, b) in chunk.iter().enumerate() {
- prolog_and_chunk[i + 1] = *b;
+ prolog_and_chunk[i] = *b;
}
- self.set_memory_start_address(i2c, (i * CHUNK_SIZE) as u8)?;
- self.write(i2c, &prolog_and_chunk)?;
+ self.set_memory_start_address(i2c, (i * CHUNK_SIZE) as u8);
+ self.write(i2c, Register::MemRw as u8, &prolog_and_chunk);
}
-
- Ok(())
}
- fn set_bank(&mut self, i2c: &mut I2c, bank: u8) -> Result<(), Error<I2c>> {
+ fn set_bank(&mut self, i2c: &mut I2c, bank: u8) {
self.write_register(i2c, Register::BankSel, bank)
}
- fn set_memory_start_address(&mut self, i2c: &mut I2c, addr: u8) -> Result<(), Error<I2c>> {
+ fn set_memory_start_address(&mut self, i2c: &mut I2c, addr: u8) {
self.write_register(i2c, Register::MemStartAddr, addr)
}
}
diff --git a/src/mpu.rs b/src/mpu/mod.rs
similarity index 70%
rename from src/mpu.rs
rename to src/mpu/mod.rs
index c935ecb695a25f337d4a9164130d962199a96be5..52efc8527958847b971abc18e5167e34732080ef 100644
--- a/src/mpu.rs
+++ b/src/mpu/mod.rs
@@ -1,18 +1,15 @@
use crate::mpu::config::DigitalLowPassFilter;
-use crate::mpu::error::Error;
use crate::mpu::sensor::Mpu6050;
use crate::mutex::Mutex;
use crate::once_cell::OnceCell;
-use crate::twi::TWI;
+use crate::twi::{TwiWrapper, TWI};
+use error::Error;
use nb::Error::WouldBlock;
-use nrf51_hal::Twi;
-use nrf51_pac::TWI0;
use structs::{Accel, Gyro, Quaternion};
#[allow(unused)]
mod config;
mod dmp_firmware;
-mod error;
mod firmware_loader;
#[allow(unused)]
mod registers;
@@ -21,34 +18,37 @@ mod sensor;
/// structs to deal with mpu output, like quaternions
pub mod structs;
+mod error;
+
/// MPU Sample Rate Divider under DMP mode
pub const SAMPLE_RATE_DIVIDER_MPU: u8 = 0;
/// MPU Sample Rate Divider under RAW mode
pub const SAMPLE_RATE_DIVIDER_RAW: u8 = 0;
+type I2c = TwiWrapper;
+
struct Mpu {
- mpu: Mpu6050<Twi<TWI0>>,
+ mpu: Mpu6050,
dmp_enabled: bool,
}
static MPU: Mutex<OnceCell<Mpu>> = Mutex::new(OnceCell::uninitialized());
pub(crate) fn initialize() {
- TWI.modify(|twi| {
- let mut mpu: Mpu6050<Twi<TWI0>> = Mpu6050::new(&mut **twi).unwrap();
-
- mpu.initialize_dmp(twi).unwrap();
-
- mpu.set_sample_rate_divider(twi, SAMPLE_RATE_DIVIDER_MPU)
- .unwrap();
- mpu.set_digital_lowpass_filter(twi, DigitalLowPassFilter::Filter5)
- .unwrap();
- MPU.modify(|m| {
- m.initialize(Mpu {
- mpu,
- dmp_enabled: true,
- })
- });
+ // Safety: The TWI mutex is not accessed in an interrupt
+ let twi = unsafe { TWI.no_critical_section_lock_mut() };
+
+ let mut mpu = Mpu6050::new(twi);
+
+ mpu.initialize_dmp(twi);
+
+ mpu.set_sample_rate_divider(twi, SAMPLE_RATE_DIVIDER_MPU);
+ mpu.set_digital_lowpass_filter(twi, DigitalLowPassFilter::Filter5);
+ MPU.modify(|m| {
+ m.initialize(Mpu {
+ mpu,
+ dmp_enabled: true,
+ })
});
}
@@ -68,10 +68,9 @@ pub fn disable_dmp() {
let mpu = unsafe { MPU.no_critical_section_lock_mut() };
mpu.mpu
- .set_sample_rate_divider(twi, SAMPLE_RATE_DIVIDER_RAW)
- .unwrap();
- mpu.mpu.disable_dmp(twi).unwrap();
- mpu.mpu.disable_fifo(twi).unwrap();
+ .set_sample_rate_divider(twi, SAMPLE_RATE_DIVIDER_RAW);
+ mpu.mpu.disable_dmp(twi);
+ mpu.mpu.disable_fifo(twi);
mpu.dmp_enabled = false;
}
@@ -79,18 +78,16 @@ pub fn disable_dmp() {
///
/// # Errors
/// when the global constant `SAMPLE_RATE_DIVIDER_MPU` is wrong (i.e. will not panic under normal conditions)
-pub fn enable_dmp() -> Result<(), Error<Twi<TWI0>>> {
+pub fn enable_dmp() {
// Safety: The TWI and MPU mutexes are not accessed in an interrupt
let twi = unsafe { TWI.no_critical_section_lock_mut() };
let mpu = unsafe { MPU.no_critical_section_lock_mut() };
mpu.mpu
- .set_sample_rate_divider(twi, SAMPLE_RATE_DIVIDER_MPU)?;
- mpu.mpu.enable_dmp(twi)?;
- mpu.mpu.enable_fifo(twi)?;
+ .set_sample_rate_divider(twi, SAMPLE_RATE_DIVIDER_MPU);
+ mpu.mpu.enable_dmp(twi);
+ mpu.mpu.enable_fifo(twi);
mpu.dmp_enabled = true;
-
- Ok(())
}
/// This reads the most recent angle from the DMP, if there are any new ones available.
@@ -102,7 +99,7 @@ pub fn enable_dmp() -> Result<(), Error<Twi<TWI0>>> {
///
/// # Errors
/// when a TWI(I2C) operation failed
-pub fn read_dmp_bytes() -> nb::Result<Quaternion, Error<Twi<TWI0>>> {
+pub fn read_dmp_bytes() -> nb::Result<Quaternion, ()> {
// Safety: The TWI and MPU mutexes are not accessed in an interrupt
let twi = unsafe { TWI.no_critical_section_lock_mut() };
let mpu = unsafe { MPU.no_critical_section_lock_mut() };
@@ -110,15 +107,24 @@ pub fn read_dmp_bytes() -> nb::Result<Quaternion, Error<Twi<TWI0>>> {
assert!(mpu.dmp_enabled);
// If there isn't a full packet ready, return none
- let mut len = mpu.mpu.get_fifo_count(twi)?;
+ let mut len = mpu.mpu.get_fifo_count(twi);
if len < 28 {
return Err(WouldBlock);
}
+ // If we got mis-aligned, we skip a packet
+ if len % 28 != 0 {
+ let skip = len % 28;
+ let mut buf = [0; 28];
+
+ let _ = mpu.mpu.read_fifo(twi, &mut buf[..skip]);
+ return Err(WouldBlock);
+ }
+
// Keep reading while there are more full packets
let mut buf = [0; 28];
while len >= 28 {
- let _ = mpu.mpu.read_fifo(twi, &mut buf)?;
+ let _ = mpu.mpu.read_fifo(twi, &mut buf);
len -= 28;
}
@@ -131,13 +137,13 @@ pub fn read_dmp_bytes() -> nb::Result<Quaternion, Error<Twi<TWI0>>> {
///
/// # Errors
/// when a TWI operation failed
-pub fn read_raw() -> Result<(Accel, Gyro), Error<Twi<TWI0>>> {
+pub fn read_raw() -> Result<(Accel, Gyro), Error> {
// Safety: The TWI and MPU mutexes are not accessed in an interrupt
let twi = unsafe { TWI.no_critical_section_lock_mut() };
let mpu = unsafe { MPU.no_critical_section_lock_mut() };
- let accel = mpu.mpu.accel(twi)?;
- let gyro = mpu.mpu.gyro(twi)?;
+ let accel = mpu.mpu.accel(twi);
+ let gyro = mpu.mpu.gyro(twi);
Ok((accel, gyro))
}
diff --git a/src/mpu/sensor.rs b/src/mpu/sensor.rs
index f5effef688f3d7e8c94b50f614463c5f7e8b890b..f2721f731dcaa5bbd2c0d7fe35e0f3e186fd63f2 100644
--- a/src/mpu/sensor.rs
+++ b/src/mpu/sensor.rs
@@ -1,84 +1,64 @@
+use crate::led::Green;
use crate::mpu::config::Fifo;
use crate::mpu::config::GyroFullScale;
use crate::mpu::config::{AccelFullScale, ClockSource, DigitalLowPassFilter};
-use crate::mpu::error::Error;
use crate::mpu::registers::Register;
use crate::mpu::structs::{Accel, Gyro};
use crate::time::delay_ms_assembly;
+use crate::twi::TwiWrapper;
use core::marker::PhantomData;
use core::time::Duration;
use embedded_hal::blocking::i2c::{Write, WriteRead};
const MPU6050_ADDRESS: u8 = 0x68;
-/// `InvenSense` MPU-6050 Driver
-pub struct Mpu6050<I2c>
-where
- I2c: Write + WriteRead,
- <I2c as WriteRead>::Error: core::fmt::Debug,
- <I2c as Write>::Error: core::fmt::Debug,
-{
- _p: PhantomData<I2c>,
-}
+pub type I2c = TwiWrapper;
+
+pub(crate) struct Mpu6050(PhantomData<()>);
-impl<I2c> Mpu6050<I2c>
-where
- I2c: Write + WriteRead,
- <I2c as WriteRead>::Error: core::fmt::Debug,
- <I2c as Write>::Error: core::fmt::Debug,
-{
+impl Mpu6050 {
/// Construct a new i2c driver for the MPU-6050
- pub fn new(i2c: &mut I2c) -> Result<Self, Error<I2c>> {
- let mut sensor = Self {
- _p: PhantomData::default(),
- };
+ pub fn new(i2c: &mut I2c) -> Self {
+ let mut sensor = Self(PhantomData::default());
- sensor.disable_sleep(i2c)?;
+ sensor.disable_sleep(i2c);
- Ok(sensor)
+ sensor
}
/// Load DMP firmware and perform all appropriate initialization.
- pub fn initialize_dmp(&mut self, i2c: &mut I2c) -> Result<(), Error<I2c>> {
- self.reset(i2c)?;
- self.disable_sleep(i2c)?;
- self.reset_signal_path(i2c)?;
- self.disable_dmp(i2c)?;
- self.set_clock_source(i2c, ClockSource::Xgyro)?;
- self.disable_interrupts(i2c)?;
- self.set_fifo_enabled(i2c, Fifo::all_disabled())?;
- self.set_accel_full_scale(i2c, AccelFullScale::G2)?;
- self.set_sample_rate_divider(i2c, 0)?;
- self.set_digital_lowpass_filter(i2c, DigitalLowPassFilter::Filter0)?;
- self.load_firmware(i2c)?;
- self.boot_firmware(i2c)?;
- self.set_gyro_full_scale(i2c, GyroFullScale::Deg2000)?;
- self.enable_fifo(i2c)?;
- self.reset_fifo(i2c)?;
- self.disable_dmp(i2c)?;
- self.enable_dmp(i2c)?;
- Ok(())
- }
-
- pub(crate) fn read(
- &mut self,
- i2c: &mut I2c,
- bytes: &[u8],
- response: &mut [u8],
- ) -> Result<(), Error<I2c>> {
- i2c.write_read(MPU6050_ADDRESS, bytes, response)
- .map_err(|e| Error::WriteRead(e))
- }
-
- pub(crate) fn write(&mut self, i2c: &mut I2c, bytes: &[u8]) -> Result<(), Error<I2c>> {
- i2c.write(MPU6050_ADDRESS, bytes)
- .map_err(|e| Error::Write(e))
- }
-
- pub(crate) fn read_register(&mut self, i2c: &mut I2c, reg: Register) -> Result<u8, Error<I2c>> {
+ pub fn initialize_dmp(&mut self, i2c: &mut I2c) {
+ self.reset(i2c);
+ self.disable_sleep(i2c);
+ self.reset_signal_path(i2c);
+ self.disable_dmp(i2c);
+ self.set_clock_source(i2c, ClockSource::Xgyro);
+ self.disable_interrupts(i2c);
+ self.set_fifo_enabled(i2c, Fifo::all_disabled());
+ self.set_accel_full_scale(i2c, AccelFullScale::G2);
+ self.set_sample_rate_divider(i2c, 0);
+ self.set_digital_lowpass_filter(i2c, DigitalLowPassFilter::Filter0);
+ self.load_firmware(i2c);
+ self.boot_firmware(i2c);
+ self.set_gyro_full_scale(i2c, GyroFullScale::Deg2000);
+ self.enable_fifo(i2c);
+ self.reset_fifo(i2c);
+ self.disable_dmp(i2c);
+ self.enable_dmp(i2c);
+ }
+
+ pub(crate) fn read(&mut self, i2c: &mut I2c, reg: u8, response: &mut [u8]) {
+ let _ = i2c.read(MPU6050_ADDRESS, reg, response);
+ }
+
+ pub(crate) fn write(&mut self, i2c: &mut I2c, reg_address: u8, bytes: &[u8]) {
+ i2c.write(MPU6050_ADDRESS, reg_address, bytes);
+ }
+
+ pub(crate) fn read_register(&mut self, i2c: &mut I2c, reg: Register) -> u8 {
let mut buf = [0; 1];
- self.read(i2c, &[reg as u8], &mut buf)?;
- Ok(buf[0])
+ self.read(i2c, reg as u8, &mut buf);
+ buf[0]
}
pub(crate) fn read_registers<'a>(
@@ -86,180 +66,142 @@ where
i2c: &mut I2c,
reg: Register,
buf: &'a mut [u8],
- ) -> Result<&'a [u8], Error<I2c>> {
- self.read(i2c, &[reg as u8], buf)?;
- Ok(buf)
+ ) -> &'a [u8] {
+ self.read(i2c, reg as u8, buf);
+ buf
}
- pub(crate) fn write_register(
- &mut self,
- i2c: &mut I2c,
- reg: Register,
- value: u8,
- ) -> Result<(), Error<I2c>> {
- self.write(i2c, &[reg as u8, value])
+ pub(crate) fn write_register(&mut self, i2c: &mut I2c, reg: Register, value: u8) {
+ self.write(i2c, reg as u8, &[value]);
}
// ------------------------------------------------------------------------
// ------------------------------------------------------------------------
/// Perform power reset of the MPU
- pub fn reset(&mut self, i2c: &mut I2c) -> Result<(), Error<I2c>> {
- let mut value = self.read_register(i2c, Register::PwrMgmt1)?;
+ pub fn reset(&mut self, i2c: &mut I2c) {
+ let mut value = self.read_register(i2c, Register::PwrMgmt1);
value |= 1 << 7;
- self.write_register(i2c, Register::PwrMgmt1, value)?;
+ self.write_register(i2c, Register::PwrMgmt1, value);
delay_ms_assembly(200);
- Ok(())
}
/// Perform reset of the signal path
- pub fn reset_signal_path(&mut self, i2c: &mut I2c) -> Result<(), Error<I2c>> {
- let mut value = self.read_register(i2c, Register::UserCtrl)?;
+ pub fn reset_signal_path(&mut self, i2c: &mut I2c) {
+ let mut value = self.read_register(i2c, Register::UserCtrl);
value |= 1 << 0;
- self.write_register(i2c, Register::UserCtrl, value)?;
+ self.write_register(i2c, Register::UserCtrl, value);
delay_ms_assembly(200);
- Ok(())
}
/// Pick the clock-source
- pub fn set_clock_source(
- &mut self,
- i2c: &mut I2c,
- clock_source: ClockSource,
- ) -> Result<(), Error<I2c>> {
- let mut value = self.read_register(i2c, Register::PwrMgmt1)?;
+ pub fn set_clock_source(&mut self, i2c: &mut I2c, clock_source: ClockSource) {
+ let mut value = self.read_register(i2c, Register::PwrMgmt1);
value |= clock_source as u8;
- self.write_register(i2c, Register::PwrMgmt1, value)?;
- Ok(())
+ self.write_register(i2c, Register::PwrMgmt1, value);
}
- pub fn disable_interrupts(&mut self, i2c: &mut I2c) -> Result<(), Error<I2c>> {
+ pub fn disable_interrupts(&mut self, i2c: &mut I2c) {
self.write_register(i2c, Register::IntEnable, 0x00)
}
- pub fn set_accel_full_scale(
- &mut self,
- i2c: &mut I2c,
- scale: AccelFullScale,
- ) -> Result<(), Error<I2c>> {
- let mut value = self.read_register(i2c, Register::AccelConfig)?;
+ pub fn set_accel_full_scale(&mut self, i2c: &mut I2c, scale: AccelFullScale) {
+ let mut value = self.read_register(i2c, Register::AccelConfig);
value |= (scale as u8) << 3;
self.write_register(i2c, Register::AccelConfig, value)
}
- pub fn set_gyro_full_scale(
- &mut self,
- i2c: &mut I2c,
- scale: GyroFullScale,
- ) -> Result<(), Error<I2c>> {
- let mut value = self.read_register(i2c, Register::GyroConfig)?;
+ pub fn set_gyro_full_scale(&mut self, i2c: &mut I2c, scale: GyroFullScale) {
+ let mut value = self.read_register(i2c, Register::GyroConfig);
value |= (scale as u8) << 3;
self.write_register(i2c, Register::GyroConfig, value)
}
- pub fn set_sample_rate_divider(&mut self, i2c: &mut I2c, div: u8) -> Result<(), Error<I2c>> {
+ pub fn set_sample_rate_divider(&mut self, i2c: &mut I2c, div: u8) {
self.write_register(i2c, Register::SmpRtDiv, div)
}
- pub fn set_digital_lowpass_filter(
- &mut self,
- i2c: &mut I2c,
- filter: DigitalLowPassFilter,
- ) -> Result<(), Error<I2c>> {
- let mut value = self.read_register(i2c, Register::Config)?;
+ pub fn set_digital_lowpass_filter(&mut self, i2c: &mut I2c, filter: DigitalLowPassFilter) {
+ let mut value = self.read_register(i2c, Register::Config);
value |= filter as u8;
self.write_register(i2c, Register::Config, value)
}
- pub fn reset_fifo(&mut self, i2c: &mut I2c) -> Result<(), Error<I2c>> {
- let mut value = self.read_register(i2c, Register::UserCtrl)?;
+ pub fn reset_fifo(&mut self, i2c: &mut I2c) {
+ let mut value = self.read_register(i2c, Register::UserCtrl);
value |= 1 << 2;
self.write_register(i2c, Register::UserCtrl, value)
}
- pub fn enable_fifo(&mut self, i2c: &mut I2c) -> Result<(), Error<I2c>> {
- let mut value = self.read_register(i2c, Register::UserCtrl)?;
+ pub fn enable_fifo(&mut self, i2c: &mut I2c) {
+ let mut value = self.read_register(i2c, Register::UserCtrl);
value |= 1 << 6;
self.write_register(i2c, Register::UserCtrl, value)
}
- pub fn disable_fifo(&mut self, i2c: &mut I2c) -> Result<(), Error<I2c>> {
- let mut value = self.read_register(i2c, Register::UserCtrl)?;
+ pub fn disable_fifo(&mut self, i2c: &mut I2c) {
+ let mut value = self.read_register(i2c, Register::UserCtrl);
value &= !(1 << 6);
self.write_register(i2c, Register::UserCtrl, value)
}
/// Set the DMP bit.
/// To perform full DMP initialization, see `initialize_dmp()`
- pub fn enable_dmp(&mut self, i2c: &mut I2c) -> Result<(), Error<I2c>> {
- let mut value = self.read_register(i2c, Register::UserCtrl)?;
+ pub fn enable_dmp(&mut self, i2c: &mut I2c) {
+ let mut value = self.read_register(i2c, Register::UserCtrl);
value |= 1 << 7;
self.write_register(i2c, Register::UserCtrl, value)
}
// Unset the DMP bit.
- pub fn disable_dmp(&mut self, i2c: &mut I2c) -> Result<(), Error<I2c>> {
- let mut value = self.read_register(i2c, Register::UserCtrl)?;
+ pub fn disable_dmp(&mut self, i2c: &mut I2c) {
+ let mut value = self.read_register(i2c, Register::UserCtrl);
value &= !(1 << 7);
self.write_register(i2c, Register::UserCtrl, value)
}
/// Reset the DMP processor
- pub fn reset_dmp(&mut self, i2c: &mut I2c) -> Result<(), Error<I2c>> {
- let mut value = self.read_register(i2c, Register::UserCtrl)?;
+ pub fn reset_dmp(&mut self, i2c: &mut I2c) {
+ let mut value = self.read_register(i2c, Register::UserCtrl);
value |= 1 << 3;
self.write_register(i2c, Register::UserCtrl, value)
}
/// Read the FIFO
- pub fn read_fifo<'a>(
- &mut self,
- i2c: &mut I2c,
- buf: &'a mut [u8],
- ) -> Result<&'a [u8], Error<I2c>> {
- let mut len = self.get_fifo_count(i2c)?;
-
- if buf.len() < len {
- len = buf.len();
- }
-
- if len == 0 {
- Ok(&buf[0..0])
- } else {
- self.read_registers(i2c, Register::FifoRw, &mut buf[0..len])
- }
+ pub fn read_fifo<'a>(&mut self, i2c: &mut I2c, buf: &'a mut [u8]) -> &'a [u8] {
+ self.read_registers(i2c, Register::FifoRw, &mut buf[..])
}
- pub fn get_fifo_enabled(&mut self, i2c: &mut I2c) -> Result<Fifo, Error<I2c>> {
- let value = self.read_register(i2c, Register::FifoEn)?;
- Ok(Fifo::from_byte(value))
+ pub fn get_fifo_enabled(&mut self, i2c: &mut I2c) -> Fifo {
+ let value = self.read_register(i2c, Register::FifoEn);
+ Fifo::from_byte(value)
}
- pub fn set_fifo_enabled(&mut self, i2c: &mut I2c, fifo: Fifo) -> Result<(), Error<I2c>> {
+ pub fn set_fifo_enabled(&mut self, i2c: &mut I2c, fifo: Fifo) {
self.write_register(i2c, Register::FifoEn, fifo.to_byte())
}
- pub fn get_fifo_count(&mut self, i2c: &mut I2c) -> Result<usize, Error<I2c>> {
+ pub fn get_fifo_count(&mut self, i2c: &mut I2c) -> usize {
let mut buf = [0; 2];
- let _value = self.read_registers(i2c, Register::FifoCount_H, &mut buf)?;
- Ok(u16::from_be_bytes(buf) as usize)
+ let _value = self.read_registers(i2c, Register::FifoCount_H, &mut buf);
+ u16::from_be_bytes(buf) as usize
}
- pub fn disable_sleep(&mut self, i2c: &mut I2c) -> Result<(), Error<I2c>> {
- let mut value = self.read_register(i2c, Register::PwrMgmt1)?;
+ pub fn disable_sleep(&mut self, i2c: &mut I2c) {
+ let mut value = self.read_register(i2c, Register::PwrMgmt1);
value &= !(1 << 6);
self.write_register(i2c, Register::PwrMgmt1, value)
}
- pub fn accel(&mut self, i2c: &mut I2c) -> Result<Accel, Error<I2c>> {
+ pub fn accel(&mut self, i2c: &mut I2c) -> Accel {
let mut data = [0; 6];
- let _ = self.read_registers(i2c, Register::AccelX_H, &mut data)?;
- Ok(Accel::from_bytes(data))
+ let _ = self.read_registers(i2c, Register::AccelX_H, &mut data);
+ Accel::from_bytes(data)
}
- pub fn gyro(&mut self, i2c: &mut I2c) -> Result<Gyro, Error<I2c>> {
+ pub fn gyro(&mut self, i2c: &mut I2c) -> Gyro {
let mut data = [0; 6];
- let _ = self.read_registers(i2c, Register::GyroX_H, &mut data)?;
- Ok(Gyro::from_bytes(data))
+ let _ = self.read_registers(i2c, Register::GyroX_H, &mut data);
+ Gyro::from_bytes(data)
}
}
diff --git a/src/twi.rs b/src/twi.rs
index f8ec8c3023f4eb471c406563740ad536af7681de..77262338547d2a5bd89cd9e883c63a10349101e7 100644
--- a/src/twi.rs
+++ b/src/twi.rs
@@ -1,26 +1,229 @@
use crate::mutex::Mutex;
use crate::once_cell::OnceCell;
+use core::sync::atomic::{AtomicBool, Ordering};
+use cortex_m::peripheral::NVIC;
use nrf51_hal::gpio::p0::{P0_02, P0_04};
use nrf51_hal::gpio::{Disconnected, Pin};
-use nrf51_hal::twi::Pins;
-use nrf51_hal::Twi;
+use nrf51_pac::interrupt;
use nrf51_pac::twi0::frequency::FREQUENCY_A;
-use nrf51_pac::TWI0;
+use nrf51_pac::{Interrupt, GPIO, TWI0};
-pub(crate) static TWI: Mutex<OnceCell<Twi<TWI0>>> = Mutex::new(OnceCell::uninitialized());
+const FREQ: FREQUENCY_A = FREQUENCY_A::K400;
-pub(crate) fn initialize(twi: TWI0, scl_pin: P0_04<Disconnected>, sda_pin: P0_02<Disconnected>) {
- let scl_pin = scl_pin.into_floating_input();
- let sda_pin = sda_pin.into_floating_input();
+pub(crate) static TWI: Mutex<OnceCell<TwiWrapper>> = Mutex::new(OnceCell::uninitialized());
+pub struct TwiWrapper {
+ twi: TWI0,
+ sent: AtomicBool,
+ recv: AtomicBool,
+}
+
+pub enum TwiStatus {
+ BufEmpty,
+ Success,
+}
+
+impl TwiWrapper {
+ fn set_sent_flag(&self, value: bool) {
+ self.sent.store(value, Ordering::SeqCst)
+ }
+
+ fn set_recv_flag(&self, value: bool) {
+ self.recv.store(value, Ordering::SeqCst)
+ }
+
+ fn wait(&self, flag: &AtomicBool) {
+ while !flag.load(Ordering::SeqCst) {
+ core::hint::spin_loop();
+ }
+ }
+
+ fn wait_sent(&self) {
+ self.wait(&self.sent)
+ }
+
+ fn wait_recv(&self) {
+ self.wait(&self.recv)
+ }
+
+ pub fn read(&self, addr: u8, reg_addr: u8, data: &mut [u8]) -> TwiStatus {
+ if data.is_empty() {
+ return TwiStatus::BufEmpty;
+ }
+
+ self.set_sent_flag(false);
+ self.set_recv_flag(false);
+
+ self.twi
+ .address
+ .write(|w| unsafe { w.address().bits(addr) });
+ self.twi.txd.write(|w| unsafe { w.txd().bits(reg_addr) });
+ self.twi.shorts.reset();
+ self.twi.tasks_starttx.write(|w| unsafe { w.bits(1) });
+
+ self.wait_sent();
+ self.set_sent_flag(false);
+
+ if data.len() == 1 {
+ self.twi.shorts.write(|w| w.bb_stop().set_bit())
+ } else {
+ self.twi.shorts.write(|w| w.bb_suspend().set_bit())
+ }
+
+ self.twi.tasks_startrx.write(|w| unsafe { w.bits(1) });
+
+ let mut bytes_left = data.len();
+ let mut write_ptr = 0;
+
+ loop {
+ self.wait_recv();
+ self.set_recv_flag(false);
+
+ data[write_ptr] = self.twi.rxd.read().rxd().bits();
+ write_ptr += 1;
+
+ bytes_left -= 1;
+ if bytes_left == 1 {
+ self.twi.shorts.write(|w| w.bb_stop().set_bit())
+ }
+ self.twi.tasks_resume.write(|w| unsafe { w.bits(1) });
+
+ if bytes_left == 0 {
+ break;
+ }
+ }
+
+ TwiStatus::Success
+ }
+
+ pub fn write(&self, addr: u8, reg_addr: u8, data: &[u8]) {
+ if data.is_empty() {
+ self.twi
+ .address
+ .write(|w| unsafe { w.address().bits(addr) });
+ self.twi.shorts.write(|w| w.bb_stop().set_bit());
+ self.twi.txd.write(|w| unsafe { w.txd().bits(reg_addr) });
+ self.twi.tasks_starttx.write(|w| unsafe { w.bits(1) });
+ self.wait_sent();
+
+ return;
+ }
+
+ self.set_sent_flag(false);
+
+ self.twi
+ .address
+ .write(|w| unsafe { w.address().bits(addr) });
+ self.twi.shorts.reset();
+ self.twi.txd.write(|w| unsafe { w.txd().bits(reg_addr) });
+ self.twi.tasks_starttx.write(|w| unsafe { w.bits(1) });
+
+ self.wait_sent();
+ self.set_sent_flag(false);
+
+ for &i in data {
+ self.twi.txd.write(|w| unsafe { w.txd().bits(i) });
+
+ self.wait_sent();
+ self.set_sent_flag(false);
+ }
+
+ self.twi.tasks_stop.write(|w| unsafe { w.bits(1) });
+ }
+}
+
+pub(crate) fn initialize(
+ twi: TWI0,
+ scl_pin: P0_04<Disconnected>,
+ sda_pin: P0_02<Disconnected>,
+ nvic: &mut NVIC,
+) {
+ let scl_pin = Pin::from(scl_pin.into_pullup_input());
+ let sda_pin = Pin::from(sda_pin.into_pullup_input());
+
+ // The TWIM peripheral requires the pins to be in a mode that is not
+ // exposed through the GPIO API, and might it might not make sense to
+ // expose it there.
+ //
+ // Until we've figured out what to do about this, let's just configure
+ // the pins through the raw peripheral API. All of the following is
+ // safe, as we own the pins now and have exclusive access to their
+ // registers.
+ for &pin in &[scl_pin.pin(), sda_pin.pin()] {
+ unsafe { &*GPIO::ptr() }.pin_cnf[pin as usize].write(|w| {
+ w.dir()
+ .input()
+ .input()
+ .connect()
+ .pull()
+ .pullup()
+ .drive()
+ .s0d1()
+ .sense()
+ .disabled()
+ });
+ }
+
+ // Set pins.
+ twi.pselscl
+ .write(|w| unsafe { w.bits(scl_pin.pin().into()) });
+ twi.pselsda
+ .write(|w| unsafe { w.bits(sda_pin.pin().into()) });
+
+ // Clear interrupts
+ twi.events_rxdready.reset();
+ twi.events_txdsent.reset();
+
+ // Set frequency.
+ twi.frequency.write(|w| w.frequency().variant(FREQ));
+
+ // Set which interrupts we want to receive
+ twi.intenset
+ .write(|w| w.txdsent().set_bit().rxdready().set_bit().error().set_bit());
+
+ twi.shorts.reset();
+ twi.enable.write(|w| w.enable().enabled());
+
+ // Initialize oncecell
+ // twi.events_rxdready.reset();
+ // twi.events_txdsent.reset();
TWI.modify(|t| {
- t.initialize(Twi::new(
+ t.initialize(TwiWrapper {
twi,
- Pins {
- scl: Pin::from(scl_pin),
- sda: Pin::from(sda_pin),
- },
- FREQUENCY_A::K400,
- ))
+ recv: false.into(),
+ sent: false.into(),
+ })
});
+
+ // Setup NVIC
+ NVIC::unpend(Interrupt::SPI0_TWI0);
+ // Safety: We are not using priority-based critical sections.
+ unsafe {
+ nvic.set_priority(Interrupt::SPI0_TWI0, 3); // Same as C template
+ NVIC::unmask(Interrupt::SPI0_TWI0);
+ }
+}
+
+#[interrupt]
+unsafe fn SPI0_TWI0() {
+ // Safety: interrupts are already turned off here, since we are inside an interrupt
+ // We might be accessing the hardware while the interrupted code also wants to, this is fine since we're only touching the EVENT registers which are not touched by the other code
+ let twi = unsafe { TWI.no_critical_section_lock_mut() };
+
+ if twi.twi.events_rxdready.read().bits() != 0 {
+ twi.twi.events_rxdready.reset();
+ twi.recv.store(true, Ordering::SeqCst);
+ }
+ if twi.twi.events_txdsent.read().bits() != 0 {
+ twi.twi.events_txdsent.reset();
+ twi.sent.store(true, Ordering::SeqCst);
+ }
+
+ // Errors are silently ignored
+ if twi.twi.events_error.read().bits() != 0 {
+ twi.twi
+ .errorsrc
+ .write(|w| w.anack().clear_bit().overrun().clear_bit()); // Clear error source
+ twi.twi.events_error.reset();
+ }
}