Added AZ and EL position readouts

This commit is contained in:
Sebastian 2022-08-24 20:08:51 +02:00
parent 4fb98ee9c5
commit e0dd01ad17
6 changed files with 95 additions and 28 deletions

2
Cargo.lock generated
View file

@ -1029,7 +1029,7 @@ dependencies = [
"embassy-util", "embassy-util",
"embedded-graphics", "embedded-graphics",
"embedded-hal 0.2.7", "embedded-hal 0.2.7",
"futures", "futures-util",
"heapless", "heapless",
"nb 1.0.0", "nb 1.0.0",
"panic-probe", "panic-probe",

View file

@ -20,7 +20,9 @@ panic-probe = { version = "0.3.0"}
cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] } cortex-m = { version = "0.7.6", features = ["critical-section-single-core"] }
cortex-m-rt = "0.7.0" cortex-m-rt = "0.7.0"
embedded-hal = "0.2.6" embedded-hal = "0.2.6"
futures = { version = "0.3.17", default-features = false, features = ["async-await"] } futures-util = { version = "0.3.23", default-features = false, features = ["async-await"] }
heapless = { version = "0.7.5", default-features = false, features = ['ufmt-impl']} heapless = { version = "0.7.5", default-features = false, features = ['ufmt-impl']}
nb = "1.0.0" nb = "1.0.0"
ufmt = "0.2.0" ufmt = "0.2.0"

View file

