Added movement task and rewired the channels

src/display.rs

@@ -4,6 +4,7 @@ use embassy_stm32::time::Hertz;
 use embassy_time::{Duration, Timer};
 use embassy_util::blocking_mutex::raw::ThreadModeRawMutex;
 use embassy_util::channel::mpmc::Receiver;
+use embassy_util::{select, Either};
 
 use embedded_graphics::{
     mono_font::{
@@ -21,14 +22,14 @@ use heapless::String;
 
 use ssd1306::{prelude::*, I2CDisplayInterface, Ssd1306};
 
-use crate::AzElPair;
+use crate::{AzElPair, RotorState};
 
 #[embassy_executor::task]
 pub async fn display_task(
     i2c1: peripherals::I2C1,
     sda: peripherals::PB6,
     scl: peripherals::PB7,
-    cmd_receiver: Receiver<'static, ThreadModeRawMutex, AzElPair, 1>,
+    cmd_receiver: Receiver<'static, ThreadModeRawMutex, RotorState, 1>,
 ) {
     let i2c = i2c::I2c::new(i2c1, sda, scl, Hertz::hz(100_000), i2c::Config::default());
 
@@ -46,9 +47,13 @@ pub async fn display_task(
         .fill_color(BinaryColor::On)
         .build();
 
-    loop {
+    let mut rotor_state = RotorState {
-        let pair = cmd_receiver.recv().await;
+        actual_pos: AzElPair { az: 0, el: 0 },
+        setpoint_pos: AzElPair { az: 0, el: 0 },
+        stopped: false,
+    };
 
+    loop {
         display.clear();
 
         Text::new("AFG rotor ctrl v0.1.0", Point::new(0, 6), text_small)
@@ -69,19 +74,28 @@ pub async fn display_task(
             .unwrap();
 
         let mut tmp: String<16> = String::new();
-        write!(tmp, "AZ: {}", pair.az).unwrap();
+        write!(tmp, "AZ: {}", rotor_state.setpoint_pos.az).unwrap();
         Text::new(&tmp, Point::new(1, 30), text_large_inv)
             .draw(&mut display)
             .unwrap();
 
         tmp.clear();
-        write!(tmp, "EL: {}", pair.el).unwrap();
+        write!(tmp, "EL: {}", rotor_state.setpoint_pos.el).unwrap();
         Text::new(&tmp, Point::new(64, 30), text_large_inv)
             .draw(&mut display)
             .unwrap();
 
         display.flush().unwrap();
 
-        Timer::after(Duration::from_millis(500)).await;
+        let result = select(
+            Timer::after(Duration::from_millis(100)),
+            cmd_receiver.recv(),
+        )
+        .await;
+
+        match result {
+            Either::First(_) => {}
+            Either::Second(new_state) => rotor_state = new_state,
+        }
     }
 }

src/main.rs

@@ -21,18 +21,29 @@ mod display;
 use display::display_task;
 
 mod usb;
-use usb::usb_task;
+use usb::{usb_task, Gs232Cmd};
 
-#[derive(PartialEq, Format)]
+mod movement;
+use movement::movement_task;
+
+#[derive(PartialEq, Format, Copy, Clone)]
 pub struct AzElPair {
     az: u16,
     el: u16,
 }
 
+#[derive(PartialEq, Format, Copy, Clone)]
+pub struct RotorState {
+    actual_pos: AzElPair,
+    setpoint_pos: AzElPair,
+    stopped: bool,
+}
+
 #[embassy_executor::main]
 async fn main(spawner: Spawner) {
-    static CMD_CHAN: Channel<ThreadModeRawMutex, AzElPair, 1> = Channel::new();
+    static CMD_CHAN: Channel<ThreadModeRawMutex, Gs232Cmd, 1> = Channel::new();
     static POS_CHAN: Channel<ThreadModeRawMutex, AzElPair, 1> = Channel::new();
+    static STATE_CHAN: Channel<ThreadModeRawMutex, RotorState, 1> = Channel::new();
 
     let mut config = Config::default();
     config.rcc.hse = Some(Hertz(8_000_000));
@@ -54,7 +65,15 @@ async fn main(spawner: Spawner) {
     spawner
         .spawn(usb_task(p.USB, p.PA12, p.PA11, CMD_CHAN.sender()))
         .unwrap();
+
     spawner
-        .spawn(display_task(p.I2C1, p.PB6, p.PB7, CMD_CHAN.receiver()))
+        .spawn(movement_task(
+            CMD_CHAN.receiver(),
+            POS_CHAN.sender(),
+            STATE_CHAN.sender(),
+        ))
+        .unwrap();
+    spawner
+        .spawn(display_task(p.I2C1, p.PB6, p.PB7, STATE_CHAN.receiver()))
         .unwrap();
 }

src/movement.rs

@@ -0,0 +1,50 @@
+use defmt::Format;
+
+use core::fmt::Write;
+use embassy_stm32::interrupt;
+use embassy_stm32::peripherals;
+use embassy_stm32::usb::Driver;
+use embassy_usb::Builder;
+use embassy_usb_serial::{CdcAcmClass, State};
+use embassy_util::blocking_mutex::raw::ThreadModeRawMutex;
+use embassy_util::channel::mpmc::{Receiver, Sender};
+
+use futures::future::join;
+use heapless::String;
+
+use crate::usb::Gs232Cmd;
+use crate::{AzElPair, RotorState};
+
+#[embassy_executor::task]
+pub async fn movement_task(
+    cmd_receiver: Receiver<'static, ThreadModeRawMutex, Gs232Cmd, 1>,
+    pos_sender: Sender<'static, ThreadModeRawMutex, AzElPair, 1>,
+    state_sender: Sender<'static, ThreadModeRawMutex, RotorState, 1>,
+) {
+    let mut rotor_state = RotorState {
+        actual_pos: AzElPair { az: 0, el: 0 },
+        setpoint_pos: AzElPair { az: 0, el: 0 },
+        stopped: false,
+    };
+
+    loop {
+        let cmd = cmd_receiver.recv().await;
+
+        match cmd {
+            Gs232Cmd::MoveTo(pair) => {
+                rotor_state.setpoint_pos = pair;
+                rotor_state.stopped = false;
+            }
+            Gs232Cmd::Stop => {
+                rotor_state.stopped = true;
+            }
+            _ => {}
+        };
+
+        join(
+            pos_sender.send(rotor_state.actual_pos),
+            state_sender.send(rotor_state),
+        )
+        .await;
+    }
+}

src/usb.rs

@@ -1,5 +1,6 @@
 use defmt::Format;
 
+use core::fmt::Write;
 use embassy_stm32::interrupt;
 use embassy_stm32::peripherals;
 use embassy_stm32::usb::Driver;
@@ -9,8 +10,6 @@ use embassy_util::blocking_mutex::raw::ThreadModeRawMutex;
 use embassy_util::channel::mpmc::Sender;
 
 use futures::future::join;
-
-use core::fmt::Write;
 use heapless::String;
 
 use crate::AzElPair;
@@ -20,7 +19,7 @@ pub async fn usb_task(
     usb: peripherals::USB,
     dp_pin: peripherals::PA12,
     dm_pin: peripherals::PA11,
-    cmd_sender: Sender<'static, ThreadModeRawMutex, AzElPair, 1>,
+    cmd_sender: Sender<'static, ThreadModeRawMutex, Gs232Cmd, 1>,
 ) {
     let irq = interrupt::take!(USB_LP_CAN1_RX0);
     let driver = Driver::new(usb, irq, dp_pin, dm_pin);
@@ -103,11 +102,12 @@ pub async fn usb_task(
                         Gs232Cmd::GetAlEz => {
                             write!(&mut resp, "AZ={} EL={}\r", az_actual, el_actual).unwrap();
                         }
-                        Gs232Cmd::MoveTo(pair) => {
+                        Gs232Cmd::MoveTo(_) => {
-                            cmd_sender.send(pair).await;
+                            cmd_sender.send(cmd).await;
                             resp.push_str("\r").unwrap();
                         }
                         Gs232Cmd::Stop => {
+                            cmd_sender.send(cmd).await;
                             resp.push_str("\r").unwrap();
                         }
                         _ => {
@@ -134,7 +134,7 @@ pub async fn usb_task(
 }
 
 #[derive(Format, PartialEq)]
-enum Gs232Cmd {
+pub enum Gs232Cmd {
     Unkown,
     GetAl,
     GetEz,
@@ -167,7 +167,7 @@ fn parse_command(data: &str) -> Gs232Cmd {
             if data.len() == 8 {
                 if let Ok(az) = data[1..4].parse::<u16>() {
                     if let Ok(el) = data[5..].parse::<u16>() {
-                        Gs232Cmd::MoveTo(AzElPair { az: az, el: el })
+                        Gs232Cmd::MoveTo(AzElPair { az, el })
                     } else {
                         Gs232Cmd::Unkown
                     }