310 lines
9.1 KiB
Rust
310 lines
9.1 KiB
Rust
#![no_main]
|
|
#![no_std]
|
|
#![feature(type_alias_impl_trait)]
|
|
use defmt_brtt as _; // global logger
|
|
|
|
use panic_probe as _;
|
|
use stm32f4xx_hal as _;
|
|
|
|
// same panicking *behavior* as `panic-probe` but doesn't print a panic message
|
|
// this prevents the panic message being printed *twice* when `defmt::panic` is invoked
|
|
#[defmt::panic_handler]
|
|
fn panic() -> ! {
|
|
cortex_m::asm::udf()
|
|
}
|
|
|
|
|
|
#[rtic::app(
|
|
device = stm32f4xx_hal::pac,
|
|
dispatchers = [SPI3]
|
|
)]
|
|
mod app {
|
|
|
|
|
|
|
|
use as5048a::AS5048A;
|
|
|
|
use stm32f4xx_hal::{
|
|
gpio::{gpioa, gpiob, gpioc, Output, PushPull},
|
|
i2c,
|
|
pac::{I2C1, SPI1},
|
|
prelude::*,
|
|
spi,
|
|
};
|
|
|
|
use num_traits::{Float, FloatConst};
|
|
|
|
use xca9548a::{SlaveAddr, Xca9548a};
|
|
|
|
use qmc5883l::{self, QMC5883L};
|
|
|
|
use rtic_monotonics::systick::prelude::*;
|
|
|
|
systick_monotonic!(Mono, 1000);
|
|
|
|
// Shared resources go here
|
|
#[shared]
|
|
struct Shared {
|
|
az_angle: i32,
|
|
}
|
|
|
|
// Local resources go here
|
|
#[local]
|
|
struct Local {
|
|
i2cmux: Xca9548a<i2c::I2c<I2C1>>,
|
|
board_led: gpioc::PC13<Output<PushPull>>,
|
|
|
|
encoder_az: AS5048A<spi::Spi<SPI1>, gpiob::PB12<Output<PushPull>>>,
|
|
encoder_el: AS5048A<spi::Spi<SPI1>, gpiob::PB13<Output<PushPull>>>,
|
|
spi_cs2: gpiob::PB14<Output<PushPull>>,
|
|
spi_cs3: gpiob::PB15<Output<PushPull>>,
|
|
spi1: spi::Spi<SPI1>,
|
|
|
|
az_enable: gpioa::PA12<Output<PushPull>>,
|
|
az_dir: gpioa::PA15<Output<PushPull>>,
|
|
az_step: gpiob::PB3<Output<PushPull>>,
|
|
|
|
el_enable: gpiob::PB4<Output<PushPull>>,
|
|
el_dir: gpioa::PA8<Output<PushPull>>,
|
|
el_step: gpioa::PA9<Output<PushPull>>,
|
|
}
|
|
|
|
#[init]
|
|
fn init(cx: init::Context) -> (Shared, Local) {
|
|
defmt::info!("init");
|
|
|
|
let rcc = cx.device.RCC.constrain();
|
|
|
|
// Freeze the configuration of all the clocks in the system and store the frozen frequencies in
|
|
// `clocks`
|
|
let clocks = rcc
|
|
.cfgr
|
|
.use_hse(25.MHz())
|
|
.require_pll48clk()
|
|
.sysclk(84.MHz())
|
|
.hclk(84.MHz())
|
|
.pclk1(42.MHz())
|
|
.pclk2(84.MHz())
|
|
.freeze();
|
|
|
|
Mono::start(cx.core.SYST, clocks.sysclk().to_Hz());
|
|
|
|
defmt::info!("Clock Setup done");
|
|
|
|
defmt::info!("Clock Setup done");
|
|
|
|
// Acquire the GPIO peripherials
|
|
let gpioa = cx.device.GPIOA.split();
|
|
let gpiob = cx.device.GPIOB.split();
|
|
let gpioc = cx.device.GPIOC.split();
|
|
|
|
let board_led = gpioc.pc13.into_push_pull_output();
|
|
|
|
// Todo: Check if internal pullups work here
|
|
let scl = gpiob.pb6.into_alternate_open_drain();
|
|
let sda = gpiob.pb7.into_alternate_open_drain();
|
|
let i2c = i2c::I2c::new(
|
|
cx.device.I2C1,
|
|
(scl, sda),
|
|
i2c::Mode::Standard {
|
|
frequency: 400.kHz(),
|
|
},
|
|
&clocks,
|
|
);
|
|
|
|
defmt::info!("I2C Setup done");
|
|
|
|
let mut i2cmux = Xca9548a::new(i2c, SlaveAddr::default());
|
|
i2cmux.select_channels(0b0000_0001).unwrap();
|
|
|
|
defmt::info!("I2C MUX Setup done");
|
|
|
|
let spi_cs0 = gpiob.pb12.into_push_pull_output();
|
|
let encoder_az = AS5048A::new(spi_cs0);
|
|
|
|
let spi_cs1 = gpiob.pb13.into_push_pull_output();
|
|
let encoder_el = AS5048A::new(spi_cs1);
|
|
|
|
let spi_cs2 = gpiob.pb14.into_push_pull_output();
|
|
let spi_cs3 = gpiob.pb15.into_push_pull_output();
|
|
|
|
let sck = gpioa.pa5.into_push_pull_output();
|
|
let poci = gpioa.pa6;
|
|
let pico = gpioa.pa7.into_push_pull_output();
|
|
let spi1 = spi::Spi::new(
|
|
cx.device.SPI1,
|
|
(sck, poci, pico),
|
|
spi::Mode {
|
|
polarity: spi::Polarity::IdleLow,
|
|
phase: spi::Phase::CaptureOnFirstTransition,
|
|
},
|
|
8.MHz(),
|
|
&clocks,
|
|
);
|
|
|
|
defmt::info!("SPI Setup done");
|
|
|
|
let az_enable = gpioa.pa12.into_push_pull_output();
|
|
let az_dir = gpioa.pa15.into_push_pull_output();
|
|
let az_step = gpiob.pb3.into_push_pull_output();
|
|
|
|
let el_enable = gpiob.pb4.into_push_pull_output();
|
|
let el_dir = gpioa.pa8.into_push_pull_output();
|
|
let el_step = gpioa.pa9.into_push_pull_output();
|
|
|
|
defmt::info!("Motor Setup done");
|
|
|
|
poll_i2c::spawn().ok();
|
|
poll_spi::spawn().ok();
|
|
|
|
(
|
|
Shared { az_angle: 0 },
|
|
Local {
|
|
i2cmux,
|
|
board_led,
|
|
encoder_az,
|
|
encoder_el,
|
|
spi_cs2,
|
|
spi_cs3,
|
|
spi1,
|
|
|
|
az_enable,
|
|
az_dir,
|
|
az_step,
|
|
|
|
el_enable,
|
|
el_dir,
|
|
el_step,
|
|
},
|
|
)
|
|
}
|
|
|
|
#[task(local = [i2cmux, board_led])]
|
|
async fn poll_i2c(cx: poll_i2c::Context) {
|
|
let i2cmux = cx.local.i2cmux;
|
|
let board_led = cx.local.board_led;
|
|
|
|
let parts = i2cmux.split();
|
|
|
|
let mut compass1 = QMC5883L::new(parts.i2c0).unwrap();
|
|
compass1.reset().unwrap();
|
|
compass1.continuous().unwrap();
|
|
|
|
let mut compass2 = QMC5883L::new(parts.i2c1).unwrap();
|
|
compass2.reset().unwrap();
|
|
compass2.continuous().unwrap();
|
|
|
|
let declination_rads: f32 = 65.0 / 180.0 * f32::PI();
|
|
|
|
loop {
|
|
board_led.toggle();
|
|
|
|
loop {
|
|
defmt::info!("Compass 1");
|
|
match compass1.mag() {
|
|
Ok((x, y, z)) => {
|
|
defmt::info!("x1: {} y1: {} z1: {}", x, y, z);
|
|
|
|
let mut heading = (y as f32).atan2(x as f32); //+ declination_rads;
|
|
if heading < 0.0 {
|
|
heading += 2.0 * f32::PI();
|
|
} else if heading > 2.0 * f32::PI() {
|
|
heading -= 2.0 * f32::PI();
|
|
}
|
|
let heading_degrees = heading * 180.0 / f32::PI();
|
|
defmt::info!("Heading1 {}", heading_degrees);
|
|
break;
|
|
}
|
|
Err(qmc5883l::Error::NotReady) => {
|
|
Mono::delay(1000.millis()).await;
|
|
}
|
|
e => {
|
|
let _ = e.unwrap();
|
|
}
|
|
}
|
|
}
|
|
|
|
loop {
|
|
defmt::info!("Compass 2");
|
|
match compass2.mag() {
|
|
Ok((x, y, z)) => {
|
|
defmt::info!("x2: {} y2: {} z2: {}", x, y, z);
|
|
|
|
let mut heading = (y as f32).atan2(x as f32); //+ declination_rads;
|
|
if heading < 0.0 {
|
|
heading += 2.0 * f32::PI();
|
|
} else if heading > 2.0 * f32::PI() {
|
|
heading -= 2.0 * f32::PI();
|
|
}
|
|
let heading_degrees = heading * 180.0 / f32::PI();
|
|
defmt::info!("Heading2 {}", heading_degrees);
|
|
break;
|
|
}
|
|
Err(qmc5883l::Error::NotReady) => {
|
|
Mono::delay(1000.millis()).await;
|
|
}
|
|
e => {
|
|
let _ = e.unwrap();
|
|
}
|
|
}
|
|
}
|
|
|
|
Mono::delay(100.millis()).await;
|
|
}
|
|
}
|
|
|
|
#[task(local = [spi1, encoder_az, encoder_el, spi_cs2, spi_cs3], shared = [az_angle])]
|
|
async fn poll_spi(mut cx: poll_spi::Context) {
|
|
let spi1 = cx.local.spi1;
|
|
let encoder_az = cx.local.encoder_az;
|
|
|
|
loop {
|
|
let (diag, gain) = encoder_az.diag_gain(spi1).unwrap();
|
|
defmt::info!("diag: {:08b} gain: {}", diag, gain);
|
|
defmt::info!("magnitude: {:?}", encoder_az.magnitude(spi1).unwrap());
|
|
|
|
let raw_angle = encoder_az.angle(spi1).unwrap();
|
|
let angle_deg = raw_angle as i32 * 3600 / 16384;
|
|
|
|
cx.shared.az_angle.lock(|az_angle| {
|
|
*az_angle = angle_deg;
|
|
});
|
|
|
|
defmt::info!("angle: {:?}", angle_deg);
|
|
|
|
Mono::delay(50.millis()).await;
|
|
}
|
|
}
|
|
|
|
#[task(local = [az_enable, az_dir, az_step], shared = [az_angle])]
|
|
async fn move_az(mut cx: move_az::Context) {
|
|
let az_enable = cx.local.az_enable;
|
|
let az_dir = cx.local.az_dir;
|
|
let az_step = cx.local.az_step;
|
|
|
|
let az_target = 42i32;
|
|
|
|
loop {
|
|
let az_angle = cx.shared.az_angle.lock(|az_angle| *az_angle);
|
|
let diff = az_angle - az_target;
|
|
if diff.abs() > 2 {
|
|
az_enable.set_high();
|
|
|
|
if diff > 0 {
|
|
az_dir.set_high();
|
|
} else {
|
|
az_dir.set_low();
|
|
}
|
|
|
|
az_step.set_high();
|
|
Mono::delay(250.micros()).await;
|
|
az_step.set_low();
|
|
Mono::delay(250.micros()).await;
|
|
} else {
|
|
az_enable.set_low();
|
|
Mono::delay(500.micros()).await;
|
|
}
|
|
}
|
|
}
|
|
}
|