diff --git a/src/Config.zig b/src/Config.zig index 1e3b3da..8d5ed1c 100644 --- a/src/Config.zig +++ b/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, diff --git a/src/YaesuController.zig b/src/YaesuController.zig index 6a3b090..8b841a7 100644 --- a/src/YaesuController.zig +++ b/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 { - // there are two different types of calibration: - // 1. feedback calibration, running to the extents of the rotator - // 2. sun calibration, which determines the azimuth and elevation angle - // offset between the rotator's physical stops and geodetic north - // - // The former is (fairly) trivial to automate, just run until stall - // (assuming there's no deadband in the feedback). The latter requires - // manual input as the human is the feedback hardware in the loop. - _ = self; +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 { @@ -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 { - self.* = .{ + 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, - .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 { - const inputs = .{ config.controller.azimuth_input, config.controller.elevation_input }; + fn updateFeedback(self: *Controller) !void { + 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 { - // 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; - + fn drive(self: *const Controller, pos_error: AzEl) !void { 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 angles = lerpAndOffsetAngles(raw); + 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}", .{ - angles.azimuth, - raw[0].voltage, + self.position.azimuth, + raw.azimuth, pos_error.azimuth, azsign.symbol(), - angles.elevation, - raw[1].voltage, + self.position.elevation, + 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) { - .initializing, .idle => { - const pos = self.updateAzEl() catch { - self.lock.lock(); - defer self.lock.unlock(); - - self.current_state = .stopped; - continue; - }; - + while (timer.mark()) : (timer.sleep()) { + self.updateFeedback() catch { self.lock.lock(); defer self.lock.unlock(); - self.position = pos; - self.current_state = self.requested_state; - }, - .calibration => { + self.current_state = .stopped; + continue; + }; + + { 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.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(); - break :blk .{ - .azimuth = self.target.azimuth - self.position.azimuth, - .elevation = self.target.elevation - self.position.elevation, + // 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, + }; }; - }; - const pos = self.drive(pos_error) catch { - self.lock.lock(); - defer self.lock.unlock(); + 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 {}; - break; - }, - }; + self.current_state = .stopped; + continue; + }; + }, + .stopped => { + // attempt to reset the drive outputs + try self.labjack.writeIoLines(.{false} ** 4); + break; + }, + } + } } }; diff --git a/src/labjack.zig b/src/labjack.zig index fe4c4bf..8df4de3 100644 --- a/src/labjack.zig +++ b/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) {