controller: restructure control loop

This should have been multiple commits, but it isn't. Sue me. This
change has two main goals:

1. Sample feedback at the beginning of the control loop iteration so
   that it is always up-to-date when we are computing the actual drive
   outputs. This means we're doing twice the amount of communication
   with the labjack (previously, setting the output and reading the
   feedback was done with a singe command). However, this makes the
   loop structure much more standard, and it means that we aren't
   constantly operating on feedback that is stale by one loop
   interval.

2. Sample feedback into a (configurable size) buffer. This lets us
   operate on aggregated feedback rather than on a single instantaneous
   data point. Right now, feedback is computed as a moving average,
   which acts as a rudimentary low-pass filter, reducing spurious
   single-loop actions due to feedback spikes or other noise. However,
   the other reason to aggregate some backwards data is that it will
   let us do automatic stall detection in a simple way, although that
   is not currently done.

src/Config.zig

@@ -127,6 +127,7 @@ controller: ControllerConfig = .{
     // this is a symmetric mask, so the minimum usable elevation is elevation_mask deg
     // and the maximum usable elevation is 180 - elevation_mask deg
     .elevation_mask = 0.0,
+    .feedback_window_samples = 3,
 },
 
 pub const VoltAngle = struct { voltage: f64, angle: f64 };
