Merge pull request #1014 from quartiq/full-pid

idsp: full pid, also redo trigger logic
quartiq · Feb 5, 2025 · 1b9121b · 1b9121b
2 parents e0d9c10 + c8d9e86
commit 1b9121b
Show file tree

Hide file tree

Showing 3 changed files with 43 additions and 28 deletions.
diff --git a/Cargo.lock b/Cargo.lock
diff --git a/hitl/loopback.py b/hitl/loopback.py
@@ -38,8 +38,9 @@ async def test_loopback(stabilizer, telemetry_queue, set_point, gain=1, channel=
         #     "max": u,
         # },
         "biquad/0/typ": "Pid",
-        "biquad/0/repr/Pid/ki": -1000,
-        "biquad/0/repr/Pid/kp": -0.1,
+        "biquad/0/repr/Pid/order": "I",
+        "biquad/0/repr/Pid/gain/i": -1000,
+        "biquad/0/repr/Pid/gain/p": -0.1,
         "biquad/0/repr/Pid/min": -10,
         "biquad/0/repr/Pid/max": 10,
         "biquad/0/repr/Pid/setpoint": set_point*gain,

diff --git a/src/bin/dual-iir.rs b/src/bin/dual-iir.rs
@@ -102,16 +102,16 @@ impl serial_settings::Settings for Settings {
 
 #[derive(Clone, Debug, Tree)]
 pub struct BiquadRepr {
+    /// Biquad parameters
+    #[tree(rename = "typ")]
+    repr: StrLeaf<iir::BiquadRepr<f32, f32>>,
     /// Subtree access
     #[tree(
         rename = "repr",
         typ = "iir::BiquadRepr<f32, f32>",
         defer = "*self.repr"
     )]
     _repr: (),
-    /// Biquad parameters
-    #[tree(rename = "typ")]
-    repr: StrLeaf<iir::BiquadRepr<f32, f32>>,
 }
 
 impl Default for BiquadRepr {
@@ -181,8 +181,12 @@ impl Channel {
 pub struct DualIir {
     /// Channel configuration
     ch: [Channel; 2],
+    /// Trigger handshake
+    #[tree(skip)]
+    trigger: bool,
     /// Trigger both signal sources
-    trigger: Leaf<bool>,
+    #[tree(validate=self.validate_trigger, rename="trigger")]
+    _trigger: Leaf<()>,
     /// Telemetry output period in seconds.
     telemetry_period: Leaf<f32>,
     /// Target IP and port for UDP streaming.
@@ -191,11 +195,23 @@ pub struct DualIir {
     stream: Leaf<StreamTarget>,
 }
 
+impl DualIir {
+    fn validate_trigger(
+        &mut self,
+        depth: usize,
+    ) -> Result<usize, &'static str> {
+        debug_assert!(!self.trigger);
+        self.trigger = true;
+        Ok(depth)
+    }
+}
+
 impl Default for DualIir {
     fn default() -> Self {
         Self {
-            telemetry_period: 10.0.into(),
-            trigger: false.into(),
+            telemetry_period: Leaf(10.0),
+            trigger: false,
+            _trigger: Leaf(()),
             stream: Default::default(),
             ch: Default::default(),
         }
@@ -217,15 +233,15 @@ mod app {
     #[shared]
     struct Shared {
         usb: UsbDevice,
-        network: NetworkUsers<DualIir, 7>,
+        network: NetworkUsers<DualIir, 8>,
         settings: Settings,
         active: [Active; 2],
         telemetry: TelemetryBuffer,
     }
 
     #[local]
     struct Local {
-        usb_terminal: SerialTerminal<Settings, 8>,
+        usb_terminal: SerialTerminal<Settings, 9>,
         sampling_timer: SamplingTimer,
         digital_inputs: (DigitalInput0, DigitalInput1),
         afes: (AFE0, AFE1),
@@ -240,7 +256,7 @@ mod app {
         let clock = SystemTimer::new(|| Systick::now().ticks());
 
         // Configure the microcontroller
-        let (stabilizer, _pounder) = hardware::setup::setup::<Settings, 8>(
+        let (stabilizer, _pounder) = hardware::setup::setup::<Settings, 9>(
             c.core,
             c.device,
             clock,
@@ -435,8 +451,8 @@ mod app {
             c.local.afes.0.set_gain(*settings.dual_iir.ch[0].gain);
             c.local.afes.1.set_gain(*settings.dual_iir.ch[1].gain);
 
-            if *settings.dual_iir.trigger {
-                settings.dual_iir.trigger = Leaf(false);
+            if settings.dual_iir.trigger {
+                settings.dual_iir.trigger = false;
                 let s = settings.dual_iir.ch.each_ref().map(|ch| {
                     let s = ch
                         .source
@@ -454,11 +470,6 @@ mod app {
                     }
                 });
             }
-
-            c.shared
-                .network
-                .lock(|net| net.direct_stream(*settings.dual_iir.stream));
-
             let b = settings.dual_iir.ch.each_ref().map(|ch| {
                 (
                     *ch.run,
@@ -473,6 +484,9 @@ mod app {
                     (a.run, a.biquad) = b;
                 }
             });
+            c.shared
+                .network
+                .lock(|net| net.direct_stream(*settings.dual_iir.stream));
         });
     }