yaes/src/YaesuController.zig
torque 0ccc65fd10
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.
2024-07-18 19:43:14 -07:00

432 lines
13 KiB
Zig
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

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);
}
};