Skip to content

Commit 0428b7f

Browse files
Vortex148Vortex148
committed
feat: initial bare mag driver
1 parent fc096c8 commit 0428b7f

File tree

7 files changed

+251
-0
lines changed

7 files changed

+251
-0
lines changed

‎common/drivers/src/lib.rs‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
#![no_std]
22
#![feature(impl_trait_in_assoc_type)]
33
pub mod ejection_channel;
4+
pub mod mmc5983ma;
45
pub mod ms561101;
56
pub mod rfd900x;

‎common/drivers/src/mmc5983ma/driver.rs‎

Lines changed: 111 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,111 @@
1+
use embassy_stm32::pac::common::W;
2+
use embassy_time::Timer;
3+
use uor_peripherals::spi::peripheral::UORMonoCsSPI;
4+
use uor_utils::linear_algebra::vectors::{VectorR3, VectorR3F};
5+
6+
use crate::mmc5983ma::utils::{MMC5983MABandwidth, MMC5983MARegisters};
7+
8+
pub struct MMC5983MA<'a> {
9+
pub spi_service: UORMonoCsSPI<'a>,
10+
}
11+
12+
impl<'a> MMC5983MA<'a> {
13+
pub fn new(spi_service: UORMonoCsSPI<'a>) -> Self {
14+
MMC5983MA { spi_service: spi_service }
15+
}
16+
17+
// Read from chip when rw_n high
18+
pub async fn transfer(
19+
&mut self,
20+
register: MMC5983MARegisters,
21+
write_data: u8,
22+
rw_n: bool,
23+
) -> u8 {
24+
let mut read_buf: [u16; 1] = [0];
25+
let mut write: u16 = 0;
26+
// Set address bits
27+
write = write | ((register as u16) << 8);
28+
write = write | (write_data as u16);
29+
30+
if rw_n {
31+
write = write | 0b1000_0000;
32+
}
33+
34+
let _ = self.spi_service.transfer::<u16>(&mut read_buf, &mut [write]).await;
35+
read_buf[0] as u8
36+
}
37+
38+
pub async fn start_measurement(
39+
&mut self,
40+
mag: bool,
41+
temp: bool,
42+
) {
43+
let mut write_data = 0;
44+
if mag {
45+
write_data = write_data | 0b0000_0001;
46+
}
47+
if temp {
48+
write_data = write_data | 0b0000_0010;
49+
}
50+
51+
self.transfer(MMC5983MARegisters::InternalControl0, write_data, false).await;
52+
}
53+
54+
pub async fn get_raw_mag_data(&mut self) -> VectorR3 {
55+
let mut x_val: u32 = 0;
56+
let mut y_val: u32 = 0;
57+
let mut z_val: u32 = 0;
58+
59+
x_val = x_val | ((self.transfer(MMC5983MARegisters::XOut0, 0, true).await as u32) << 10);
60+
x_val = x_val | ((self.transfer(MMC5983MARegisters::XOut1, 0, true).await as u32) << 2);
61+
62+
y_val = y_val | ((self.transfer(MMC5983MARegisters::YOut0, 0, true).await as u32) << 10);
63+
y_val = y_val | ((self.transfer(MMC5983MARegisters::YOut1, 0, true).await as u32) << 2);
64+
65+
z_val = z_val | ((self.transfer(MMC5983MARegisters::ZOut0, 0, true).await as u32) << 10);
66+
z_val = z_val | ((self.transfer(MMC5983MARegisters::ZOut1, 0, true).await as u32) << 2);
67+
68+
// The lower two bits of the X,Y, and Z vector magnitudes
69+
let xyz_lower = self.transfer(MMC5983MARegisters::XYZOut2, 0, true).await as u32;
70+
71+
// TODO: GET RID OF THE EVIL MAGIC NUMBERS GRRRR
72+
x_val = x_val | ((xyz_lower & 0b1100_0000) >> 6);
73+
y_val = y_val | ((xyz_lower & 0b0011_0000) >> 6);
74+
z_val = z_val | ((xyz_lower & 0b0000_1100) >> 6);
75+
76+
VectorR3::new(x_val as i64, y_val as i64, z_val as i64)
77+
}
78+
79+
pub async fn get_18bit_mag(&mut self) -> VectorR3F {
80+
let sensor_vector = VectorR3F::from(self.get_raw_mag_data().await);
81+
let mut output: VectorR3F = VectorR3F::new(-5.0, -5.0, -5.0)
82+
+ (sensor_vector * VectorR3F::new((10 / 2_i32.pow(18) as f64), (10 / 2_i32.pow(18) as f64), (10 / 2_i32.pow(18) as f64)));
83+
output
84+
}
85+
86+
pub async fn generate_mag_vector(
87+
&mut self,
88+
bandwidth: MMC5983MABandwidth,
89+
) -> VectorR3F {
90+
self.transfer(MMC5983MARegisters::InternalControl1, bandwidth.clone() as u8, false);
91+
self.start_measurement(true, false).await;
92+
// TODO: GET RID OF MORE EVIL MAGIC NUMBERS GRRRR
93+
94+
match bandwidth {
95+
MMC5983MABandwidth::Hz100 => {
96+
Timer::after_millis(9).await;
97+
}
98+
MMC5983MABandwidth::Hz200 => {
99+
Timer::after_millis(5).await;
100+
}
101+
MMC5983MABandwidth::Hz400 => {
102+
Timer::after_millis(3).await;
103+
}
104+
MMC5983MABandwidth::Hz800 => {
105+
Timer::after_millis(1).await;
106+
}
107+
}
108+
// This all assumes 18 bit mode
109+
self.get_18bit_mag().await
110+
}
111+
}