@ -50,7 +50,7 @@ pub async fn display_task(
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: false, stopped: true,
}; };
loop { loop {
@ -72,20 +72,28 @@ pub async fn display_task(
.draw(&mut display) .draw(&mut display)
.unwrap(); .unwrap();
Rectangle::new(Point::new(0, 19), Size::new(128, 23)) if !rotor_state.stopped {
.into_styled(style_filled) Rectangle::new(Point::new(0, 19), Size::new(128, 23))
.draw(&mut display) .into_styled(style_filled)
.unwrap(); .draw(&mut display)
.unwrap();
};
let setpoint_style = if !rotor_state.stopped {
text_large_inv
} else {
text_large
};
tmp.clear(); tmp.clear();
uwrite!(tmp, "AZ: {}", rotor_state.setpoint_pos.az).unwrap(); uwrite!(tmp, "AZ: {}", rotor_state.setpoint_pos.az).unwrap();
Text::new(&tmp, Point::new(1, 30), text_large_inv) Text::new(&tmp, Point::new(1, 30), setpoint_style)
.draw(&mut display) .draw(&mut display)
.unwrap(); .unwrap();
tmp.clear(); tmp.clear();
uwrite!(tmp, "EL: {}", rotor_state.setpoint_pos.el).unwrap(); uwrite!(tmp, "EL: {}", rotor_state.setpoint_pos.el).unwrap();
Text::new(&tmp, Point::new(64, 30), text_large_inv) Text::new(&tmp, Point::new(64, 30), setpoint_style)
.draw(&mut display) .draw(&mut display)
.unwrap(); .unwrap();

View file

@ -2,9 +2,7 @@
#![no_main] #![no_main]
#![feature(type_alias_impl_trait)] #![feature(type_alias_impl_trait)]
use core::fmt::Write; use defmt::Format;
use defmt::{panic, Format};
use defmt_rtt as _; use defmt_rtt as _;
use panic_probe as _; use panic_probe as _;
@ -15,7 +13,6 @@ use embassy_stm32::Config;
use embassy_time::{Duration, Timer}; use embassy_time::{Duration, Timer};
use embassy_util::blocking_mutex::raw::ThreadModeRawMutex; use embassy_util::blocking_mutex::raw::ThreadModeRawMutex;
use embassy_util::channel::mpmc::Channel; use embassy_util::channel::mpmc::Channel;
use embassy_util::Forever;
mod display; mod display;
use display::display_task; use display::display_task;
@ -74,10 +71,13 @@ async fn main(spawner: Spawner) {
spawner spawner
.spawn(movement_task( .spawn(movement_task(
p.ADC1,
p.PA0,
p.PA1,
p.PA2,
p.PA3, p.PA3,
p.PA4, p.PA4,
p.PA5, p.PA5,
p.PA6,
CMD_CHAN.receiver(), CMD_CHAN.receiver(),
POS_CHAN.sender(), POS_CHAN.sender(),
STATE_CHAN.sender(), STATE_CHAN.sender(),

View file

@ -1,21 +1,33 @@
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::{Duration, Instant, Timer}; use embassy_time::{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};
use futures_util::future::join;
use futures::future::join; use heapless::Vec;
use crate::usb::Gs232Cmd; use crate::usb::Gs232Cmd;
use crate::{AzElPair, RotorState}; use crate::{AzElPair, RotorState};
const AZ_MIN_READING: f32 = 0.0;
const AZ_MAX_READING: f32 = 4096.0;
const AZ_RANGE: f32 = 360.0;
const EL_MIN_READING: f32 = 0.0;
const EL_MAX_READING: f32 = 4096.0;
const EL_RANGE: f32 = 90.0;
#[embassy_executor::task] #[embassy_executor::task]
pub async fn movement_task( pub async fn movement_task(
cw_pin: peripherals::PA3, adc1: peripherals::ADC1,
ccw_pin: peripherals::PA4, mut az_pin: peripherals::PA0,
up_pin: peripherals::PA5, mut el_pin: peripherals::PA1,
down_pin: peripherals::PA6, cw_pin: peripherals::PA2,
ccw_pin: peripherals::PA3,
up_pin: peripherals::PA4,
down_pin: peripherals::PA5,
cmd_receiver: Receiver<'static, ThreadModeRawMutex, Gs232Cmd, 1>, cmd_receiver: Receiver<'static, ThreadModeRawMutex, Gs232Cmd, 1>,
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>,
@ -23,14 +35,21 @@ pub async fn movement_task(
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: false, stopped: true,
}; };
let mut adc = Adc::new(adc1, &mut Delay);
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);
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 { loop {
match select( match select(
cmd_receiver.recv(), cmd_receiver.recv(),
@ -49,14 +68,28 @@ pub async fn movement_task(
_ => {} _ => {}
}, },
Either::Second(_) => { Either::Second(_) => {
let az_reading = adc.read(&mut az_pin) as f32;
let el_reading = adc.read(&mut el_pin) as f32;
az_average.add(az_reading);
el_average.add(el_reading);
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;
rotor_state.actual_pos.az = az_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 {
rotor_state.actual_pos.az += 1;
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
{ {
rotor_state.actual_pos.az -= 1;
cw_pin.set_low(); cw_pin.set_low();
ccw_pin.set_high(); ccw_pin.set_high();
} else { } else {
@ -65,13 +98,11 @@ pub async fn movement_task(
} }
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 {
rotor_state.actual_pos.el += 1;
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
{ {
rotor_state.actual_pos.el -= 1;
up_pin.set_low(); up_pin.set_low();
down_pin.set_high(); down_pin.set_high();
} else { } else {
@ -84,8 +115,34 @@ pub async fn movement_task(
state_sender.send(rotor_state), state_sender.send(rotor_state),
) )
.await; .await;
//state_sender.send(rotor_state).await;
} }
}; };
} }
} }
struct Average {
pos: usize,
data: Vec<f32, 10>,
}
impl Average {
fn new(initial: f32) -> Average {
let mut data: Vec<f32, 10> = Vec::new();
data.resize(10, initial).unwrap();
Average { pos: 0, data }
}
fn add(&mut self, sample: f32) {
self.data[self.pos] = sample;
self.pos = (self.pos + 1) % self.data.len();
}
fn average(&self) -> f32 {
let mut sum = 0.0;
for sample in &self.data {
sum += sample;
}
sum / self.data.len() as f32
}
}

View file

@ -9,7 +9,7 @@ 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};
use futures::future::join; use futures_util::future::join;
use heapless::String; use heapless::String;
use ufmt::uwrite; use ufmt::uwrite;