190 lines
6.8 KiB
Rust
190 lines
6.8 KiB
Rust
use embassy_stm32::adc::Adc;
|
|
use embassy_stm32::gpio::{Level, Output, Speed};
|
|
use embassy_stm32::peripherals;
|
|
use embassy_time::{with_timeout, Delay, Duration, Timer};
|
|
use embassy_util::blocking_mutex::raw::ThreadModeRawMutex;
|
|
use embassy_util::channel::mpmc::{Receiver, Sender};
|
|
use embassy_util::{select, Either};
|
|
use futures_util::future::join;
|
|
use heapless::Vec;
|
|
|
|
use crate::usb::Gs232Cmd;
|
|
use crate::{AzElPair, RotorState};
|
|
|
|
// ADC reading for azimuth 0°
|
|
const AZ_MIN_READING: f32 = 0.0;
|
|
// ADC reading for azimuth 360°
|
|
const AZ_MAX_READING: f32 = 4096.0;
|
|
// Range of motion for azimuth (0 to AZ_RANGE)
|
|
const AZ_RANGE: f32 = 360.0;
|
|
// Tolerance for the azimuth setpoint
|
|
const AZ_SLOP : i16 = 1;
|
|
|
|
// ADC reading for elevation 0°
|
|
const EL_MIN_READING: f32 = 0.0;
|
|
// ADC reading for elevation 360°
|
|
const EL_MAX_READING: f32 = 4096.0;
|
|
// Range of motion for elevantion (0 to EL_RANGE)
|
|
const EL_RANGE: f32 = 180.0;
|
|
// Tolerance for the elevation setpoint
|
|
const EL_SLOP : i16 = 1;
|
|
|
|
#[embassy_executor::task]
|
|
pub async fn movement_task(
|
|
adc1: peripherals::ADC1,
|
|
mut az_pin: peripherals::PA0,
|
|
mut el_pin: peripherals::PA1,
|
|
cw_pin: peripherals::PA2,
|
|
ccw_pin: peripherals::PA3,
|
|
up_pin: peripherals::PA4,
|
|
down_pin: peripherals::PA5,
|
|
cmd_receiver: Receiver<'static, ThreadModeRawMutex, Gs232Cmd, 1>,
|
|
pos_sender: Sender<'static, ThreadModeRawMutex, AzElPair, 1>,
|
|
state_sender: Sender<'static, ThreadModeRawMutex, RotorState, 1>,
|
|
) {
|
|
// Initialize the rotor state
|
|
let mut rotor_state = RotorState {
|
|
actual_pos: AzElPair { az: 0, el: 0 },
|
|
setpoint_pos: AzElPair { az: 0, el: 0 },
|
|
stopped: true,
|
|
};
|
|
|
|
// Setup output pins for moving the rotor
|
|
let mut cw_pin = Output::new(cw_pin, Level::Low, Speed::Low);
|
|
let mut ccw_pin = Output::new(ccw_pin, Level::Low, Speed::Low);
|
|
let mut up_pin = Output::new(up_pin, Level::Low, Speed::Low);
|
|
let mut down_pin = Output::new(down_pin, Level::Low, Speed::Low);
|
|
|
|
// Setup the ADC for reading the rotor positions
|
|
let mut adc = Adc::new(adc1, &mut Delay);
|
|
|
|
// Do an initial ADC reading to initialize the averages
|
|
let az_reading = adc.read(&mut az_pin) as f32;
|
|
let el_reading = adc.read(&mut el_pin) as f32;
|
|
let mut az_average = Average::new(az_reading);
|
|
let mut el_average = Average::new(el_reading);
|
|
|
|
loop {
|
|
// Wait until either a new command has been received or 100ms have elapsed
|
|
match select(
|
|
cmd_receiver.recv(),
|
|
Timer::after(Duration::from_millis(100)),
|
|
)
|
|
.await
|
|
{
|
|
// A new command has been received. This task only cares about MoveTo and Stop.
|
|
Either::First(cmd) => match cmd {
|
|
// Move to command. Update the setpoint pair in the rotor state
|
|
Gs232Cmd::MoveTo(pair) => {
|
|
rotor_state.setpoint_pos = pair;
|
|
rotor_state.stopped = false;
|
|
}
|
|
// Stop command. Set the stopped flag.
|
|
Gs232Cmd::Stop => {
|
|
rotor_state.stopped = true;
|
|
}
|
|
// Everthing elese is an noop.
|
|
_ => {}
|
|
},
|
|
|
|
// Second case of the select statement. Timer has elapsed.
|
|
Either::Second(_) => {
|
|
// First read the current rotor position
|
|
let az_reading = adc.read(&mut az_pin) as f32;
|
|
let el_reading = adc.read(&mut el_pin) as f32;
|
|
|
|
// Apply the averaging filters
|
|
az_average.add(az_reading);
|
|
el_average.add(el_reading);
|
|
|
|
// Calculate the position in degreee
|
|
let az_actual = (az_average.average() - AZ_MIN_READING)
|
|
/ (AZ_MAX_READING - AZ_MIN_READING)
|
|
* AZ_RANGE;
|
|
let el_actual = (el_average.average() - EL_MIN_READING)
|
|
/ (EL_MAX_READING - EL_MIN_READING)
|
|
* EL_RANGE;
|
|
|
|
// Update the rotor state
|
|
rotor_state.actual_pos.az = az_actual as u16;
|
|
rotor_state.actual_pos.el = el_actual as u16;
|
|
|
|
let delta_az =
|
|
rotor_state.setpoint_pos.az as i16 - rotor_state.actual_pos.az as i16;
|
|
let delta_el =
|
|
rotor_state.setpoint_pos.el as i16 - rotor_state.actual_pos.el as i16;
|
|
|
|
if !rotor_state.stopped && delta_az > AZ_SLOP {
|
|
// Azimuth needs to move clockwise
|
|
cw_pin.set_high();
|
|
ccw_pin.set_low();
|
|
} else if !rotor_state.stopped && delta_az < -AZ_SLOP {
|
|
// Azimuth needs to move counter clockwise
|
|
cw_pin.set_low();
|
|
ccw_pin.set_high();
|
|
} else {
|
|
// Either azimuth is on the setpoint or the rotor has beend stopped.
|
|
cw_pin.set_low();
|
|
ccw_pin.set_low();
|
|
}
|
|
|
|
if !rotor_state.stopped && delta_el > EL_SLOP {
|
|
// Elevation needs to move up
|
|
up_pin.set_high();
|
|
down_pin.set_low();
|
|
} else if !rotor_state.stopped && delta_el < -EL_SLOP {
|
|
// Elevation needs to move down
|
|
up_pin.set_low();
|
|
down_pin.set_high();
|
|
} else {
|
|
// Either elevation is on the setpoint or the rotor has beend stopped.
|
|
up_pin.set_low();
|
|
down_pin.set_low();
|
|
}
|
|
|
|
// Send the state to the display task and the position usb.
|
|
// Use timeouts to prevent blocking if display or usb task are unresponsive.
|
|
join(
|
|
with_timeout(
|
|
Duration::from_millis(100),
|
|
pos_sender.send(rotor_state.actual_pos),
|
|
),
|
|
with_timeout(Duration::from_millis(100), state_sender.send(rotor_state)),
|
|
)
|
|
.await;
|
|
}
|
|
};
|
|
}
|
|
}
|
|
|
|
// Simple sliding average filter
|
|
struct Average {
|
|
pos: usize,
|
|
data: Vec<f32, 5>,
|
|
}
|
|
|
|
impl Average {
|
|
// Create a new filter and prefill the state using an initial value
|
|
fn new(initial: f32) -> Average {
|
|
let mut data: Vec<f32, 5> = Vec::new();
|
|
data.resize(5, initial).unwrap();
|
|
Average { pos: 0, data }
|
|
}
|
|
|
|
// Adds a new value to internal state
|
|
fn add(&mut self, sample: f32) {
|
|
self.data[self.pos] = sample;
|
|
self.pos = (self.pos + 1) % self.data.len();
|
|
}
|
|
|
|
// Calculate the average value from the internal state
|
|
fn average(&self) -> f32 {
|
|
let mut sum = 0.0;
|
|
for sample in &self.data {
|
|
sum += sample;
|
|
}
|
|
|
|
sum / self.data.len() as f32
|
|
}
|
|
}
|