From 1498fd27fffe1b66feed999f55d478a7f6673197 Mon Sep 17 00:00:00 2001 From: Sebastian Date: Wed, 4 Dec 2024 18:25:26 +0100 Subject: [PATCH] First end-to-end movement --- Cargo.lock | 267 ++++++++++++++++++++++++++++++++++++------- daemon/Cargo.toml | 5 + daemon/src/main.rs | 68 +++++++++-- daemon/src/rotor.rs | 82 +++++++++---- firmware/src/main.rs | 99 ++++++++++++---- protocol/src/lib.rs | 7 ++ 6 files changed, 436 insertions(+), 92 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index c5a32f8..99766db 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -53,7 +53,7 @@ version = "1.1.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "6d36fc52c7f6c869915e99412912f22093507da8d9e942ceaf66fe4b7c14422a" dependencies = [ - "windows-sys", + "windows-sys 0.52.0", ] [[package]] @@ -63,7 +63,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "5bf74e1b6e971609db8ca7a9ce79fd5768ab6ae46441c572e46cf596f59e57f8" dependencies = [ "anstyle", - "windows-sys", + "windows-sys 0.52.0", ] [[package]] @@ -230,9 +230,9 @@ checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b" [[package]] name = "bytes" -version = "1.6.1" +version = "1.9.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a12916984aab3fa6e39d655a33e09c0071eb36d6ab3aea5c2d78551f1df6d952" +checksum = "325918d6fe32f23b19878fe4b34794ae41fc19ddbe53b10571a4874d44ffd39b" [[package]] name = "cc" @@ -319,7 +319,7 @@ dependencies = [ "lazy_static", "libc", "unicode-width", - "windows-sys", + "windows-sys 0.52.0", ] [[package]] @@ -612,42 +612,92 @@ dependencies = [ ] [[package]] -name = "futures-channel" -version = "0.3.30" +name = "futures" +version = "0.3.31" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "eac8f7d7865dcb88bd4373ab671c8cf4508703796caa2b1985a9ca867b3fcb78" +checksum = "65bc07b1a8bc7c85c5f2e110c476c7389b4554ba72af57d8445ea63a576b0876" +dependencies = [ + "futures-channel", + "futures-core", + "futures-executor", + "futures-io", + "futures-sink", + "futures-task", + "futures-util", +] + +[[package]] +name = "futures-channel" +version = "0.3.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2dff15bf788c671c1934e366d07e30c1814a8ef514e1af724a602e8a2fbe1b10" dependencies = [ "futures-core", + "futures-sink", ] [[package]] name = "futures-core" -version = "0.3.30" +version = "0.3.31" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dfc6580bb841c5a68e9ef15c77ccc837b40a7504914d52e47b8b0e9bbda25a1d" +checksum = "05f29059c0c2090612e8d742178b0580d2dc940c837851ad723096f87af6663e" [[package]] -name = "futures-sink" -version = "0.3.30" +name = "futures-executor" +version = "0.3.31" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9fb8e00e87438d937621c1c6269e53f536c14d3fbd6a042bb24879e57d474fb5" - -[[package]] -name = "futures-task" -version = "0.3.30" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "38d84fa142264698cdce1a9f9172cf383a0c82de1bddcf3092901442c4097004" - -[[package]] -name = "futures-util" -version = "0.3.30" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3d6401deb83407ab3da39eba7e33987a73c3df0c82b4bb5813ee871c19c41d48" +checksum = "1e28d1d997f585e54aebc3f97d39e72338912123a67330d723fdbb564d646c9f" dependencies = [ "futures-core", "futures-task", + "futures-util", +] + +[[package]] +name = "futures-io" +version = "0.3.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9e5c1b78ca4aae1ac06c48a526a655760685149f0d465d21f37abfe57ce075c6" + +[[package]] +name = "futures-macro" +version = "0.3.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "162ee34ebcb7c64a8abebc059ce0fee27c2262618d7b60ed8faf72fef13c3650" +dependencies = [ + "proc-macro2", + "quote", + "syn 2.0.72", +] + +[[package]] +name = "futures-sink" +version = "0.3.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e575fab7d1e0dcb8d0c7bcf9a63ee213816ab51902e6d244a95819acacf1d4f7" + +[[package]] +name = "futures-task" +version = "0.3.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f90f7dce0722e95104fcb095585910c0977252f286e354b5e3bd38902cd99988" + +[[package]] +name = "futures-util" +version = "0.3.31" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9fa08315bb612088cc391249efdc3bc77536f16c91f6cf495e6fbe85b20a4a81" +dependencies = [ + "futures-channel", + "futures-core", + "futures-io", + "futures-macro", + "futures-sink", + "futures-task", + "memchr", "pin-project-lite", "pin-utils", + "slab", ] [[package]] @@ -872,7 +922,7 @@ checksum = "f23ff5ef2b80d608d61efee834934d862cd92461afc0560dedf493e4c033738b" dependencies = [ "hermit-abi", "libc", - "windows-sys", + "windows-sys 0.52.0", ] [[package]] @@ -980,6 +1030,15 @@ version = "2.7.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "78ca9ab1a0babb1e7d5695e3530886289c18cf2f87ec19a575a0abdce112e3a3" +[[package]] +name = "memoffset" +version = "0.7.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5de893c32cde5f383baa4c04c5d6dbdd735cfd4a794b0debdb2bb1b421da5ff4" +dependencies = [ + "autocfg", +] + [[package]] name = "mime" version = "0.3.17" @@ -1011,6 +1070,18 @@ dependencies = [ "adler", ] +[[package]] +name = "mio" +version = "0.8.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a4a650543ca06a924e8b371db273b2756685faae30f8487da1b56505a8f78b0c" +dependencies = [ + "libc", + "log", + "wasi", + "windows-sys 0.48.0", +] + [[package]] name = "mio" version = "1.0.1" @@ -1020,7 +1091,20 @@ dependencies = [ "hermit-abi", "libc", "wasi", - "windows-sys", + "windows-sys 0.52.0", +] + +[[package]] +name = "mio-serial" +version = "5.0.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "20a4c60ca5c9c0e114b3bd66ff4aa5f9b2b175442be51ca6c4365d687a97a2ac" +dependencies = [ + "log", + "mio 0.8.11", + "nix", + "serialport", + "winapi", ] [[package]] @@ -1047,6 +1131,8 @@ dependencies = [ "bitflags 1.3.2", "cfg-if", "libc", + "memoffset", + "pin-utils", ] [[package]] @@ -1194,7 +1280,7 @@ dependencies = [ "libc", "redox_syscall", "smallvec", - "windows-targets", + "windows-targets 0.52.6", ] [[package]] @@ -1365,9 +1451,12 @@ version = "0.1.0" dependencies = [ "anyhow", "axum", + "bytes", "clap", "dfu-libusb", "fern", + "futures", + "futures-util", "humantime", "indicatif", "libusb1-sys", @@ -1380,6 +1469,8 @@ dependencies = [ "serialport", "tokio", "tokio-macros 0.2.6", + "tokio-serial", + "tokio-util", "tower-http", "tracing", "tracing-subscriber", @@ -1631,6 +1722,15 @@ dependencies = [ "libc", ] +[[package]] +name = "slab" +version = "0.4.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8f92a496fb766b417c996b9c5e57daf2f7ad3b0bebe1ccfca4856390e3d3bb67" +dependencies = [ + "autocfg", +] + [[package]] name = "smallvec" version = "1.13.2" @@ -1644,7 +1744,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ce305eb0b4296696835b71df73eb912e0f1ffd2556a501fcede6e0c50349191c" dependencies = [ "libc", - "windows-sys", + "windows-sys 0.52.0", ] [[package]] @@ -1810,13 +1910,13 @@ dependencies = [ "backtrace", "bytes", "libc", - "mio", + "mio 1.0.1", "parking_lot", "pin-project-lite", "signal-hook-registry", "socket2", "tokio-macros 2.4.0", - "windows-sys", + "windows-sys 0.52.0", ] [[package]] @@ -1842,14 +1942,31 @@ dependencies = [ ] [[package]] -name = "tokio-util" -version = "0.7.11" +name = "tokio-serial" +version = "5.4.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9cf6b47b3771c49ac75ad09a6162f53ad4b8088b76ac60e8ec1455b31a189fe1" +checksum = "aa6e2e4cf0520a99c5f87d5abb24172b5bd220de57c3181baaaa5440540c64aa" +dependencies = [ + "bytes", + "cfg-if", + "futures", + "log", + "mio-serial", + "tokio", + "tokio-util", +] + +[[package]] +name = "tokio-util" +version = "0.7.13" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d7fcaa8d55a2bdd6b83ace262b016eca0d79ee02818c5c1bcdf0305114081078" dependencies = [ "bytes", "futures-core", "futures-sink", + "futures-util", + "hashbrown", "pin-project-lite", "tokio", ] @@ -2117,13 +2234,37 @@ version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f" +[[package]] +name = "windows-sys" +version = "0.48.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "677d2418bec65e3338edb076e806bc1ec15693c5d0104683f2efe857f61056a9" +dependencies = [ + "windows-targets 0.48.5", +] + [[package]] name = "windows-sys" version = "0.52.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "282be5f36a8ce781fad8c8ae18fa3f9beff57ec1b52cb3de0789201425d9a33d" dependencies = [ - "windows-targets", + "windows-targets 0.52.6", +] + +[[package]] +name = "windows-targets" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9a2fa6e2155d7247be68c096456083145c183cbbbc2764150dda45a87197940c" +dependencies = [ + "windows_aarch64_gnullvm 0.48.5", + "windows_aarch64_msvc 0.48.5", + "windows_i686_gnu 0.48.5", + "windows_i686_msvc 0.48.5", + "windows_x86_64_gnu 0.48.5", + "windows_x86_64_gnullvm 0.48.5", + "windows_x86_64_msvc 0.48.5", ] [[package]] @@ -2132,28 +2273,46 @@ version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "9b724f72796e036ab90c1021d4780d4d3d648aca59e491e6b98e725b84e99973" dependencies = [ - "windows_aarch64_gnullvm", - "windows_aarch64_msvc", - "windows_i686_gnu", + "windows_aarch64_gnullvm 0.52.6", + "windows_aarch64_msvc 0.52.6", + "windows_i686_gnu 0.52.6", "windows_i686_gnullvm", - "windows_i686_msvc", - "windows_x86_64_gnu", - "windows_x86_64_gnullvm", - "windows_x86_64_msvc", + "windows_i686_msvc 0.52.6", + "windows_x86_64_gnu 0.52.6", + "windows_x86_64_gnullvm 0.52.6", + "windows_x86_64_msvc 0.52.6", ] +[[package]] +name = "windows_aarch64_gnullvm" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2b38e32f0abccf9987a4e3079dfb67dcd799fb61361e53e2882c3cbaf0d905d8" + [[package]] name = "windows_aarch64_gnullvm" version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "32a4622180e7a0ec044bb555404c800bc9fd9ec262ec147edd5989ccd0c02cd3" +[[package]] +name = "windows_aarch64_msvc" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dc35310971f3b2dbbf3f0690a219f40e2d9afcf64f9ab7cc1be722937c26b4bc" + [[package]] name = "windows_aarch64_msvc" version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "09ec2a7bb152e2252b53fa7803150007879548bc709c039df7627cabbd05d469" +[[package]] +name = "windows_i686_gnu" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a75915e7def60c94dcef72200b9a8e58e5091744960da64ec734a6c6e9b3743e" + [[package]] name = "windows_i686_gnu" version = "0.52.6" @@ -2166,24 +2325,48 @@ version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "0eee52d38c090b3caa76c563b86c3a4bd71ef1a819287c19d586d7334ae8ed66" +[[package]] +name = "windows_i686_msvc" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8f55c233f70c4b27f66c523580f78f1004e8b5a8b659e05a4eb49d4166cca406" + [[package]] name = "windows_i686_msvc" version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "240948bc05c5e7c6dabba28bf89d89ffce3e303022809e73deaefe4f6ec56c66" +[[package]] +name = "windows_x86_64_gnu" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "53d40abd2583d23e4718fddf1ebec84dbff8381c07cae67ff7768bbf19c6718e" + [[package]] name = "windows_x86_64_gnu" version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "147a5c80aabfbf0c7d901cb5895d1de30ef2907eb21fbbab29ca94c5b08b1a78" +[[package]] +name = "windows_x86_64_gnullvm" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0b7b52767868a23d5bab768e390dc5f5c55825b6d30b86c844ff2dc7414044cc" + [[package]] name = "windows_x86_64_gnullvm" version = "0.52.6" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "24d5b23dc417412679681396f2b49f3de8c1473deb516bd34410872eff51ed0d" +[[package]] +name = "windows_x86_64_msvc" +version = "0.48.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ed94fce61571a4006852b7389a063ab983c02eb1bb37b47f8272ce92d06d9538" + [[package]] name = "windows_x86_64_msvc" version = "0.52.6" diff --git a/daemon/Cargo.toml b/daemon/Cargo.toml index 95f39bd..05bec0d 100644 --- a/daemon/Cargo.toml +++ b/daemon/Cargo.toml @@ -24,3 +24,8 @@ libusb1-sys = "0.6" rusb = "0.9" clap = { version = "4.5.19", features = ["derive"] } indicatif = "0.17.8" +tokio-serial = {version = "5.4.4", features = ["codec", "rt"] } +tokio-util = { version = "0.7.13", features = ["codec", "rt"] } +bytes = "1.9.0" +futures-util = "0.3.31" +futures = "0.3.31" diff --git a/daemon/src/main.rs b/daemon/src/main.rs index e52eece..e4cfcf1 100644 --- a/daemon/src/main.rs +++ b/daemon/src/main.rs @@ -1,7 +1,16 @@ use anyhow::Result; +use anyhow::{anyhow, Context}; +use axum::{ + extract::State, + http::StatusCode, + routing::{get, post}, + Json, Router, +}; +use clap::Parser; use fern::colors::{Color, ColoredLevelConfig}; use log::{debug, error, info, warn, Level}; use serde_json::{json, Value}; +use std::time::Duration; use std::{borrow::Borrow, io}; use tokio::{ self, @@ -10,13 +19,7 @@ use tokio::{ sync::{mpsc, watch}, task::JoinSet, }; - -use axum::{ - extract::State, - http::StatusCode, - routing::{get, post}, - Json, Router, -}; +use tokio_serial; use tower_http::{ services::{ServeDir, ServeFile}, trace::TraceLayer, @@ -86,16 +89,61 @@ struct AxumAppState { pos_rx: watch::Receiver<(f32, f32)>, } +#[derive(Parser)] +struct Cli { + /// The usb serial number of the radom-controller + #[arg(short, long)] + serialnumber: String, + + /// Listen address for the webserver + #[arg(short, long, default_value = "0.0.0.0:8000")] + web_listen_address: String, + + /// Listen address for rotctl + #[arg(short, long, default_value = "0.0.0.0:1337")] + rotctl_listen_address: String, +} + #[tokio::main] async fn main() -> Result<()> { setup_logger()?; + let args = Cli::parse(); + + let ports = tokio_serial::available_ports().unwrap_or(Vec::::new()); + + let mut radom_port: Option = None; + for port in ports { + match port.port_type { + serialport::SerialPortType::UsbPort(usb_port_info) => { + match usb_port_info.serial_number { + Some(serial) => { + debug!("Found a serial port with: {}", serial); + if serial == args.serialnumber { + radom_port = Some(port.port_name.to_owned()); + info!("Found radom-controller as {}", port.port_name) + } + } + None => continue, + } + } + _ => continue, + } + } + + let radom_port = match radom_port { + Some(port) => port, + _ => { + return Err(anyhow!("No matching port found.")); + } + }; + let (cmd_tx, cmd_rx) = mpsc::channel::(16); let (pos_tx, pos_rx) = watch::channel::<(f32, f32)>((0.0, 0.0)); let mut tasks = JoinSet::new(); - tasks.spawn(async move { control_rotor(cmd_rx, pos_tx).await }); + tasks.spawn(async move { control_rotor(cmd_rx, pos_tx, radom_port).await }); let state = AxumAppState { pos_rx: pos_rx.clone(), @@ -108,14 +156,14 @@ async fn main() -> Result<()> { .with_state(state) .layer(TraceLayer::new_for_http()); - let listener = tokio::net::TcpListener::bind("0.0.0.0:8000").await?; + let listener = tokio::net::TcpListener::bind(args.web_listen_address).await?; axum::serve(listener, app).await?; Ok(()) }); tasks.spawn(async move { - let listener = TcpListener::bind("127.0.0.1:1337").await?; + let listener = TcpListener::bind(args.rotctl_listen_address).await?; loop { let (socket, _) = listener.accept().await?; diff --git a/daemon/src/rotor.rs b/daemon/src/rotor.rs index 3c9b769..0a19c2b 100644 --- a/daemon/src/rotor.rs +++ b/daemon/src/rotor.rs @@ -1,5 +1,11 @@ use anyhow::Result; +use bytes::{BufMut, BytesMut}; +use futures::{stream::StreamExt, SinkExt}; use log::{debug, error, info, warn}; +use postcard::{from_bytes_cobs, to_stdvec_cobs}; +use radomctl_protocol::{HostMessage, PositionTarget, RadomMessage}; +use std::{env, io, str, time::Duration}; +use tokio::time::sleep; use tokio::{ self, io::{AsyncBufReadExt, AsyncWriteExt, BufStream}, @@ -7,46 +13,82 @@ use tokio::{ sync::{self, mpsc, watch}, time, }; +use tokio_serial::SerialPortBuilderExt; +use tokio_util::codec::{Decoder, Encoder}; use crate::rotctlprotocol::{parse_command, Command}; +struct ProtocolCodec; + +impl Decoder for ProtocolCodec { + type Item = RadomMessage; + type Error = io::Error; + + fn decode(&mut self, src: &mut BytesMut) -> Result, Self::Error> { + let frame_end = src.as_ref().iter().position(|b| *b == 0); + if let Some(n) = frame_end { + let mut frame = src.split_to(n + 1); + let host_msg = from_bytes_cobs::(&mut frame).unwrap(); + return Ok(Some(host_msg)); + } + Ok(None) + } +} + +impl Encoder for ProtocolCodec { + type Error = io::Error; + + fn encode(&mut self, item: HostMessage, dst: &mut BytesMut) -> Result<(), Self::Error> { + let msg_bytes = to_stdvec_cobs(&item).unwrap(); + dst.put(msg_bytes.as_slice()); + dst.put_u8(0); + Ok(()) + } +} + pub async fn control_rotor( mut rx_cmd: mpsc::Receiver, pos_tx: watch::Sender<(f32, f32)>, + radom_port: String, ) -> Result<()> { - let mut actual_az = 0.0; - let mut actual_el = 0.0; + let port = tokio_serial::new(radom_port, 115_200) + .timeout(Duration::from_millis(10)) + .open_native_async() + .expect("Failed to open port"); - let mut target_az = 0.0; - let mut target_el = 0.0; + let (mut port_writer, mut port_reader) = ProtocolCodec.framed(port).split(); loop { tokio::select! { Some(command) = rx_cmd.recv() => { match command { Command::SetPos(az, el) => { - info!("Received set pos {} {}", az, el); - target_az = az; - target_el = el; + //info!("Received set pos {} {}", az, el); + port_writer.send(HostMessage::SetTarget(PositionTarget { az, el })).await?; } _ => {} } }, + _ = time::sleep(time::Duration::from_millis(100)) => { - if target_az < actual_az { - actual_az -= 1.0; - } else if target_az > actual_az { - actual_az += 1.0; - } - - if target_el < actual_el { - actual_el -= 1.0; - } else if target_el > actual_el { - actual_el += 1.0; - } - - pos_tx.send((actual_az, actual_el)).unwrap(); + //info!("Requesting status"); + port_writer.send(HostMessage::RequestStatus).await?; }, + + msg = port_reader.next() => { + match msg { + Some(Ok(msg)) => { + match msg { + RadomMessage::Status(status) => { + //info!("Received status {:?}", status); + pos_tx.send((status.position.az, status.position.el)).unwrap(); + } + _ => {} + } + } + _ => {} + } + } else => return Ok(()) }; } diff --git a/firmware/src/main.rs b/firmware/src/main.rs index c3ea2d7..b541a2e 100644 --- a/firmware/src/main.rs +++ b/firmware/src/main.rs @@ -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::(&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(); } diff --git a/protocol/src/lib.rs b/protocol/src/lib.rs index 9ae57a1..1da7752 100644 --- a/protocol/src/lib.rs +++ b/protocol/src/lib.rs @@ -11,6 +11,7 @@ pub enum RadomMessage { #[derive(Serialize, Deserialize, Debug, PartialEq)] pub enum HostMessage { RequestStatus, + SetTarget(PositionTarget), TriggerDFUBootloader, } @@ -35,3 +36,9 @@ pub enum Alarm { AZEncoderFault, ELEncoderFault, } + +#[derive(Serialize, Deserialize, Debug, PartialEq)] +pub struct PositionTarget { + pub az: f32, + pub el: f32, +}