Added comments to movement.rs

This commit is contained in:
Sebastian 2022-08-28 22:03:17 +02:00
parent ce049b81d2
commit 045eada9d3

View file

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