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.
432 lines
13 KiB
Zig
432 lines
13 KiB
Zig
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);
|
||
}
|
||
};
|