‎common/drivers/src/mmc5983ma/mod.rs‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,2 @@
1+
pub mod driver;
2+
pub mod utils;

‎common/drivers/src/mmc5983ma/utils.rs‎

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
pub enum MMC5983MARegisters {
2+
XOut0 = 0x00,
3+
XOut1 = 0x01,
4+
YOut0 = 0x02,
5+
YOut1 = 0x03,
6+
ZOut0 = 0x04,
7+
ZOut1 = 0x05,
8+
XYZOut2 = 0x06,
9+
TOut = 0x07,
10+
Status = 0x08,
11+
InternalControl0 = 0x09,
12+
InternalControl1 = 0x0A,
13+
InternalControl2 = 0x0B,
14+
InternalControl3 = 0x0C,
15+
ProductID1 = 0x2F,
16+
}
17+
18+
// Note that the increasing the bandwidth decreases time the internal filter is running for, resulting in a lower resolution measurement
19+
#[derive(Clone)]
20+
pub enum MMC5983MABandwidth {
21+
Hz100 = 0b00,
22+
Hz200 = 0b01,
23+
Hz400 = 0b10,
24+
Hz800 = 0b11,
25+
}

‎common/uor-utils/src/lib.rs‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
#![no_std]
22
pub mod csv;
3+
pub mod linear_algebra;
34
#[cfg(feature = "messages")]
45
pub mod messages;
56
pub mod signal_processing;

‎common/uor-utils/src/linear_algebra/mod.rs‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
pub mod vectors;

‎common/uor-utils/src/linear_algebra/vectors.rs‎

Lines changed: 110 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,110 @@
1+
pub struct VectorR3 {
2+
internal: [i64; 3],
3+
}
4+
5+
impl VectorR3 {
6+
pub fn new(
7+
i: i64,
8+
j: i64,
9+
k: i64,
10+
) -> Self {
11+
VectorR3 { internal: [i, j, k] }
12+
}
13+
14+
pub fn zero_vector() -> VectorR3 {
15+
VectorR3::new(0, 0, 0)
16+
}
17+
}
18+
19+
// Do the basic implementations for VectorR3 to be a vector space (Addition)
20+
impl core::ops::Add<VectorR3> for VectorR3 {
21+
type Output = VectorR3;
22+
23+
fn add(
24+
self,
25+
rhs: VectorR3,
26+
) -> Self::Output {
27+
VectorR3::new(
28+
self.internal[0] + rhs.internal[0],
29+
self.internal[1] + rhs.internal[1],
30+
self.internal[2] + rhs.internal[2],
31+
)
32+
}
33+
}
34+
// Do the basic implementations for VectorR3 to be a vector space (Scalar Multiplication)
35+
impl core::ops::Mul<i64> for VectorR3 {
36+
type Output = VectorR3;
37+
38+
fn mul(
39+
self,
40+
rhs: i64,
41+
) -> Self::Output {
42+
VectorR3::new(self.internal[0] * rhs, self.internal[1] * rhs, self.internal[2] * rhs)
43+
}
44+
}
45+
46+
pub struct VectorR3F {
47+
internal: [f64; 3],
48+
}
49+
50+
impl VectorR3F {
51+
pub fn new(
52+
i: f64,
53+
j: f64,
54+
k: f64,
55+
) -> Self {
56+
VectorR3F { internal: [i, j, k] }
57+
}
58+
59+
pub fn zero_vector() -> VectorR3F {
60+
VectorR3F::new(0.0, 0.0, 0.0)
61+
}
62+
}
63+
// Do the basic implementations for VectorR3 to be a vector space (Addition)
64+
impl core::ops::Add<VectorR3F> for VectorR3F {
65+
type Output = VectorR3F;
66+
67+
fn add(
68+
self,
69+
rhs: VectorR3F,
70+
) -> Self::Output {
71+
VectorR3F::new(
72+
self.internal[0] + rhs.internal[0],
73+
self.internal[1] + rhs.internal[1],
74+
self.internal[2] + rhs.internal[2],
75+
)
76+
}
77+
}
78+
// Do the basic implementations for VectorR3 to be a vector space (Scalar Multiplication)
79+
impl core::ops::Mul<f64> for VectorR3F {
80+
type Output = VectorR3F;
81+
82+
fn mul(
83+
self,
84+
rhs: f64,
85+
) -> Self::Output {
86+
VectorR3F::new(self.internal[0] * rhs, self.internal[1] * rhs, self.internal[2] * rhs)
87+
}
88+
}
89+
impl core::ops::Mul<VectorR3F> for VectorR3F {
90+
type Output = VectorR3F;
91+
92+
fn mul(
93+
self,
94+
rhs: VectorR3F,
95+
) -> Self::Output {
96+
VectorR3F::new(
97+
self.internal[0] * rhs.internal[0],
98+
self.internal[1] * rhs.internal[1],
99+
self.internal[2] * rhs.internal[2],
100+
)
101+
}
102+
}
103+
104+
impl core::convert::From<VectorR3> for VectorR3F {
105+
fn from(value: VectorR3) -> Self {
106+
let (i, j, k) = value.internal.into();
107+
108+
VectorR3F::new(i as f64, j as f64, k as f64)
109+
}
110+
}

0 commit comments

Comments
 (0)