From c076456fc459b7f33d7ede36316ab0c5e72c441c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Tue, 28 May 2019 08:34:42 +0000 Subject: [PATCH 1/6] cargo: drop lexical --- Cargo.lock | 22 ---------------------- Cargo.toml | 1 - 2 files changed, 23 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index a270c99..80f366d 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.7 (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" @@ -239,7 +225,6 @@ name = "serde-json-core" version = "0.0.1" 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)", ] @@ -285,11 +270,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" @@ -361,14 +341,12 @@ 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_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 stm32h7 0.7.0 (registry+https://github.com/rust-lang/crates.io-index)" = "64173a3f8edb1bf9e17b14861695da2a7e6ca98afc99e0f249b62e4962cc478d" "checksum syn 0.15.33 (registry+https://github.com/rust-lang/crates.io-index)" = "ec52cd796e5f01d0067225a5392e70084acc4c0013fa71d55166d38a8b307836" "checksum typenum 1.10.0 (registry+https://github.com/rust-lang/crates.io-index)" = "612d636f949607bdf9b123b4a6f6d966dedf3ff669f7f045890d3a4a73948169" diff --git a/Cargo.toml b/Cargo.toml index 46643d6..ad5ab45 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -40,7 +40,6 @@ heapless = { version = "0.4" } [patch.crates-io] serde-json-core = { path = "../serde-json-core" } -lexical-core = { path = "../rust-lexical/lexical-core" } [dependencies.smoltcp] #git = "https://github.com/m-labs/smoltcp" From 6d927f99d9dc859745ca29b47b23360fd0122053 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Tue, 28 May 2019 08:40:17 +0000 Subject: [PATCH 2/6] cargo: use published serde-json-core --- Cargo.lock | 4 +++- Cargo.toml | 9 +++++---- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 80f366d..8f3d6d3 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -223,6 +223,7 @@ 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)", "serde 1.0.91 (registry+https://github.com/rust-lang/crates.io-index)", @@ -260,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 (registry+https://github.com/rust-lang/crates.io-index)", ] @@ -344,6 +345,7 @@ dependencies = [ "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" diff --git a/Cargo.toml b/Cargo.toml index ad5ab45..5dc4552 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -34,15 +34,16 @@ stm32h7 = { version = "0.7", features = ["stm32h7x3", "rt"] } 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" } +[dependencies.serde-json-core] +# version = "0.0" +git = "https://github.com/quartiq/serde-json-core.git" +rev = "6143ac2" [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"] From cdb57d72fa0f37d5485eae60c1b4911317870a62 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Tue, 28 May 2019 09:49:07 +0000 Subject: [PATCH 3/6] frontend: cleanup --- stabilizer.py | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/stabilizer.py b/stabilizer.py index 256b6e6..beecd50 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 @@ -40,7 +40,7 @@ class IIR: self.y_max = float(1 << 15) - 1 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 +70,7 @@ class IIR: self.ba[4] = 0. def set_x_offset(self, o): - b = self.ba[:3].sum() + b = self.ba[:3].sum()*((1 << 15) - 1) self.y_offset = b*o @@ -81,11 +81,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 +100,6 @@ if __name__ == "__main__": i.set_x_offset(args.offset) s = StabilizerConfig() await s.connect(args.stabilizer) - r = await s.set(0, i) + r = await s.set(args.channel, i) loop.run_until_complete(main()) From a15b490b42bd3dae988e90984932f5c4106d22c6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Tue, 28 May 2019 09:49:17 +0000 Subject: [PATCH 4/6] iir: remove old setting --- src/main.rs | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/src/main.rs b/src/main.rs index 9c8f670..6248406 100644 --- a/src/main.rs +++ b/src/main.rs @@ -619,16 +619,6 @@ fn main() -> ! { //dbgmcu.apb1lfz1.modify(|_, w| w.stop_tim2().set_bit()); // stop tim2 in debug unsafe { ptr::write_volatile(0x5c00_103c as *mut usize, 0x0000_0001) }; - 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); From 1cff400d0091f864708a4c041551fcbecb18e7b0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Tue, 28 May 2019 10:15:15 +0000 Subject: [PATCH 5/6] cleanup, robustify --- README.md | 21 +++++++++++++++++---- src/iir.rs | 1 + src/main.rs | 14 +++++++++----- stabilizer.py | 8 +++++--- 4 files changed, 32 insertions(+), 12 deletions(-) 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 6248406..cee7123 100644 --- a/src/main.rs +++ b/src/main.rs @@ -692,7 +692,7 @@ fn main() -> ! { #[derive(Deserialize,Serialize)] struct Request { - channel: i8, + channel: u8, iir: IIR, } @@ -731,10 +731,14 @@ fn handle_command(socket: &mut net::socket::TcpSocket) { } else { match from_slice::(&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: 500, 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); diff --git a/stabilizer.py b/stabilizer.py index beecd50..0b1e86a 100644 --- a/stabilizer.py +++ b/stabilizer.py @@ -32,12 +32,13 @@ 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 = OD() @@ -70,7 +71,7 @@ class IIR: self.ba[4] = 0. def set_x_offset(self, o): - b = self.ba[:3].sum()*((1 << 15) - 1) + b = self.ba[:3].sum()*self.full_scale self.y_offset = b*o @@ -100,6 +101,7 @@ if __name__ == "__main__": i.set_x_offset(args.offset) s = StabilizerConfig() await s.connect(args.stabilizer) + assert args.channel in range(2) r = await s.set(args.channel, i) loop.run_until_complete(main()) From b0921f67eb03e4387a1b331c2cfa21c1d005ad5b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Tue, 28 May 2019 10:59:26 +0000 Subject: [PATCH 6/6] refactor server --- src/main.rs | 85 ++++++++++++++++++++++++++++++++--------------------- 1 file changed, 51 insertions(+), 34 deletions(-) diff --git a/src/main.rs b/src/main.rs index cee7123..2997d71 100644 --- a/src/main.rs +++ b/src/main.rs @@ -652,6 +652,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); @@ -661,14 +662,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); } } { @@ -676,7 +670,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); } } @@ -702,37 +696,48 @@ 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) => { if request.channel > 1 { - Response{ code: 500, message: "invalid channel" } + Response{ code: 530, message: "invalid channel" } } else { cortex_m::interrupt::free(|_| { unsafe { IIR_CH[request.channel as usize] = request.iir; }; @@ -741,17 +746,29 @@ fn handle_command(socket: &mut net::socket::TcpSocket) { } }, 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, @@ -765,7 +782,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"]