rotctl: actually quit when receiving the quit message

Since gpredict doesn't have a park button or anything, this will just
automatically park the antenna when the gpredict rotator controller
disconnects. This may or may not actually be a good idea. We will see.

src/Config.zig

@@ -92,6 +92,7 @@ pub fn validate(self: Config, err_writer: anytype) !void {
 rotctl: RotControlConfig = .{
     .listen_address = "127.0.0.1",
     .listen_port = 4533,
+    .autopark = false,
 },
 labjack: LabjackConfig = .{
     .device = .autodetect,
@@ -127,6 +128,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 };
@@ -150,6 +152,7 @@ pub const MinMax = struct {
 const RotControlConfig = struct {
     listen_address: []const u8,
     listen_port: u16,
+    autopark: bool,
 };
 
 const LabjackConfig = struct {
@@ -179,6 +182,8 @@ const ControllerConfig = struct {
     angle_offset: AzEl,
     elevation_mask: f64,
 
+    feedback_window_samples: u8,
+
     const OutPair = struct {
         increase: lj.DigitalOutputChannel,
         decrease: lj.DigitalOutputChannel,

src/RotCtl.zig

@@ -33,7 +33,7 @@ pub fn run(allocator: std.mem.Allocator) !void {
         .rotator = try YaesuController.init(allocator),
     };
 
-    while (true) {
+    while (interface.running) {
         const client = try server.accept();
         defer {
             log.info("disconnecting client", .{});
@@ -42,8 +42,6 @@ pub fn run(allocator: std.mem.Allocator) !void {
         }
 
         interface.writer = .{ .unbuffered_writer = client.stream.writer() };
-        interface.running = true;
-        defer interface.running = false;
 
         log.info("client connected from {}", .{client.address});
 
@@ -60,6 +58,11 @@ pub fn run(allocator: std.mem.Allocator) !void {
                 std.mem.trim(u8, fbs.getWritten(), &std.ascii.whitespace),
             ) catch break;
         }
+
+        // loop ended due to client disconnect
+        if (interface.running and config.rotctl.autopark) {
+            interface.rotator.startPark();
+        }
     }
 }
 

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,9 +357,8 @@ 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();
 
@@ -303,10 +366,15 @@ const Controller = struct {
                 continue;
             };
 
+            {
                 self.lock.lock();
                 defer self.lock.unlock();
 
-                self.position = pos;
+                self.setPosition(self.feedback_buffer.get());
+            }
+
+            switch (self.current_state) {
+                .initializing, .idle => {
                     self.current_state = self.requested_state;
                 },
                 .calibration => {
@@ -328,26 +396,21 @@ const Controller = struct {
                         };
                     };
 
-                const pos = self.drive(pos_error) catch {
+                    self.drive(pos_error) 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 = self.requested_state;
                 },
                 .stopped => {
                     // attempt to reset the drive outputs
-                _ = self.updateAzEl() catch {};
+                    try self.labjack.writeIoLines(.{false} ** 4);
                     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) {