AFG/radomctld
3
0
Fork 0
Code Issues 1 Pull requests Projects Releases Packages Wiki Activity

First end-to-end movement

Browse source
This commit is contained in:
Sebastian 2024-12-04 18:25:26 +01:00
parent f7483fe42a
commit 1498fd27ff
6 changed files with 436 additions and 92 deletions
Download patch file Download diff file Expand all files Collapse all files

99
firmware/src/main.rs
View file

@ -43,7 +43,7 @@ mod app {
use qmc5883l::{self, QMC5883L};
use radomctl_protocol::*;
use radomctl_protocol::{HostMessage, *};
use crate::bootloader;
use rtic_monotonics::systick::prelude::*;
@ -57,6 +57,10 @@ mod app {
struct Shared {
az_angle: i32,
az_compass: i32,
az_target: i32,
el_angle: i32,
el_target: i32,
}
// Local resources go here
@ -223,10 +227,14 @@ mod app {
poll_i2c::spawn().ok();
poll_spi::spawn().ok();
move_az::spawn().ok();
move_el::spawn().ok();
(
Shared {
az_angle: 0,
az_target: 0,
el_angle: 0,
el_target: 0,
az_compass: 0,
},
Local {
@ -332,16 +340,18 @@ mod app {
}
}
#[task(local = [spi1, encoder_az, encoder_el, spi_cs2, spi_cs3], shared = [az_angle])]
#[task(local = [spi1, encoder_az, encoder_el, spi_cs2, spi_cs3], shared = [az_angle, el_angle])]
async fn poll_spi(mut cx: poll_spi::Context) {
let spi1 = cx.local.spi1;
let encoder_az = cx.local.encoder_az;
let encoder_el = cx.local.encoder_el;
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;
@ -349,24 +359,29 @@ mod app {
*az_angle = angle_deg;
});
defmt::info!("angle: {:?}", angle_deg);
defmt::info!("az angle: {:?}", angle_deg);
let raw_angle = encoder_el.angle(spi1).unwrap();
let angle_deg = raw_angle as i32 * 3600 / 16384;
cx.shared.el_angle.lock(|el_angle| {
*el_angle = angle_deg;
});
defmt::info!("el angle: {:?}", angle_deg);
Mono::delay(1.millis()).await;
}
}
#[task(local = [az_enable, az_dir, az_step], shared = [az_angle, az_compass])]
#[task(local = [az_enable, az_dir, az_step], shared = [az_angle, az_target])]
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 mut counter = 0;
let mut az_target = 0;
loop {
//let az_target = 1800; //cx.shared.az_compass.lock(|az_compass| *az_compass);
let az_target = cx.shared.az_target.lock(|az_target| *az_target);
let az_angle = cx.shared.az_angle.lock(|az_angle| *az_angle);
let diff = az_angle - az_target;
@ -385,7 +400,7 @@ mod app {
1.millis()
};
if diff.abs() > 5 {
if diff.abs() > 50 {
az_enable.set_low();
if diff < 0 {
az_dir.set_high();
@ -401,16 +416,56 @@ mod app {
az_enable.set_high();
Mono::delay(delay).await;
}
counter = (counter + 1) % 1000;
if counter == 0 {
az_target = (az_target + 450) % 3600;
};
}
}
#[task(binds=OTG_FS, local=[usb_dev, usb_serial, usb_buffer])]
fn usb_fs(cx: usb_fs::Context) {
#[task(local = [el_enable, el_dir, el_step], shared = [el_angle, el_target])]
async fn move_el(mut cx: move_el::Context) {
let el_enable = cx.local.el_enable;
let el_dir = cx.local.el_dir;
let el_step = cx.local.el_step;
loop {
let el_target = cx.shared.el_target.lock(|el_target| *el_target);
let el_angle = cx.shared.el_angle.lock(|el_angle| *el_angle);
let diff = el_angle - el_target;
defmt::info!(
"angle diff/target/actual: {:?}/{:?}/{:?}",
diff,
el_target,
el_angle
);
let delay = if diff.abs() < 10 {
10.millis()
} else if diff < 100 {
5.millis()
} else {
1.millis()
};
if diff.abs() > 50 {
el_enable.set_low();
if diff < 0 {
el_dir.set_high();
} else {
el_dir.set_low();
}
el_step.set_low();
Mono::delay(delay / 2).await;
el_step.set_high();
Mono::delay(delay / 2).await;
} else {
el_enable.set_high();
Mono::delay(delay).await;
}
}
}
#[task(binds=OTG_FS, local=[usb_dev, usb_serial, usb_buffer], shared=[az_target, el_target, az_angle, el_angle])]
fn usb_fs(mut cx: usb_fs::Context) {
let usb_dev = cx.local.usb_dev;
let serial = cx.local.usb_serial;
let buffer = cx.local.usb_buffer;
@ -443,8 +498,8 @@ mod app {
HostMessage::RequestStatus => {
let status = StatusMessage {
position: Position {
az: 42.0,
el: 23.0,
az: cx.shared.az_angle.lock(|az| (*az / 10) as f32),
el: cx.shared.el_angle.lock(|el| (*el / 10) as f32),
az_endcoder: 0.0,
el_encoder: 0.0,
az_magnetic: 0.0,
@ -457,6 +512,10 @@ mod app {
to_vec_cobs::<RadomMessage, USB_BUFFER_SIZE>(&device_msg).unwrap();
serial.write(bytes.as_slice()).unwrap();
}
HostMessage::SetTarget(pos) => {
cx.shared.az_target.lock(|az| *az = (pos.az * 10.0) as i32);
cx.shared.el_target.lock(|el| *el = (pos.el * 10.0) as i32);
}
HostMessage::TriggerDFUBootloader => {
bootloader::reboot_to_bootloader();
}