@@ -179,6 +180,8 @@ const ControllerConfig = struct {
     angle_offset: AzEl,
     elevation_mask: f64,
 
+    feedback_window_samples: u8,
+
     const OutPair = struct {
         increase: lj.DigitalOutputChannel,
         decrease: lj.DigitalOutputChannel,

src/YaesuController.zig

@@ -36,19 +36,16 @@ pub fn calibrate(allocator: std.mem.Allocator, routine: CalibrationRoutine) !voi
 }
 
 pub fn init(allocator: std.mem.Allocator) !YaesuController {
-    const lock = try allocator.create(std.Thread.Mutex);
-    errdefer allocator.destroy(lock);
-    lock.* = .{};
-
     const controller = try allocator.create(Controller);
     errdefer allocator.destroy(controller);
-    controller.init(lock);
+    controller.* = try Controller.init(allocator);
+    errdefer controller.deinit(allocator);
     // do this in the main thread so we can throw the error about it synchronously.
     try controller.connectLabjack();
 
     return .{
         .control_thread = try std.Thread.spawn(.{}, runController, .{controller}),
-        .lock = lock,
+        .lock = &controller.lock,
         .controller = controller,
     };
 }
@@ -101,16 +98,15 @@ pub fn currentPosition(self: YaesuController) AzEl {
     return self.controller.position;
 }
 
-pub fn startCalibration(self: YaesuController) void {
+pub fn waitForUpdate(self: YaesuController) AzEl {
-    // there are two different types of calibration:
+    const controller = @constCast(self.controller);
-    // 1. feedback calibration, running to the extents of the rotator
+
-    // 2. sun calibration, which determines the azimuth and elevation angle
+    self.lock.lock();
-    //    offset between the rotator's physical stops and geodetic north
+    defer self.lock.unlock();
-    //
+
-    // The former is (fairly) trivial to automate, just run until stall
+    controller.condition.wait(self.lock);
-    // (assuming there's no deadband in the feedback). The latter requires
+
-    // manual input as the human is the feedback hardware in the loop.
+    return controller.position;
-    _ = self;
 }
 
 pub fn quit(self: YaesuController) void {
@@ -155,16 +151,77 @@ fn runController(controller: *Controller) void {
     };
 }
 
+const FeedbackBuffer = struct {
+    samples: []f64,
+    index: usize = 0,
+
+    fn initZero(allocator: std.mem.Allocator, samples: usize) !FeedbackBuffer {
+        const buf = try allocator.alloc(f64, samples * 2);
+        @memset(buf, 0);
+        return .{ .samples = buf };
+    }
+
+    fn deinit(self: FeedbackBuffer, allocator: std.mem.Allocator) void {
+        allocator.free(self.samples);
+    }
+
+    fn push(self: *FeedbackBuffer, sample: [2]lj.AnalogReadResult) void {
+        const halfpoint = @divExact(self.samples.len, 2);
+        defer self.index = (self.index + 1) % halfpoint;
+
+        self.samples[self.index] = sample[0].voltage;
+        self.samples[self.index + halfpoint] = sample[1].voltage;
+    }
+
+    inline fn mean(data: []f64) f64 {
+        var accum: f64 = 0;
+        for (data) |pt| {
+            accum += pt;
+        }
+        return accum / @as(f64, @floatFromInt(data.len));
+    }
+
+    fn lerp(input: f64, cal_points: Config.MinMax) f64 {
+        return (input - cal_points.minimum.voltage) * cal_points.slope() + cal_points.minimum.angle;
+    }
+
+    fn get(self: FeedbackBuffer) AzEl {
+        const halfpoint = @divExact(self.samples.len, 2);
+
+        return .{
+            .azimuth = lerp(
+                mean(self.samples[0..halfpoint]),
+                config.labjack.feedback_calibration.azimuth,
+            ) + config.controller.angle_offset.azimuth,
+            .elevation = lerp(
+                mean(self.samples[halfpoint..]),
+                config.labjack.feedback_calibration.elevation,
+            ) + config.controller.angle_offset.elevation,
+        };
+    }
+
+    fn getRaw(self: FeedbackBuffer) AzEl {
+        const halfpoint = @divExact(self.samples.len, 2);
+        return .{
+            .azimuth = mean(self.samples[0..halfpoint]),
+            .elevation = mean(self.samples[halfpoint..]),
+        };
+    }
+};
+
 const Controller = struct {
     target: AzEl,
     position: AzEl,
+    feedback_buffer: FeedbackBuffer,
 
     current_state: ControllerState,
     requested_state: ControllerState,
 
-    lock: *std.Thread.Mutex,
     labjack: lj.Labjack,
 
+    lock: std.Thread.Mutex = .{},
+    condition: std.Thread.Condition = .{},
+
     const ControllerState = enum {
         initializing,
         idle,
@@ -173,13 +230,13 @@ const Controller = struct {
         stopped,
     };
 
-    fn init(self: *Controller, lock: *std.Thread.Mutex) void {
+    fn init(allocator: std.mem.Allocator) !Controller {
-        self.* = .{
+        return .{
             .target = .{ .azimuth = 0, .elevation = 0 },
             .position = .{ .azimuth = 0, .elevation = 0 },
+            .feedback_buffer = try FeedbackBuffer.initZero(allocator, config.controller.feedback_window_samples),
             .current_state = .stopped,
             .requested_state = .idle,
-            .lock = lock,
             .labjack = switch (config.labjack.device) {
                 .autodetect => lj.Labjack.autodetect(),
                 .serial_number => |sn| lj.Labjack.with_serial_number(sn),
@@ -187,6 +244,10 @@ const Controller = struct {
         };
     }
 
+    fn deinit(self: Controller, allocator: std.mem.Allocator) void {
+        self.feedback_buffer.deinit(allocator);
+    }
+
     fn connectLabjack(self: *Controller) !void {
         const info = try self.labjack.connect();
         try self.labjack.setAllDigitalOutputLow();
@@ -233,26 +294,23 @@ const Controller = struct {
             .positive;
     }
 
-    fn updateAzEl(self: *const Controller) !AzEl {
+    fn updateFeedback(self: *Controller) !void {
-        const inputs = .{ config.controller.azimuth_input, config.controller.elevation_input };
+        const inputs = .{
+            config.controller.azimuth_input,
+            config.controller.elevation_input,
+        };
 
         const raw = try self.labjack.readAnalogWriteDigital(
             2,
             inputs,
-            .{false} ** 4,
+            null,
             true,
         );
 
-        return lerpAndOffsetAngles(raw);
+        self.feedback_buffer.push(raw);
     }
 
-    fn drive(self: *const Controller, pos_error: AzEl) !AzEl {
+    fn drive(self: *const Controller, pos_error: AzEl) !void {
-        // NOTE: feedback will be roughly config.controller.loop_interval_ns out of
-        //       date. For high loop rates, this shouldn't be an issue.
-
-        const inputs = .{ config.controller.azimuth_input, config.controller.elevation_input };
-        var drive_signal: [4]bool = .{false} ** 4;
-
         const azsign = signDeadzone(
             pos_error.azimuth,
             config.controller.angle_tolerance.azimuth,
@@ -263,29 +321,35 @@ const Controller = struct {
             config.controller.angle_tolerance.elevation,
         );
 
+        var drive_signal: [4]bool = .{false} ** 4;
         drive_signal[config.controller.azimuth_outputs.increase.io] = azsign == .positive;
         drive_signal[config.controller.azimuth_outputs.decrease.io] = azsign == .negative;
         drive_signal[config.controller.elevation_outputs.increase.io] = elsign == .positive;
         drive_signal[config.controller.elevation_outputs.decrease.io] = elsign == .negative;
 
-        const raw = try self.labjack.readAnalogWriteDigital(2, inputs, drive_signal, true);
+        const raw = self.feedback_buffer.getRaw();
-        const angles = lerpAndOffsetAngles(raw);
 
         log.info(
             // -180.1 is 6 chars. -5.20 is 5 chars
             "az: {d: >6.1}° ({d: >5.2} V) Δ {d: >6.1}° => {u}, el: {d: >6.1}° ({d: >5.2} V) Δ {d: >6.1}° => {u}",
             .{
-                angles.azimuth,
+                self.position.azimuth,
-                raw[0].voltage,
+                raw.azimuth,
                 pos_error.azimuth,
                 azsign.symbol(),
-                angles.elevation,
+                self.position.elevation,
-                raw[1].voltage,
+                raw.elevation,
                 pos_error.elevation,
                 elsign.symbol(),
             },
         );
-        return angles;
+
+        try self.labjack.writeIoLines(drive_signal);
+    }
+
+    fn setPosition(self: *Controller, position: AzEl) void {
+        self.position = position;
+        self.condition.broadcast();
     }
 
     fn run(self: *Controller) !void {
@@ -293,61 +357,60 @@ const Controller = struct {
 
         var timer: LoopTimer = .{ .interval_ns = config.controller.loop_interval_ns };
 
-        while (timer.mark()) : (timer.sleep()) switch (self.current_state) {
+        while (timer.mark()) : (timer.sleep()) {
-            .initializing, .idle => {
+            self.updateFeedback() catch {
-                const pos = self.updateAzEl() catch {
-                    self.lock.lock();
-                    defer self.lock.unlock();
-
-                    self.current_state = .stopped;
-                    continue;
-                };
-
                 self.lock.lock();
                 defer self.lock.unlock();
 
-                self.position = pos;
+                self.current_state = .stopped;
-                self.current_state = self.requested_state;
+                continue;
-            },
+            };
-            .calibration => {
+
+            {
                 self.lock.lock();
                 defer self.lock.unlock();
 
-                // run calibration routine. psych, this does nothing. gottem
+                self.setPosition(self.feedback_buffer.get());
-                self.current_state = .idle;
+            }
-                self.requested_state = self.current_state;
+
-            },
+            switch (self.current_state) {
-            .running => {
+                .initializing, .idle => {
-                const pos_error: AzEl = blk: {
+                    self.current_state = self.requested_state;
+                },
+                .calibration => {
                     self.lock.lock();
                     defer self.lock.unlock();
 
-                    break :blk .{
+                    // run calibration routine. psych, this does nothing. gottem
-                        .azimuth = self.target.azimuth - self.position.azimuth,
+                    self.current_state = .idle;
-                        .elevation = self.target.elevation - self.position.elevation,
+                    self.requested_state = self.current_state;
+                },
+                .running => {
+                    const pos_error: AzEl = blk: {
+                        self.lock.lock();
+                        defer self.lock.unlock();
+
+                        break :blk .{
+                            .azimuth = self.target.azimuth - self.position.azimuth,
+                            .elevation = self.target.elevation - self.position.elevation,
+                        };
                     };
-                };
 
-                const pos = self.drive(pos_error) catch {
+                    self.drive(pos_error) catch {
-                    self.lock.lock();
+                        self.lock.lock();
-                    defer self.lock.unlock();
+                        defer self.lock.unlock();
 
-                    self.current_state = .stopped;
+                        self.current_state = .stopped;
-                    continue;
+                        continue;
-                };
+                    };
-
+                },
-                self.lock.lock();
+                .stopped => {
-                defer self.lock.unlock();
+                    // attempt to reset the drive outputs
-
+                    try self.labjack.writeIoLines(.{false} ** 4);
-                self.position = pos;
+                    break;
-                self.current_state = self.requested_state;
+                },
-            },
+            }
-            .stopped => {
+        }
-                // attempt to reset the drive outputs
-                _ = self.updateAzEl() catch {};
-                break;
-            },
-        };
     }
 };
 

src/labjack.zig

@@ -68,6 +68,30 @@ pub const Labjack = struct {
             return status.toError();
     }
 
+    pub fn writeIoLines(self: Labjack, out: [4]bool) LabjackError!void {
+        var id = self.cId();
+
+        var d_modes: c_long = 0xFF_FF;
+        var d_outputs: c_long = 0;
+        var d_states: c_long = 0;
+        const io_modes: c_long = 0b1111;
+        var io_outputs: c_long = PackedOutput.fromBoolArray(out).toCLong();
+
+        const status = c_api.DigitalIO(
+            &id,
+            self.demo(),
+            &d_modes,
+            io_modes,
+            &d_outputs,
+            &io_outputs,
+            1, // actually update the pin modes
+            &d_states,
+        );
+
+        if (!status.okay())
+            return status.toError();
+    }
+
     /// Read one analog input channel, either single-ended or differential
     pub fn analogReadOne(self: Labjack, input: AnalogInput) LabjackError!AnalogReadResult {
         if (!input.channel.isDifferential() and input.gain_index != 0) {