const std = @import("std"); const lj = @import("./labjack.zig"); const Config = @import("./Config.zig"); const config = Config.global; const log = std.log.scoped(.yaesu_controller); const YaesuController = @This(); control_thread: std.Thread, lock: *std.Thread.Mutex, controller: *const Controller, pub const AzEl = struct { azimuth: f64, elevation: f64, }; pub const CalibrationRoutine = enum { feedback, orientation, }; pub fn calibrate(allocator: std.mem.Allocator, routine: CalibrationRoutine) !void { const controller = try YaesuController.init(allocator); defer { controller.quit(); controller.control_thread.join(); } switch (routine) { .feedback => try controller.calibrate_feedback(), .orientation => try controller.calibrate_orientation(), } } pub fn init(allocator: std.mem.Allocator) !YaesuController { const controller = try allocator.create(Controller); errdefer allocator.destroy(controller); 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 = &controller.lock, .controller = controller, }; } fn inRange(request: f64, comptime dof: enum { azimuth, elevation }) bool { return switch (dof) { // zig fmt: off .azimuth => request >= ( config.labjack.feedback_calibration.azimuth.minimum.angle + config.controller.angle_offset.azimuth ) and request <= ( config.labjack.feedback_calibration.azimuth.maximum.angle + config.controller.angle_offset.azimuth ), .elevation => request >= ( config.labjack.feedback_calibration.elevation.minimum.angle + config.controller.angle_offset.elevation ) and request <= ( config.labjack.feedback_calibration.elevation.maximum.angle + config.controller.angle_offset.elevation ), // zig fmt: on }; } pub fn setTarget(self: YaesuController, target: AzEl) error{OutOfRange}!void { self.lock.lock(); defer self.lock.unlock(); const masked_target: AzEl = .{ .azimuth = target.azimuth, .elevation = @min( @max(target.elevation, config.controller.elevation_mask), 180.0 - config.controller.elevation_mask, ), }; if (!inRange(masked_target.azimuth, .azimuth) or !inRange(masked_target.elevation, .elevation)) return error.OutOfRange; const controller = @constCast(self.controller); controller.target = masked_target; controller.requested_state = .running; } pub fn currentPosition(self: YaesuController) AzEl { self.lock.lock(); defer self.lock.unlock(); return self.controller.position; } pub fn waitForUpdate(self: YaesuController) AzEl { const controller = @constCast(self.controller); self.lock.lock(); defer self.lock.unlock(); controller.condition.wait(self.lock); return controller.position; } pub fn quit(self: YaesuController) void { self.lock.lock(); defer self.lock.unlock(); const controller = @constCast(self.controller); controller.requested_state = .stopped; } pub fn stop(self: YaesuController) void { self.lock.lock(); defer self.lock.unlock(); const controller = @constCast(self.controller); controller.target = controller.position; controller.requested_state = .idle; } pub fn startPark(self: YaesuController) void { self.setTarget(config.controller.parking_posture) catch unreachable; } fn calibrate_feedback(self: YaesuController) !void { _ = self; log.err("this isn't implemented yet, sorry.", .{}); return error.NotImplemented; } fn calibrate_orientation(self: YaesuController) !void { _ = self; log.err("this isn't implemented yet, sorry.", .{}); return error.NotImplemented; } fn runController(controller: *Controller) void { controller.run() catch { log.err( "the rotator control loop has terminated unexpectedly!!!!", .{}, ); }; } 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, labjack: lj.Labjack, lock: std.Thread.Mutex = .{}, condition: std.Thread.Condition = .{}, const ControllerState = enum { initializing, idle, calibration, running, stopped, }; fn init(allocator: std.mem.Allocator) !Controller { 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, .labjack = switch (config.labjack.device) { .autodetect => lj.Labjack.autodetect(), .serial_number => |sn| lj.Labjack.with_serial_number(sn), }, }; } 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(); self.labjack.id = info.local_id; } fn lerpOne(input: f64, cal_points: Config.MinMax) f64 { return (input - cal_points.minimum.voltage) * cal_points.slope() + cal_points.minimum.angle; } fn lerpAndOffsetAngles(input: [2]lj.AnalogReadResult) AzEl { return .{ .azimuth = lerpOne( input[0].voltage, config.labjack.feedback_calibration.azimuth, ) + config.controller.angle_offset.azimuth, .elevation = lerpOne( input[1].voltage, config.labjack.feedback_calibration.elevation, ) + config.controller.angle_offset.elevation, }; } const Sign = enum { negative, zero, positive, pub fn symbol(self: Sign) u21 { return switch (self) { .negative => '-', .zero => '×', .positive => '+', }; } }; fn signDeadzone(offset: f64, deadzone: f64) Sign { return if (@abs(offset) < deadzone) .zero else if (offset < 0) .negative else .positive; } fn updateFeedback(self: *Controller) !void { const inputs = .{ config.controller.azimuth_input, config.controller.elevation_input, }; const raw = try self.labjack.readAnalogWriteDigital( 2, inputs, null, true, ); self.feedback_buffer.push(raw); } fn drive(self: *const Controller, pos_error: AzEl) !void { const azsign = signDeadzone( pos_error.azimuth, config.controller.angle_tolerance.azimuth, ); const elsign = signDeadzone( pos_error.elevation, 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 = self.feedback_buffer.getRaw(); 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}", .{ self.position.azimuth, raw.azimuth, pos_error.azimuth, azsign.symbol(), self.position.elevation, raw.elevation, pos_error.elevation, elsign.symbol(), }, ); try self.labjack.writeIoLines(drive_signal); } fn setPosition(self: *Controller, position: AzEl) void { self.position = position; self.condition.broadcast(); } fn run(self: *Controller) !void { self.current_state = .initializing; var timer: LoopTimer = .{ .interval_ns = config.controller.loop_interval_ns }; while (timer.mark()) : (timer.sleep()) { self.updateFeedback() catch { self.lock.lock(); defer self.lock.unlock(); self.current_state = .stopped; continue; }; self.lock.lock(); defer self.lock.unlock(); self.setPosition(self.feedback_buffer.get()); switch (self.current_state) { .initializing, .idle => { self.current_state = self.requested_state; }, .calibration => { self.lock.lock(); defer self.lock.unlock(); // run calibration routine. psych, this does nothing. gottem self.current_state = .idle; 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, }; }; self.drive(pos_error) catch { self.lock.lock(); defer self.lock.unlock(); self.current_state = .stopped; continue; }; }, .stopped => { // attempt to reset the drive outputs try self.labjack.writeIoLines(.{false} ** 4); break; }, } } } }; pub const LoopTimer = struct { interval_ns: u64, start: i128 = 0, pub fn mark(self: *LoopTimer) bool { self.start = std.time.nanoTimestamp(); return true; } pub fn sleep(self: *LoopTimer) void { const now = std.time.nanoTimestamp(); const elapsed: u64 = @intCast(now - self.start); std.time.sleep(self.interval_ns - elapsed); } };