Compare commits

..

4 Commits

Author SHA1 Message Date
8d9fee7796 build: disable libusb logging by default
It is quite verbose and not very useful.
2024-07-11 22:14:20 -07:00
b12a44b88b main: hook up calibration stubs
I guess I will be finishing this later.
2024-07-11 22:01:16 -07:00
8ccd292fed config: make it possible not to leak
Using the GPA is a bit annoying sometimes. The other option would be to
just use page allocator to allocate the config and bypass the GPA.
2024-07-11 21:55:34 -07:00
2afb88ef5f controller: make controller info printout more useful
This has a lot more relevant information now. Anyway, this has been
tested on real hardware, and it appears to work pretty well. I am
considering changing the control loop so that it isn't always
operating on stale feedback (two LabJack calls per loop when actively
controlling pointing). Also the calibration routines need to be
implemented.
2024-07-11 21:55:34 -07:00
6 changed files with 118 additions and 304 deletions

View File

@@ -24,7 +24,7 @@ Unfortunately, all platforms have additional steps that must be taken (some easi
#### Windows
This compiles for, and runs on, Windows. However, in order for it to work out of the box, it has to link against the (proprietary) Windows LabJack U12 driver instead of the open source libUSB driver. This has two main limitations: the Windows driver is only available for the `x86` and `x86-64` architectures, and it is exclusively distributed as a dynamic library, meaning that `yaes.exe` must be distributed alongside `ljackuw.dll`. When building for Windows targets, the appropriate library file will be copied to the binary installation directory (`zig-out/bin` by default).
This compiles for, and runs on, Windows. However, the Labjack U12 by default gets assigned the Windows USB HID driver, which causes the USB report descriptor control transfer read to get mangled for mystery reasons that presumably made sense to some egghead at Microsoft at some point in time. Fortunately, this can be relatively easily fixed by using a tool like [zadig] to set the Labjack U12 to use the WinUSB driver instead of the HID driver. This is not an endorsement of the above outlined process, but rather an explanation for persons either foolish or desperate.
#### macOS
@@ -33,3 +33,5 @@ This works on macOS, though it has to be run with `sudo`, as access to the USB h
#### Linux
You probably need to install the included udev rules file in order for the USB device to be accessible as a user. This is buried in the source tree as `deps/labjack/exodriver/90-labjack.rules`. These should probably go in `/etc/udev/rules.d` if you are installing them manually and I have properly understood the various Linux folder conventions.
[zadig]: https://zadig.akeo.ie

View File

@@ -92,7 +92,6 @@ 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,
@@ -128,7 +127,6 @@ 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 };
@@ -152,7 +150,6 @@ pub const MinMax = struct {
const RotControlConfig = struct {
listen_address: []const u8,
listen_port: u16,
autopark: bool,
};
const LabjackConfig = struct {
@@ -182,8 +179,6 @@ const ControllerConfig = struct {
angle_offset: AzEl,
elevation_mask: f64,
feedback_window_samples: u8,
const OutPair = struct {
increase: lj.DigitalOutputChannel,
decrease: lj.DigitalOutputChannel,

View File

@@ -10,9 +10,12 @@ const log = std.log.scoped(.RotCtl);
writer: std.io.BufferedWriter(512, std.net.Stream.Writer),
running: bool,
rotator: *YaesuController,
rotator: YaesuController,
pub fn run(allocator: std.mem.Allocator) !void {
// var server = std.net.StreamServer.init(.{ .reuse_address = true });
// defer server.deinit();
const listen_addr = try std.net.Address.parseIp(
config.rotctl.listen_address,
config.rotctl.listen_port,
@@ -27,19 +30,20 @@ pub fn run(allocator: std.mem.Allocator) !void {
var interface: RotCtl = .{
.writer = undefined,
.running = true,
.rotator = try YaesuController.create(allocator),
.rotator = try YaesuController.init(allocator),
};
while (interface.running) {
while (true) {
const client = try server.accept();
defer {
log.info("disconnecting client", .{});
if (!config.rotctl.autopark)
interface.rotator.stop();
interface.rotator.stop();
client.stream.close();
}
interface.writer = .{ .unbuffered_writer = client.stream.writer() };
interface.running = true;
defer interface.running = false;
log.info("client connected from {}", .{client.address});
@@ -56,13 +60,7 @@ 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();
}
interface.rotator.control_thread.join();
}
fn write(self: *RotCtl, buf: []const u8) !void {
@@ -174,6 +172,7 @@ fn handleHamlibCommand(
if (first.len == 1 or first[0] == '\\') {
switch (first[0]) {
// NOTE: this is not technically supported by rotctld.
'q', 'Q' => try self.quit(first, &tokens),
'S' => try self.stop(first, &tokens),
'K' => try self.park(first, &tokens),
'p' => try self.getPosition(first, &tokens),
@@ -244,8 +243,8 @@ const HamlibCommand = struct {
};
const rotctl_commands = [_]HamlibCommand{
.{ .long = "quit", .callback = quit },
.{ .long = "exit", .callback = quit },
.{ .short = 'q', .callback = quit }, // quit
.{ .short = 'Q', .callback = quit }, // quit
.{ .long = "AOS", .callback = blindAck },
.{ .long = "LOS", .callback = blindAck },
.{ .short = 'P', .long = "set_pos", .callback = setPosition }, // azimuth: f64, elevation: f64

View File

@@ -8,8 +8,6 @@ const log = std.log.scoped(.yaesu_controller);
const YaesuController = @This();
pub var singleton: ?*YaesuController = null;
control_thread: std.Thread,
lock: *std.Thread.Mutex,
controller: *const Controller,
@@ -25,7 +23,7 @@ pub const CalibrationRoutine = enum {
};
pub fn calibrate(allocator: std.mem.Allocator, routine: CalibrationRoutine) !void {
const controller = try YaesuController.create(allocator);
const controller = try YaesuController.init(allocator);
defer {
controller.quit();
controller.control_thread.join();
@@ -37,29 +35,22 @@ pub fn calibrate(allocator: std.mem.Allocator, routine: CalibrationRoutine) !voi
}
}
pub fn create(allocator: std.mem.Allocator) !*YaesuController {
if (singleton) |_| {
log.err("Controller singleton already exists.", .{});
return error.AlreadyInitialized;
}
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.* = try Controller.init(allocator);
errdefer controller.deinit(allocator);
controller.init(lock);
// do this in the main thread so we can throw the error about it synchronously.
try controller.connectLabjack();
const self = try allocator.create(YaesuController);
errdefer allocator.destroy(self);
self.* = .{
return .{
.control_thread = try std.Thread.spawn(.{}, runController, .{controller}),
.lock = &controller.lock,
.lock = lock,
.controller = controller,
};
singleton = self;
return self;
}
fn inRange(request: f64, comptime dof: enum { azimuth, elevation }) bool {
@@ -100,7 +91,7 @@ pub fn setTarget(self: YaesuController, target: AzEl) error{OutOfRange}!void {
const controller = @constCast(self.controller);
controller.target = masked_target;
controller.requestState(.running);
controller.requested_state = .running;
}
pub fn currentPosition(self: YaesuController) AzEl {
@@ -110,15 +101,16 @@ pub fn currentPosition(self: YaesuController) AzEl {
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 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 quit(self: YaesuController) void {
@@ -126,7 +118,7 @@ pub fn quit(self: YaesuController) void {
defer self.lock.unlock();
const controller = @constCast(self.controller);
controller.requestState(.stopped);
controller.requested_state = .stopped;
}
pub fn stop(self: YaesuController) void {
@@ -135,7 +127,7 @@ pub fn stop(self: YaesuController) void {
const controller = @constCast(self.controller);
controller.target = controller.position;
controller.requestState(.idle);
controller.requested_state = .idle;
}
pub fn startPark(self: YaesuController) void {
@@ -163,77 +155,16 @@ 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,
@@ -242,13 +173,13 @@ const Controller = struct {
stopped,
};
fn init(allocator: std.mem.Allocator) !Controller {
return .{
fn init(self: *Controller, lock: *std.Thread.Mutex) void {
self.* = .{
.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),
@@ -256,28 +187,12 @@ 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();
self.labjack.id = info.local_id;
}
// this function is run with the lock already acquired
fn propagateState(self: *Controller) void {
if (self.current_state == .stopped) return;
self.current_state = self.requested_state;
}
// this function is run with the lock already acquired
fn requestState(self: *Controller, request: ControllerState) void {
if (self.current_state == .stopped) return;
self.requested_state = request;
}
fn lerpOne(input: f64, cal_points: Config.MinMax) f64 {
return (input - cal_points.minimum.voltage) * cal_points.slope() + cal_points.minimum.angle;
}
@@ -300,10 +215,10 @@ const Controller = struct {
zero,
positive,
pub fn symbol(self: Sign) u21 {
pub fn symbol(self: Sign) u8 {
return switch (self) {
.negative => '-',
.zero => '×',
.zero => '=',
.positive => '+',
};
}
@@ -318,23 +233,26 @@ const Controller = struct {
.positive;
}
fn updateFeedback(self: *Controller) !void {
const inputs = .{
config.controller.azimuth_input,
config.controller.elevation_input,
};
fn updateAzEl(self: *const Controller) !AzEl {
const inputs = .{ config.controller.azimuth_input, config.controller.elevation_input };
const raw = try self.labjack.readAnalogWriteDigital(
2,
inputs,
null,
.{false} ** 4,
true,
);
self.feedback_buffer.push(raw);
return lerpAndOffsetAngles(raw);
}
fn drive(self: *const Controller, pos_error: AzEl) !void {
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;
const azsign = signDeadzone(
pos_error.azimuth,
config.controller.angle_tolerance.azimuth,
@@ -345,111 +263,107 @@ 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 = self.feedback_buffer.getRaw();
const raw = try self.labjack.readAnalogWriteDigital(2, inputs, drive_signal, true);
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}",
"az: {d:.1}° ({d:.2} V) {d:.1}° => {c}, el: {d:.1}° ({d:.2} V) {d:.1}° => {c}",
.{
self.position.azimuth,
raw.azimuth,
angles.azimuth,
raw[0].voltage,
pos_error.azimuth,
azsign.symbol(),
self.position.elevation,
raw.elevation,
angles.elevation,
raw[1].voltage,
pos_error.elevation,
elsign.symbol(),
},
);
try self.labjack.writeIoLines(drive_signal);
}
fn setPosition(self: *Controller, position: AzEl) void {
self.position = position;
self.condition.broadcast();
return angles;
}
fn run(self: *Controller) !void {
self.current_state = .initializing;
var timer = LoopTimer.init(config.controller.loop_interval_ns);
var timer: LoopTimer = .{ .interval_ns = config.controller.loop_interval_ns };
while (timer.mark()) : (timer.sleep()) {
const fbfail = if (self.updateFeedback()) |_| false else |_| true;
{
self.lock.lock();
defer self.lock.unlock();
self.setPosition(self.feedback_buffer.get());
if (fbfail) self.requestState(.stopped);
self.propagateState();
}
switch (self.current_state) {
.initializing, .idle => {},
.calibration => {
while (timer.mark()) : (timer.sleep()) switch (self.current_state) {
.initializing, .idle => {
const pos = self.updateAzEl() catch {
self.lock.lock();
defer self.lock.unlock();
// run calibration routine. psych, this does nothing. gottem
self.current_state = .idle;
self.requestState(.idle);
},
.running => {
const pos_error: AzEl = blk: {
self.lock.lock();
defer self.lock.unlock();
self.current_state = .stopped;
continue;
};
break :blk .{
.azimuth = self.target.azimuth - self.position.azimuth,
.elevation = self.target.elevation - self.position.elevation,
};
self.lock.lock();
defer self.lock.unlock();
self.position = pos;
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();
const pos = 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;
},
}
}
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;
},
};
}
};
pub const LoopTimer = struct {
interval_ns: u64,
timer: std.time.Timer,
pub fn init(interval_ns: u64) LoopTimer {
return .{
.interval_ns = interval_ns,
.timer = std.time.Timer.start() catch @panic("Could not create timer"),
};
}
start: i128 = 0,
pub fn mark(self: *LoopTimer) bool {
self.timer.reset();
self.start = std.time.nanoTimestamp();
return true;
}
pub fn sleep(self: *LoopTimer) void {
const elapsed = self.timer.read();
std.time.sleep(self.interval_ns -| elapsed);
const now = std.time.nanoTimestamp();
const elapsed: u64 = @intCast(now - self.start);
std.time.sleep(self.interval_ns - elapsed);
}
};

View File

@@ -68,30 +68,6 @@ 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) {

View File

@@ -1,5 +1,4 @@
const std = @import("std");
const builtin = @import("builtin");
const Config = @import("./Config.zig");
const lj = @import("./labjack.zig");
@@ -10,72 +9,11 @@ const udev = @import("udev_rules");
const log = std.log.scoped(.main);
fn quit() noreturn {
if (YaesuController.singleton) |controller| {
controller.quit();
controller.control_thread.join();
}
std.process.exit(1);
}
const moreposix = struct {
pub extern "c" fn sigaddset(set: *std.posix.sigset_t, signo: c_int) c_int;
pub extern "c" fn sigdelset(set: *std.posix.sigset_t, signo: c_int) c_int;
pub extern "c" fn sigemptyset(set: *std.posix.sigset_t) c_int;
pub extern "c" fn sigfillset(set: *std.posix.sigset_t) c_int;
pub extern "c" fn sigismember(set: *const std.posix.sigset_t, signo: c_int) c_int;
// stdlib prototype is wrong, it doesn't take optional pointers.
pub extern "c" fn pthread_sigmask(how: c_int, noalias set: ?*const std.posix.sigset_t, noalias oldset: ?*std.posix.sigset_t) c_int;
};
const psigs = [_]c_int{ std.posix.SIG.INT, std.posix.SIG.HUP, std.posix.SIG.QUIT };
fn posixSignalHandlerThread() void {
var set: std.posix.sigset_t = undefined;
_ = moreposix.sigemptyset(&set);
for (psigs) |sig|
_ = moreposix.sigaddset(&set, sig);
var sig: c_int = 0;
_ = std.posix.system.sigwait(&set, &sig);
log.info("Got exit signal", .{});
quit();
}
// Windows runs this handler in a thread, so calling quit directly should be safe.
fn windowsEventHandler(code: std.os.windows.DWORD) callconv(std.os.windows.WINAPI) std.os.windows.BOOL {
_ = code;
log.info("Got exit signal", .{});
quit();
}
fn addExitHandler() !void {
if (comptime builtin.os.tag == .windows) {
try std.os.windows.SetConsoleCtrlHandler(windowsEventHandler, true);
} else if (comptime std.Thread.use_pthreads) {
var set: std.posix.sigset_t = undefined;
_ = moreposix.sigemptyset(&set);
for (psigs) |sig|
_ = moreposix.sigaddset(&set, sig);
_ = moreposix.pthread_sigmask(std.posix.SIG.BLOCK, &set, null);
// nobody cares about the thread
_ = try std.Thread.spawn(.{}, posixSignalHandlerThread, .{});
} else {
log.err("not windows and not pthreads = disaster", .{});
}
}
fn printStderr(comptime fmt: []const u8, args: anytype) void {
std.debug.print(fmt ++ "\n", args);
}
pub fn main() !u8 {
if (comptime builtin.os.tag == .windows) {
// set output to UTF-8 on Windows
_ = std.os.windows.kernel32.SetConsoleOutputCP(65001);
}
var gpa = std.heap.GeneralPurposeAllocator(.{}){};
defer _ = gpa.deinit();
const allocator = gpa.allocator();
@@ -122,14 +60,9 @@ pub fn main() !u8 {
defer Config.deinit();
addExitHandler() catch {
log.err("Could not install quit handler.", .{});
return 1;
};
RotCtl.run(allocator) catch |err| {
log.err("rotator controller ceased unexpectedly! {s}", .{@errorName(err)});
quit();
return 1;
};
} else if (std.mem.eql(u8, args[1], commands.calibrate)) {
if (args.len < 3 or args.len > 4) {
@@ -147,14 +80,9 @@ pub fn main() !u8 {
return 1;
};
addExitHandler() catch {
log.err("Could not install quit handler.", .{});
return 1;
};
YaesuController.calibrate(allocator, routine) catch |err| {
log.err("Calibration failed: {s}", .{@errorName(err)});
quit();
return 1;
};
} else if (std.mem.eql(u8, args[1], commands.help)) {
if (args.len != 3) {