diff --git a/Cargo.lock b/Cargo.lock index 73f240f..1276a36 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -122,15 +122,6 @@ dependencies = [ "hash32 0.1.0 (registry+https://github.com/rust-lang/crates.io-index)", ] -[[package]] -name = "lexical-core" -version = "0.4.0" -dependencies = [ - "cfg-if 0.1.9 (registry+https://github.com/rust-lang/crates.io-index)", - "ryu 0.2.8 (registry+https://github.com/rust-lang/crates.io-index)", - "static_assertions 0.2.5 (registry+https://github.com/rust-lang/crates.io-index)", -] - [[package]] name = "log" version = "0.4.6" @@ -208,11 +199,6 @@ dependencies = [ "semver 0.9.0 (registry+https://github.com/rust-lang/crates.io-index)", ] -[[package]] -name = "ryu" -version = "0.2.8" -source = "registry+https://github.com/rust-lang/crates.io-index" - [[package]] name = "semver" version = "0.9.0" @@ -237,9 +223,9 @@ dependencies = [ [[package]] name = "serde-json-core" version = "0.0.1" +source = "git+https://github.com/quartiq/serde-json-core.git?rev=6143ac2#6143ac229ec3f5382555f37fe673413c8b46b0cf" dependencies = [ "heapless 0.4.4 (registry+https://github.com/rust-lang/crates.io-index)", - "lexical-core 0.4.0", "serde 1.0.91 (registry+https://github.com/rust-lang/crates.io-index)", ] @@ -275,7 +261,7 @@ dependencies = [ "panic-abort 0.3.1 (registry+https://github.com/rust-lang/crates.io-index)", "panic-semihosting 0.5.2 (registry+https://github.com/rust-lang/crates.io-index)", "serde 1.0.91 (registry+https://github.com/rust-lang/crates.io-index)", - "serde-json-core 0.0.1", + "serde-json-core 0.0.1 (git+https://github.com/quartiq/serde-json-core.git?rev=6143ac2)", "smoltcp 0.5.0 (registry+https://github.com/rust-lang/crates.io-index)", "stm32h7 0.7.0", ] @@ -285,11 +271,6 @@ name = "stable_deref_trait" version = "1.1.1" source = "registry+https://github.com/rust-lang/crates.io-index" -[[package]] -name = "static_assertions" -version = "0.2.5" -source = "registry+https://github.com/rust-lang/crates.io-index" - [[package]] name = "stm32h7" version = "0.7.0" @@ -360,14 +341,13 @@ dependencies = [ "checksum rand_core 0.3.1 (registry+https://github.com/rust-lang/crates.io-index)" = "7a6fdeb83b075e8266dcc8762c22776f6877a63111121f5f8c7411e5be7eed4b" "checksum rand_core 0.4.0 (registry+https://github.com/rust-lang/crates.io-index)" = "d0e7a549d590831370895ab7ba4ea0c1b6b011d106b5ff2da6eee112615e6dc0" "checksum rustc_version 0.2.3 (registry+https://github.com/rust-lang/crates.io-index)" = "138e3e0acb6c9fb258b19b67cb8abd63c00679d2851805ea151465464fe9030a" -"checksum ryu 0.2.8 (registry+https://github.com/rust-lang/crates.io-index)" = "b96a9549dc8d48f2c283938303c4b5a77aa29bfbc5b54b084fb1630408899a8f" "checksum semver 0.9.0 (registry+https://github.com/rust-lang/crates.io-index)" = "1d7eb9ef2c18661902cc47e535f9bc51b78acd254da71d375c2f6720d9a40403" "checksum semver-parser 0.7.0 (registry+https://github.com/rust-lang/crates.io-index)" = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3" "checksum serde 1.0.91 (registry+https://github.com/rust-lang/crates.io-index)" = "a72e9b96fa45ce22a4bc23da3858dfccfd60acd28a25bcd328a98fdd6bea43fd" +"checksum serde-json-core 0.0.1 (git+https://github.com/quartiq/serde-json-core.git?rev=6143ac2)" = "" "checksum serde_derive 1.0.91 (registry+https://github.com/rust-lang/crates.io-index)" = "101b495b109a3e3ca8c4cbe44cf62391527cdfb6ba15821c5ce80bcd5ea23f9f" "checksum smoltcp 0.5.0 (registry+https://github.com/rust-lang/crates.io-index)" = "fef582369edb298c6c41319a544ca9c4e83622f226055ccfcb35974fbb55ed34" "checksum stable_deref_trait 1.1.1 (registry+https://github.com/rust-lang/crates.io-index)" = "dba1a27d3efae4351c8051072d619e3ade2820635c3958d826bfea39d59b54c8" -"checksum static_assertions 0.2.5 (registry+https://github.com/rust-lang/crates.io-index)" = "c19be23126415861cb3a23e501d34a708f7f9b2183c5252d690941c2e69199d5" "checksum syn 0.15.34 (registry+https://github.com/rust-lang/crates.io-index)" = "a1393e4a97a19c01e900df2aec855a29f71cf02c402e2f443b8d2747c25c5dbe" "checksum typenum 1.10.0 (registry+https://github.com/rust-lang/crates.io-index)" = "612d636f949607bdf9b123b4a6f6d966dedf3ff669f7f045890d3a4a73948169" "checksum unicode-xid 0.1.0 (registry+https://github.com/rust-lang/crates.io-index)" = "fc72304796d0818e357ead4e000d19c9c174ab23dc11093ac919054d20a6a7fc" diff --git a/Cargo.toml b/Cargo.toml index a5732e7..87dc596 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -33,13 +33,13 @@ cortex-m-log = { version = "0.5", features = ["log-integration"] } log = "0.4" panic-abort = "0.3" panic-semihosting = { version = "0.5.2", optional = true } -serde-json-core = { version = "0.0" } serde = { version = "1.0", features = ["derive"], default-features = false } heapless = { version = "0.4" } -[patch.crates-io] -serde-json-core = { path = "../serde-json-core" } -lexical-core = { path = "../rust-lexical/lexical-core" } +[dependencies.serde-json-core] +# version = "0.0" +git = "https://github.com/quartiq/serde-json-core.git" +rev = "6143ac2" [dependencies.stm32h7] path = "../stm32-rs/stm32h7" @@ -47,7 +47,8 @@ path = "../stm32-rs/stm32h7" features = ["stm32h7x3", "rt"] [dependencies.smoltcp] -#git = "https://github.com/m-labs/smoltcp" +#git = "https://github.com/m-labs/smoltcp.git" +#rev = "cd893e6" version = "0.5" features = ["proto-ipv4", "socket-tcp"] default-features = false diff --git a/README.md b/README.md index c786184..3a51d16 100644 --- a/README.md +++ b/README.md @@ -9,14 +9,20 @@ * dual channel * SPI ADC * SPI DAC -* fixed AFE gains -* 500 kHz rate, timed -* < 2 µs latency, unmatched +* 500 kHz rate, timed sampling +* 2 µs latency, unmatched between channels * f32 IIR math * generic biquad (second order) IIR filter * anti-windup * derivative kick avoidance -* configurable output limits + +## Limitations/TODOs + +* Fixed AFE gains +* The IP and MAC address are [hardcoded](src/main.rs) +* Expose configurable limits +* 100Base-T only +* Digital IO, GPIO header, AFE header, EEM header are not handled ## Hardware @@ -33,3 +39,10 @@ See https://github.com/sinara-hw/Stabilizer * `rustup target add thumbv7em-none-eabihf` * `openocd -f stabilizer.cfg` and leave it running * `cargo run --release` + + +## Protocol + +Stabilizer can be configured via newline-delimited JSON over TCP. +It listens on port 1235. [stabilizer.py](stabilizer.py) contains a reference +implementation of the protocol. diff --git a/src/iir.rs b/src/iir.rs index 85c3787..344028f 100644 --- a/src/iir.rs +++ b/src/iir.rs @@ -39,6 +39,7 @@ fn macc(y0: T, x: &[T], a: &[T]) -> T x.iter().zip(a.iter()).map(|(&i, &j)| i * j).fold(y0, |y, xa| y + xa) } +#[allow(unused)] impl IIR { pub fn set_pi(&mut self, kp: f32, ki: f32, g: f32) -> Result<(), &str> { let ki = copysign(ki, kp); diff --git a/src/main.rs b/src/main.rs index f4f7cfc..14b08ec 100644 --- a/src/main.rs +++ b/src/main.rs @@ -601,16 +601,6 @@ fn main() -> ! { let dbgmcu = dp.DBGMCU; dbgmcu.apb1lfz1.modify(|_, w| w.tim2().set_bit()); - unsafe { - let t = 2e-6*2.; - IIR_CH[0].set_pi(1., 0., 0.).unwrap(); - IIR_CH[0].set_x_offset(0.*SCALE); - - IIR_CH[1].set_pi(-0.1, -10.*t, 0.).unwrap(); - IIR_CH[1].set_x_offset(0.1*SCALE); - IIR_CH[1].get_x_offset().unwrap(); - } - eth::setup(&rcc, &dp.SYSCFG); eth::setup_pins(&dp.GPIOA, &dp.GPIOB, &dp.GPIOC, &dp.GPIOG); @@ -644,6 +634,7 @@ fn main() -> ! { }); let mut last = 0; + let mut server = Server::new(); loop { // if ETHERNET_PENDING.swap(false, Ordering::Relaxed) { } let time = TIME.load(Ordering::Relaxed); @@ -653,14 +644,7 @@ fn main() -> ! { socket.listen(1234).unwrap_or_else(|e| warn!("TCP listen error: {:?}", e)); } else if last != time && socket.can_send() { last = time; - let s = unsafe { Status{ - t: time, - x0: IIR_STATE[0][0], - y0: IIR_STATE[0][2], - x1: IIR_STATE[1][0], - y1: IIR_STATE[1][2], - }}; - send_response(socket, &s); + handle_status(socket, time); } } { @@ -668,7 +652,7 @@ fn main() -> ! { if !(socket.is_open() || socket.is_listening()) { socket.listen(1235).unwrap_or_else(|e| warn!("TCP listen error: {:?}", e)); } else { - handle_command(socket); + server.handle_command(socket); } } @@ -684,7 +668,7 @@ fn main() -> ! { #[derive(Deserialize,Serialize)] struct Request { - channel: i8, + channel: u8, iir: IIR, } @@ -694,52 +678,79 @@ struct Response<'a> { message: &'a str, } -fn send_response(socket: &mut net::socket::TcpSocket, msg: &T) { +fn reply(socket: &mut net::socket::TcpSocket, msg: &T) { let mut u: String = to_string(msg).unwrap(); u.push('\n').unwrap(); socket.write_str(&u).unwrap(); } -fn handle_command(socket: &mut net::socket::TcpSocket) { - let mut data: Vec = Vec::new(); - let mut discard: bool = false; - while socket.can_recv() { - if socket.recv(|buf| { - let (len, found) = match buf.iter().position(|&c| c as char == '\n') { - Some(end) => (end + 1, true), - None => (buf.len(), false), - }; - if data.len() + len >= data.capacity() { - discard = true; - data.clear(); - } else if !discard && len > 0 { - data.extend_from_slice(&buf[..len - 1]).unwrap(); +struct Server { + data: Vec, + discard: bool, +} + +impl Server { + fn new() -> Self { + Self { data: Vec::new(), discard: false } + } + + fn handle_command(&mut self, socket: &mut net::socket::TcpSocket) { + while socket.can_recv() { + let found = socket.recv(|buf| { + let (len, found) = match buf.iter().position(|&c| c as char == '\n') { + Some(end) => (end + 1, true), + None => (buf.len(), false), + }; + if self.data.len() + len >= self.data.capacity() { + self.discard = true; + self.data.clear(); + } else if !self.discard && len > 0 { + self.data.extend_from_slice(&buf[..len - 1]).unwrap(); + } + (len, found) + }).unwrap(); + if !found { + continue; } - (len, found) - }).unwrap() { - let resp = if discard { - discard = false; - Response{ code: 500, message: "command buffer overflow" } + let resp = if self.discard { + self.discard = false; + Response{ code: 520, message: "command buffer overflow" } } else { - match from_slice::(&data) { + match from_slice::(&self.data) { Ok(request) => { - cortex_m::interrupt::free(|_| { - unsafe { IIR_CH[request.channel as usize] = request.iir; }; - }); - Response{ code: 200, message: "ok" } + if request.channel > 1 { + Response{ code: 530, message: "invalid channel" } + } else { + cortex_m::interrupt::free(|_| { + unsafe { IIR_CH[request.channel as usize] = request.iir; }; + }); + Response{ code: 200, message: "ok" } + } }, Err(err) => { - warn!("parse error {}", err); + warn!("parse error {:?}", err); Response{ code: 550, message: "parse error" } }, } }; - send_response(socket, &resp); + self.data.clear(); + reply(socket, &resp); socket.close(); } } } +fn handle_status(socket: &mut net::socket::TcpSocket, time: u32) { + let s = unsafe { Status{ + t: time, + x0: IIR_STATE[0][0], + y0: IIR_STATE[0][2], + x1: IIR_STATE[1][0], + y1: IIR_STATE[1][2], + }}; + reply(socket, &s); +} + #[derive(Serialize)] struct Status { t: u32, @@ -753,7 +764,7 @@ const SCALE: f32 = ((1 << 15) - 1) as f32; static mut IIR_STATE: [IIRState; 2] = [[0.; 5]; 2]; static mut IIR_CH: [IIR; 2] = [ IIR{ ba: [0., 0., 0., 0., 0.], y_offset: 0., - y_min: -SCALE, y_max: SCALE }; 2]; + y_min: -SCALE - 1., y_max: SCALE }; 2]; // seems to slow it down // #[link_section = ".data.spi1"] diff --git a/stabilizer.py b/stabilizer.py index 256b6e6..0b1e86a 100644 --- a/stabilizer.py +++ b/stabilizer.py @@ -1,6 +1,6 @@ import json import asyncio -from collections import OrderedDict +from collections import OrderedDict as OD import logging import numpy as np @@ -17,14 +17,14 @@ class StabilizerConfig: self.reader, self.writer = await asyncio.open_connection(host, port) async def set(self, channel, iir): - up = OrderedDict([("channel", channel), ("iir", iir.as_dict())]) + up = OD([("channel", channel), ("iir", iir.as_dict())]) s = json.dumps(up, separators=(",", ":")) assert "\n" not in s logger.debug("send %s", s) self.writer.write(s.encode() + b"\n") r = (await self.reader.readline()).decode() logger.debug("recv %s", r) - ret = json.loads(r, object_pairs_hook=OrderedDict) + ret = json.loads(r, object_pairs_hook=OD) if ret["code"] != 200: raise StabilizerError(ret) return ret @@ -32,15 +32,16 @@ class StabilizerConfig: class IIR: t_update = 2e-6 + full_scale = float((1 << 15) - 1) def __init__(self): self.ba = np.zeros(5, np.float32) self.y_offset = 0. - self.y_min = -float(1 << 15) - self.y_max = float(1 << 15) - 1 + self.y_min = -self.full_scale - 1 + self.y_max = self.full_scale def as_dict(self): - iir = OrderedDict() + iir = OD() iir["ba"] = [float(_) for _ in self.ba] iir["y_offset"] = self.y_offset iir["y_min"] = self.y_min @@ -70,7 +71,7 @@ class IIR: self.ba[4] = 0. def set_x_offset(self, o): - b = self.ba[:3].sum() + b = self.ba[:3].sum()*self.full_scale self.y_offset = b*o @@ -81,11 +82,12 @@ if __name__ == "__main__": p.add_argument("-c", "--channel", default=0, type=int, help="Stabilizer channel to configure") p.add_argument("-o", "--offset", default=0., type=float, - help="X offset, in ADC LSB") + help="input offset, in units of full scale") p.add_argument("-p", "--proportional-gain", default=1., type=float, - help="Proportional gain, in DAC LSB/ADC LSB") + help="Proportional gain, in units of 1") p.add_argument("-i", "--integral-gain", default=0., type=float, - help="Integral gain, in DAC LSB/(ADC LSB*s)") + help="Integral gain, in units of Hz, " + "sign taken from proportional-gain") args = p.parse_args() @@ -99,6 +101,7 @@ if __name__ == "__main__": i.set_x_offset(args.offset) s = StabilizerConfig() await s.connect(args.stabilizer) - r = await s.set(0, i) + assert args.channel in range(2) + r = await s.set(args.channel, i) loop.run_until_complete(main())