Compare commits
3 Commits
49fce9c584
...
e639a17424
Author | SHA1 | Date | |
---|---|---|---|
e639a17424 | |||
10c40d7d50 | |||
c32390f7c1 |
4
deps/labjack/exodriver/build.zig
vendored
4
deps/labjack/exodriver/build.zig
vendored
@ -17,6 +17,10 @@ pub fn build(b: *std.Build) !void {
|
||||
.link_libc = true,
|
||||
});
|
||||
|
||||
if (optimize == .Debug) {
|
||||
liblabjackusb.defineCMacro("LJ_DEBUG", "1");
|
||||
}
|
||||
|
||||
liblabjackusb.addCSourceFile(.{ .file = b.path("liblabjackusb/labjackusb.c") });
|
||||
liblabjackusb.installHeader(b.path("liblabjackusb/labjackusb.h"), "labjackusb.h");
|
||||
|
||||
|
@ -505,6 +505,7 @@ static HANDLE LJUSB_OpenSpecificDevice(libusb_device *dev, const struct libusb_d
|
||||
return NULL;
|
||||
}
|
||||
|
||||
#if defined(_WIN32)
|
||||
// Test if the kernel driver has the U12.
|
||||
if (desc->idProduct == U12_PRODUCT_ID && libusb_kernel_driver_active(devh, 0)) {
|
||||
#if LJ_DEBUG
|
||||
@ -521,6 +522,7 @@ static HANDLE LJUSB_OpenSpecificDevice(libusb_device *dev, const struct libusb_d
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
#endif // _WIN32
|
||||
|
||||
r = libusb_claim_interface(devh, 0);
|
||||
if (r < 0) {
|
||||
|
8
deps/labjack/ljacklm/build.zig
vendored
8
deps/labjack/ljacklm/build.zig
vendored
@ -17,6 +17,14 @@ pub fn build(b: *std.Build) !void {
|
||||
.link_libc = true,
|
||||
});
|
||||
|
||||
if (optimize == .Debug) {
|
||||
libljacklm.defineCMacro("LJ_DEBUG", "1");
|
||||
}
|
||||
|
||||
if (target.result.os.tag == .windows) {
|
||||
libljacklm.defineCMacro("LJACKLM_USE_WINDOWS_MUTEX_SHIM", "1");
|
||||
}
|
||||
|
||||
libljacklm.addCSourceFile(.{ .file = b.path("libljacklm/ljacklm.c") });
|
||||
libljacklm.installHeader(b.path("libljacklm/ljacklm.h"), "ljacklm.h");
|
||||
|
||||
|
24
deps/labjack/ljacklm/libljacklm/ljacklm.c
vendored
24
deps/labjack/ljacklm/libljacklm/ljacklm.c
vendored
@ -23,7 +23,14 @@
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <unistd.h>
|
||||
#include <pthread.h>
|
||||
|
||||
#if defined(LJACKLM_USE_WINDOWS_MUTEX_SHIM)
|
||||
#include <windows.h>
|
||||
#include "windows_mutex_shim.h"
|
||||
#else
|
||||
#include <pthread.h>
|
||||
#endif // LJACKLM_USE_WINDOWS_MUTEX_SHIM
|
||||
|
||||
#include "labjackusb.h"
|
||||
|
||||
|
||||
@ -286,7 +293,9 @@ long GetU12Information( HANDLE hDevice,
|
||||
long *fcddMaxSize,
|
||||
long *hvcMaxSize);
|
||||
|
||||
#if !defined(LJACKLM_USE_WINDOWS_MUTEX_SHIM)
|
||||
unsigned long GetTickCount( void);
|
||||
#endif
|
||||
|
||||
|
||||
__attribute__((constructor))
|
||||
@ -7211,6 +7220,17 @@ long GetU12Information( HANDLE hDevice,
|
||||
temp = (unsigned long)LJUSB_GetDeviceDescriptorReleaseNumber(hDevice) * 65536; //upper two bytes of serial #
|
||||
|
||||
result = LJUSB_GetHIDReportDescriptor(hDevice, repDesc, 75);
|
||||
|
||||
#if defined(LJ_DEBUG)
|
||||
|
||||
fprintf(stderr, "U12 HID ReportDescriptor (hex, %lu B): ", result);
|
||||
for (int idx = 0; idx < result; idx++) {
|
||||
fprintf(stderr, "%02hhX", repDesc[idx]);
|
||||
}
|
||||
fprintf(stderr, "\n");
|
||||
|
||||
#endif // LJ_DEBUG
|
||||
|
||||
if(result < 75)
|
||||
{
|
||||
//Failed getting descriptor. First capability would of been input, so
|
||||
@ -7266,6 +7286,7 @@ long GetU12Information( HANDLE hDevice,
|
||||
}
|
||||
|
||||
|
||||
#if !defined(LJACKLM_USE_WINDOWS_MUTEX_SHIM)
|
||||
//======================================================================
|
||||
//GetTickCount: Implementation of GetTickCount() for Unix. Returns the
|
||||
// current time, expressed as millisconds since the Epoch
|
||||
@ -7276,3 +7297,4 @@ unsigned long GetTickCount( void)
|
||||
gettimeofday(&tv, NULL);
|
||||
return (tv.tv_sec * 1000) + (tv.tv_usec / 1000);
|
||||
}
|
||||
#endif
|
||||
|
28
deps/labjack/ljacklm/libljacklm/windows_mutex_shim.h
vendored
Normal file
28
deps/labjack/ljacklm/libljacklm/windows_mutex_shim.h
vendored
Normal file
@ -0,0 +1,28 @@
|
||||
#ifndef LJACKLM_WIN_MUTEX
|
||||
#define LJACKLM_WIN_MUTEX
|
||||
|
||||
typedef CRITICAL_SECTION pthread_mutex_t;
|
||||
|
||||
static inline int pthread_mutex_init(pthread_mutex_t *mutex, void *attr) {
|
||||
InitializeCriticalSection(mutex);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static inline void pthread_mutex_lock(pthread_mutex_t *mutex) {
|
||||
EnterCriticalSection(mutex);
|
||||
}
|
||||
|
||||
static inline int pthread_mutex_unlock(pthread_mutex_t *mutex) {
|
||||
LeaveCriticalSection(mutex);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static inline int pthread_mutex_trylock(pthread_mutex_t *mutex) {
|
||||
return TryEnterCriticalSection(mutex) == 0;
|
||||
}
|
||||
|
||||
static inline void pthread_mutex_destroy(pthread_mutex_t *mutex) {
|
||||
DeleteCriticalSection(mutex);
|
||||
}
|
||||
|
||||
#endif // LJACKLM_WIN_MUTEX
|
112
src/Config.zig
Normal file
112
src/Config.zig
Normal file
@ -0,0 +1,112 @@
|
||||
const std = @import("std");
|
||||
|
||||
const AzEl = @import("./LabjackYaesu.zig").AzEl;
|
||||
const lj = @import("./labjack.zig");
|
||||
|
||||
const Config = @This();
|
||||
|
||||
var global_internal: Config = undefined;
|
||||
pub const global: *const Config = &global_internal;
|
||||
|
||||
pub fn load(allocator: std.mem.Allocator, reader: anytype) !void {
|
||||
var jread = std.json.Reader(1024, @TypeOf(reader)).init(allocator, reader);
|
||||
defer jread.deinit();
|
||||
|
||||
global_internal = try std.json.parseFromTokenSourceLeaky(
|
||||
Config,
|
||||
allocator,
|
||||
&jread,
|
||||
.{},
|
||||
);
|
||||
}
|
||||
|
||||
pub fn loadDefault(allocator: std.mem.Allocator) void {
|
||||
_ = allocator;
|
||||
global_internal = .{};
|
||||
}
|
||||
|
||||
pub fn destroy(allocator: std.mem.Allocator) void {
|
||||
// TODO: implement this probably
|
||||
_ = allocator;
|
||||
}
|
||||
|
||||
rotctl: RotControlConfig = .{
|
||||
.listen_address = "127.0.0.1",
|
||||
.listen_port = 5432,
|
||||
},
|
||||
labjack: LabjackConfig = .{
|
||||
.device = .autodetect,
|
||||
.feedback_calibration = .{
|
||||
.azimuth = .{
|
||||
.minimum = .{ .voltage = 0.0, .angle = 0.0 },
|
||||
.maximum = .{ .voltage = 5.0, .angle = 450.0 },
|
||||
},
|
||||
.elevation = .{
|
||||
.minimum = .{ .voltage = 0.0, .angle = 0.0 },
|
||||
.maximum = .{ .voltage = 5.0, .angle = 180.0 },
|
||||
},
|
||||
},
|
||||
},
|
||||
controller: ControllerConfig = .{
|
||||
.azimuth_input = .{ .channel = .diff_01, .gain_index = 2 },
|
||||
.elevation_input = .{ .channel = .diff_23, .gain_index = 2 },
|
||||
.azimuth_outputs = .{ .increase = .{ .io = 0 }, .decrease = .{ .io = 1 } },
|
||||
.elevation_outputs = .{ .increase = .{ .io = 2 }, .decrease = .{ .io = 3 } },
|
||||
.loop_interval_ns = 50_000_000,
|
||||
.parking_posture = .{ .azimuth = 180, .elevation = 90 },
|
||||
.angle_tolerance = .{ .azimuth = 1, .elevation = 1 },
|
||||
},
|
||||
|
||||
pub const VoltAngle = struct { voltage: f64, angle: f64 };
|
||||
pub const MinMax = struct {
|
||||
minimum: VoltAngle,
|
||||
maximum: VoltAngle,
|
||||
|
||||
pub inline fn slope(self: MinMax) f64 {
|
||||
return self.angleDiff() / self.voltDiff();
|
||||
}
|
||||
|
||||
pub inline fn voltDiff(self: MinMax) f64 {
|
||||
return self.maximum.voltage - self.minimum.voltage;
|
||||
}
|
||||
|
||||
pub inline fn angleDiff(self: MinMax) f64 {
|
||||
return self.maximum.angle - self.minimum.angle;
|
||||
}
|
||||
};
|
||||
|
||||
const RotControlConfig = struct {
|
||||
listen_address: []const u8,
|
||||
listen_port: u16,
|
||||
};
|
||||
|
||||
const LabjackConfig = struct {
|
||||
device: union(enum) {
|
||||
autodetect,
|
||||
serial_number: i32,
|
||||
},
|
||||
// Very basic two-point calibration for each degree of freedom. All other angles are
|
||||
// linearly interpolated from these two points. This assumes the feedback is linear,
|
||||
// which seems to be a mostly reasonable assumption in practice.
|
||||
feedback_calibration: struct {
|
||||
azimuth: MinMax,
|
||||
elevation: MinMax,
|
||||
},
|
||||
};
|
||||
|
||||
const ControllerConfig = struct {
|
||||
azimuth_input: lj.AnalogInput,
|
||||
elevation_input: lj.AnalogInput,
|
||||
|
||||
azimuth_outputs: OutPair,
|
||||
elevation_outputs: OutPair,
|
||||
|
||||
loop_interval_ns: u64,
|
||||
parking_posture: AzEl,
|
||||
angle_tolerance: AzEl,
|
||||
|
||||
const OutPair = struct {
|
||||
increase: lj.DigitalOutputChannel,
|
||||
decrease: lj.DigitalOutputChannel,
|
||||
};
|
||||
};
|
260
src/LabjackYaesu.zig
Normal file
260
src/LabjackYaesu.zig
Normal file
@ -0,0 +1,260 @@
|
||||
const std = @import("std");
|
||||
|
||||
const lj = @import("./labjack.zig");
|
||||
const Config = @import("./Config.zig");
|
||||
const config = Config.global;
|
||||
|
||||
const log = std.log.scoped(.labjack_yaesu);
|
||||
|
||||
const LabjackYaesu = @This();
|
||||
|
||||
control_thread: std.Thread,
|
||||
lock: *std.Thread.Mutex,
|
||||
controller: *const Controller,
|
||||
|
||||
pub const AzEl = struct {
|
||||
azimuth: f64,
|
||||
elevation: f64,
|
||||
};
|
||||
|
||||
pub fn init(allocator: std.mem.Allocator) !LabjackYaesu {
|
||||
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);
|
||||
// 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,
|
||||
.controller = controller,
|
||||
};
|
||||
}
|
||||
|
||||
pub fn setTarget(self: LabjackYaesu, target: AzEl) void {
|
||||
self.lock.lock();
|
||||
defer self.lock.unlock();
|
||||
|
||||
const controller = @constCast(self.controller);
|
||||
controller.target = target;
|
||||
controller.requested_state = .running;
|
||||
}
|
||||
|
||||
pub fn startCalibration(self: LabjackYaesu) 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 idle(self: LabjackYaesu) void {
|
||||
self.lock.lock();
|
||||
defer self.lock.unlock();
|
||||
|
||||
const controller = @constCast(self.controller);
|
||||
controller.requested_state = .idle;
|
||||
}
|
||||
|
||||
pub fn stop(self: LabjackYaesu) void {
|
||||
self.lock.lock();
|
||||
defer self.lock.unlock();
|
||||
|
||||
const controller = @constCast(self.controller);
|
||||
controller.requested_state = .stopped;
|
||||
}
|
||||
|
||||
fn runController(controller: *Controller) void {
|
||||
controller.run() catch {
|
||||
log.err(
|
||||
"the labjack control loop has terminated unexpectedly!!!!",
|
||||
.{},
|
||||
);
|
||||
};
|
||||
}
|
||||
|
||||
const Controller = struct {
|
||||
target: AzEl,
|
||||
position: AzEl,
|
||||
|
||||
current_state: ControllerState,
|
||||
requested_state: ControllerState,
|
||||
|
||||
lock: *std.Thread.Mutex,
|
||||
labjack: lj.Labjack,
|
||||
|
||||
const ControllerState = enum {
|
||||
initializing,
|
||||
idle,
|
||||
calibration,
|
||||
running,
|
||||
stopped,
|
||||
};
|
||||
|
||||
fn init(self: *Controller, lock: *std.Thread.Mutex) void {
|
||||
self.* = .{
|
||||
.target = .{ .azimuth = 0, .elevation = 0 },
|
||||
.position = .{ .azimuth = 0, .elevation = 0 },
|
||||
.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),
|
||||
},
|
||||
};
|
||||
}
|
||||
|
||||
fn connectLabjack(self: *Controller) !void {
|
||||
const info = try self.labjack.connect();
|
||||
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 lerpAngles(input: [2]lj.AnalogReadResult) AzEl {
|
||||
return .{
|
||||
.azimuth = lerpOne(input[0].voltage, config.labjack.feedback_calibration.azimuth),
|
||||
.elevation = lerpOne(input[1].voltage, config.labjack.feedback_calibration.elevation),
|
||||
};
|
||||
}
|
||||
|
||||
fn signDeadzone(offset: f64, deadzone: f64) enum { negative, zero, positive } {
|
||||
return if (@abs(offset) < deadzone)
|
||||
.zero
|
||||
else if (offset < 0)
|
||||
.negative
|
||||
else
|
||||
.positive;
|
||||
}
|
||||
|
||||
fn updateAzEl(self: *const Controller) !AzEl {
|
||||
const inputs = .{ config.controller.azimuth_input, config.controller.elevation_input };
|
||||
|
||||
const raw = try self.labjack.readAnalogWriteDigital(
|
||||
2,
|
||||
inputs,
|
||||
.{false} ** 4,
|
||||
true,
|
||||
);
|
||||
|
||||
return lerpAngles(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;
|
||||
|
||||
const azsign = signDeadzone(
|
||||
pos_error.azimuth,
|
||||
config.controller.angle_tolerance.azimuth,
|
||||
);
|
||||
|
||||
const elsign = signDeadzone(
|
||||
pos_error.elevation,
|
||||
config.controller.angle_tolerance.elevation,
|
||||
);
|
||||
|
||||
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);
|
||||
|
||||
return lerpAngles(raw);
|
||||
}
|
||||
|
||||
fn run(self: *Controller) !void {
|
||||
self.current_state = .initializing;
|
||||
|
||||
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;
|
||||
};
|
||||
|
||||
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,
|
||||
};
|
||||
};
|
||||
|
||||
const pos = 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;
|
||||
},
|
||||
};
|
||||
}
|
||||
};
|
||||
|
||||
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);
|
||||
}
|
||||
};
|
210
src/RotCtl.zig
Normal file
210
src/RotCtl.zig
Normal file
@ -0,0 +1,210 @@
|
||||
const std = @import("std");
|
||||
|
||||
const config = @import("./Config.zig").global;
|
||||
const LabjackYaesu = @import("./LabjackYaesu.zig");
|
||||
|
||||
const RotCtl = @This();
|
||||
|
||||
const log = std.log.scoped(.RotCtl);
|
||||
|
||||
writer: std.io.BufferedWriter(512, std.net.Stream.Writer),
|
||||
running: bool,
|
||||
rotator: LabjackYaesu,
|
||||
|
||||
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,
|
||||
);
|
||||
|
||||
var server = listen_addr.listen(.{ .reuse_address = true }) catch {
|
||||
log.err("Could not listen on {}. Is it already in use?", .{listen_addr});
|
||||
return;
|
||||
};
|
||||
log.info("Listening for client on: {}", .{listen_addr});
|
||||
|
||||
var interface: RotCtl = .{
|
||||
.writer = undefined,
|
||||
.running = true,
|
||||
.rotator = try LabjackYaesu.init(allocator),
|
||||
};
|
||||
|
||||
while (true) {
|
||||
const client = try server.accept();
|
||||
defer {
|
||||
log.info("disconnecting client", .{});
|
||||
client.stream.close();
|
||||
}
|
||||
|
||||
interface.writer = .{ .unbuffered_writer = client.stream.writer() };
|
||||
interface.running = true;
|
||||
defer interface.running = false;
|
||||
|
||||
log.info("client connected from {}", .{client.address});
|
||||
|
||||
var readbuffer = [_]u8{0} ** 512;
|
||||
var fbs = std.io.fixedBufferStream(&readbuffer);
|
||||
const reader = client.stream.reader();
|
||||
|
||||
while (interface.running) : (fbs.reset()) {
|
||||
reader.streamUntilDelimiter(fbs.writer(), '\n', readbuffer.len) catch break;
|
||||
try interface.handleHamlibCommand(fbs.getWritten());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn write(self: *RotCtl, buf: []const u8) !void {
|
||||
try self.writer.writer().writeAll(buf);
|
||||
try self.writer.flush();
|
||||
}
|
||||
|
||||
fn replyStatus(self: *RotCtl, comptime status: HamlibErrorCode) !void {
|
||||
try self.write(comptime status.replyFrame() ++ "\n");
|
||||
}
|
||||
|
||||
fn handleHamlibCommand(
|
||||
self: *RotCtl,
|
||||
command: []const u8,
|
||||
) !void {
|
||||
var tokens = std.mem.tokenizeScalar(u8, command, ' ');
|
||||
|
||||
const first = tokens.next().?;
|
||||
if (first.len == 1 or first[0] == '\\') {
|
||||
switch (first[0]) {
|
||||
'q' => {
|
||||
self.running = false;
|
||||
self.replyStatus(.okay) catch return;
|
||||
},
|
||||
'\\' => {
|
||||
return try self.parseLongCommand(first[1..], &tokens);
|
||||
},
|
||||
else => |cmd| {
|
||||
log.err("unknown command {}", .{cmd});
|
||||
try self.replyStatus(.not_implemented);
|
||||
},
|
||||
}
|
||||
} else if (std.mem.eql(u8, first, "AOS")) {
|
||||
// gpredict just kind of shoves this message in on top of the HamLib
|
||||
// protocol.
|
||||
try self.replyStatus(.okay);
|
||||
} else if (std.mem.eql(u8, first, "LOS")) {
|
||||
try self.replyStatus(.okay);
|
||||
} else try self.replyStatus(.not_supported);
|
||||
}
|
||||
|
||||
fn parseLongCommand(
|
||||
self: *RotCtl,
|
||||
command: []const u8,
|
||||
tokens: *std.mem.TokenIterator(u8, .scalar),
|
||||
) !void {
|
||||
_ = tokens;
|
||||
|
||||
for (rotctl_commands) |check| {
|
||||
if (check.long) |long| {
|
||||
if (command.len >= long.len and std.mem.eql(u8, long, command)) {
|
||||
log.warn("Unsupported command {s}", .{command});
|
||||
break;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
log.warn("Unknown command {s}", .{command});
|
||||
}
|
||||
return self.replyStatus(.not_supported);
|
||||
}
|
||||
|
||||
const HamlibErrorCode = enum(u8) {
|
||||
okay = 0,
|
||||
invalid_parameter = 1,
|
||||
invalid_configuration = 2,
|
||||
out_of_memory = 3,
|
||||
not_implemented = 4,
|
||||
timeout = 5,
|
||||
io_error = 6,
|
||||
internal_error = 7,
|
||||
protocol_error = 8,
|
||||
command_rejected = 9,
|
||||
parameter_truncated = 10,
|
||||
not_supported = 11,
|
||||
not_targetable = 12,
|
||||
bus_error = 13,
|
||||
bus_busy = 14,
|
||||
invalid_arg = 15,
|
||||
invalid_vfo = 16,
|
||||
domain_error = 17,
|
||||
deprecated = 18,
|
||||
security = 19,
|
||||
power = 20,
|
||||
|
||||
fn replyFrame(comptime self: HamlibErrorCode) []const u8 {
|
||||
return std.fmt.comptimePrint(
|
||||
"RPRT {d}",
|
||||
.{-@as(i8, @intCast(@intFromEnum(self)))},
|
||||
);
|
||||
}
|
||||
};
|
||||
|
||||
const HamlibCommand = struct {
|
||||
short: ?u8 = null,
|
||||
long: ?[]const u8 = null,
|
||||
};
|
||||
|
||||
const rotctl_commands = [_]HamlibCommand{
|
||||
.{ .short = 'q' }, // quit
|
||||
.{ .short = 'Q' }, // quit
|
||||
.{ .short = 'P', .long = "set_pos" }, // azimuth: f64, elevation: f64
|
||||
.{ .short = 'p', .long = "get_pos" }, // return az: f64, el: f64
|
||||
.{ .short = 'M', .long = "move" }, // direction: enum { up=2, down=4, left=8, right=16 }, speed: i8 (0-100 or -1)
|
||||
.{ .short = 'S', .long = "stop" },
|
||||
.{ .short = 'K', .long = "park" },
|
||||
.{ .short = 'C', .long = "set_conf" }, // token: []const u8, value: []const u8
|
||||
.{ .short = 'R', .long = "reset" }, // u1 (1 is reset all)
|
||||
.{ .short = '_', .long = "get_info" }, // return Model name
|
||||
.{ .short = 'K', .long = "park" },
|
||||
.{ .long = "dump_state" }, // ???
|
||||
.{ .short = '1', .long = "dump_caps" }, // ???
|
||||
.{ .short = 'w', .long = "send_cmd" }, // []const u8, send serial command directly to the rotator
|
||||
.{ .short = 'L', .long = "lonlat2loc" }, // return Maidenhead locator for given long: f64 and lat: f64, locator precision: u4 (2-12)
|
||||
.{ .short = 'l', .long = "loc2lonlat" }, // the inverse of the above
|
||||
.{ .short = 'D', .long = "dms2dec" }, // deg, min, sec, 0 (positive) or 1 (negative)
|
||||
.{ .short = 'd', .long = "dec2dms" },
|
||||
.{ .short = 'E', .long = "dmmm2dec" },
|
||||
.{ .short = 'e', .long = "dec2dmmm" },
|
||||
.{ .short = 'B', .long = "grb" },
|
||||
.{ .short = 'A', .long = "a_sp2a_lp" },
|
||||
.{ .short = 'a', .long = "d_sp2d_lp" },
|
||||
.{ .long = "pause" },
|
||||
};
|
||||
|
||||
// D, dms2dec 'Degrees' 'Minutes' 'Seconds' 'S/W'
|
||||
// Returns 'Dec Degrees', a signed floating point value.
|
||||
// 'Degrees' and 'Minutes' are integer values.
|
||||
// 'Seconds' is a floating point value.
|
||||
// 'S/W' is a flag with ’1’ indicating South latitude or West longitude and ’0’ North or East (the flag is needed as computers don’t recognize a signed zero even though only the 'Degrees' value is typically signed in DMS notation).
|
||||
// d, dec2dms 'Dec Degrees'
|
||||
// Returns 'Degrees' 'Minutes' 'Seconds' 'S/W'.
|
||||
// Values are as in dms2dec above.
|
||||
// E, dmmm2dec 'Degrees' 'Dec Minutes' 'S/W'
|
||||
// Returns 'Dec Degrees', a signed floating point value.
|
||||
// 'Degrees' is an integer value.
|
||||
// 'Dec Minutes' is a floating point value.
|
||||
// 'S/W' is a flag as in dms2dec above.
|
||||
// e, dec2dmmm 'Dec Deg'
|
||||
// Returns 'Degrees' 'Minutes' 'S/W'.
|
||||
// Values are as in dmmm2dec above.
|
||||
// B, qrb 'Lon 1' 'Lat 1' 'Lon 2' 'Lat 2'
|
||||
// Returns 'Distance' and 'Azimuth'.
|
||||
// 'Distance' is in km.
|
||||
// 'Azimuth' is in degrees.
|
||||
// Supplied Lon/Lat values are signed floating point numbers.
|
||||
// A, a_sp2a_lp 'Short Path Deg'
|
||||
// Returns 'Long Path Deg'.
|
||||
// Both the supplied argument and returned value are floating point values within the range of 0.00 to 360.00.
|
||||
// Note: Supplying a negative value will return an error message.
|
||||
// a, d_sp2d_lp 'Short Path km'
|
||||
// Returns 'Long Path km'.
|
||||
// Both the supplied argument and returned value are floating point values.
|
||||
// pause 'Seconds'
|
||||
// Pause for the given whole (integer) number of 'Seconds' before sending the next command to the rotator.
|
@ -5,13 +5,27 @@ pub fn getDriverVersion() f32 {
|
||||
}
|
||||
|
||||
pub const Labjack = struct {
|
||||
id: ?u32 = null,
|
||||
id: ?i32 = null,
|
||||
demo: bool = false,
|
||||
|
||||
pub fn autodetect() Labjack {
|
||||
return .{};
|
||||
}
|
||||
|
||||
pub fn with_serial_number(sn: i32) Labjack {
|
||||
return .{ .id = sn };
|
||||
}
|
||||
|
||||
pub fn connect(self: Labjack) LabjackError!struct { local_id: i32, firmware_version: f32 } {
|
||||
var id = self.cId();
|
||||
const version = c_api.GetFirmwareVersion(&id);
|
||||
|
||||
return if (version == 0)
|
||||
@as(c_api.LabjackCError, @bitCast(id)).toError()
|
||||
else
|
||||
.{ .local_id = @intCast(id), .firmware_version = version };
|
||||
}
|
||||
|
||||
pub fn firmwareVersion(self: Labjack) LabjackError!f32 {
|
||||
var id = self.cId();
|
||||
const version = c_api.GetFirmwareVersion(&id);
|
||||
@ -24,7 +38,7 @@ pub const Labjack = struct {
|
||||
|
||||
/// 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 != 0) {
|
||||
if (!input.channel.isDifferential() and input.gain_index != 0) {
|
||||
return error.InvalidGain;
|
||||
}
|
||||
|
||||
@ -36,7 +50,7 @@ pub const Labjack = struct {
|
||||
&id,
|
||||
@intFromBool(self.demo),
|
||||
input.channelNumber(),
|
||||
input.gain,
|
||||
input.gain_index,
|
||||
&over_v,
|
||||
&res.voltage,
|
||||
);
|
||||
@ -79,7 +93,7 @@ pub const Labjack = struct {
|
||||
var gains: [incount]c_long = undefined;
|
||||
for (inputs, &in_channels, &gains) |from, *inc, *gain| {
|
||||
inc.* = from.channelNumber();
|
||||
gain.* = from.gain;
|
||||
gain.* = from.gain_index;
|
||||
}
|
||||
var v_out: [4]f32 = .{0} ** 4;
|
||||
var over_v: c_long = 0;
|
||||
@ -119,7 +133,7 @@ pub const Labjack = struct {
|
||||
|
||||
pub const AnalogInput = struct {
|
||||
channel: AnalogInputChannel,
|
||||
gain: Gain = 0,
|
||||
gain_index: GainIndex = 0,
|
||||
|
||||
pub fn channelNumber(self: AnalogInput) u4 {
|
||||
return @intFromEnum(self.channel);
|
||||
@ -191,7 +205,7 @@ pub const DigitalOutputChannel = union(enum) {
|
||||
// 5 => G=10 ±2 volts
|
||||
// 6 => G=16 ±1.25 volts
|
||||
// 7 => G=20 ±1 volt
|
||||
pub const Gain = u3;
|
||||
pub const GainIndex = u3;
|
||||
|
||||
pub const PackedOutput = packed struct(u4) {
|
||||
io0: bool,
|
||||
@ -199,6 +213,18 @@ pub const PackedOutput = packed struct(u4) {
|
||||
io2: bool,
|
||||
io3: bool,
|
||||
|
||||
pub fn setOut(self: *PackedOutput, output: DigitalOutput) !void {
|
||||
switch (output) {
|
||||
.io => |num| switch (num) {
|
||||
0 => self.io0 = output.level,
|
||||
1 => self.io1 = output.level,
|
||||
2 => self.io2 = output.level,
|
||||
3 => self.io3 = output.level,
|
||||
},
|
||||
.d => return error.Invalid,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn fromBoolArray(states: [4]bool) PackedOutput {
|
||||
return .{
|
||||
.io0 = states[0],
|
||||
@ -214,6 +240,30 @@ pub const PackedOutput = packed struct(u4) {
|
||||
};
|
||||
|
||||
pub const c_api = struct {
|
||||
pub const vendor_id: u16 = 0x0CD5;
|
||||
pub const u12_product_id: u16 = 0x0001;
|
||||
|
||||
pub extern fn OpenLabJack(
|
||||
errorcode: *LabjackCError,
|
||||
vendor_id: c_uint,
|
||||
product_id: c_uint,
|
||||
idnum: *c_long,
|
||||
serialnum: *c_long,
|
||||
caldata: *[20]c_long,
|
||||
) c_long;
|
||||
|
||||
pub extern fn CloseAll(local_id: c_long) c_long;
|
||||
|
||||
pub extern fn GetU12Information(
|
||||
handle: *anyopaque,
|
||||
serialnum: *c_long,
|
||||
local_id: *c_long,
|
||||
power: *c_long,
|
||||
cal_data: *[20]c_long,
|
||||
fcdd_max_size: *c_long,
|
||||
hvc_max_size: *c_long,
|
||||
) c_long;
|
||||
|
||||
pub extern fn EAnalogIn(
|
||||
idnum: *c_long,
|
||||
demo: c_long,
|
49
src/main.zig
49
src/main.zig
@ -1,25 +1,38 @@
|
||||
const std = @import("std");
|
||||
|
||||
const ljack = @import("./ljacklm.zig");
|
||||
const Config = @import("./Config.zig");
|
||||
const lj = @import("./labjack.zig");
|
||||
const RotCtl = @import("./RotCtl.zig");
|
||||
|
||||
pub fn main() !void {
|
||||
const ver = ljack.getDriverVersion();
|
||||
const log = std.log.scoped(.main);
|
||||
|
||||
pub fn main() !u8 {
|
||||
var gpa = std.heap.GeneralPurposeAllocator(.{}){};
|
||||
defer _ = gpa.deinit();
|
||||
const allocator = gpa.allocator();
|
||||
|
||||
blk: {
|
||||
const conf_file = std.fs.cwd().openFile("yaes.json", .{}) catch {
|
||||
log.warn("Could not load config file yaes.json. Using default config.", .{});
|
||||
Config.loadDefault(allocator);
|
||||
break :blk;
|
||||
};
|
||||
defer conf_file.close();
|
||||
|
||||
Config.load(allocator, conf_file.reader()) catch {
|
||||
log.err("Could not parse config file yaes.json. Good luck figuring out why.", .{});
|
||||
return 1;
|
||||
};
|
||||
}
|
||||
defer Config.destroy(allocator);
|
||||
|
||||
const ver = lj.getDriverVersion();
|
||||
std.debug.print("Driver version: {d}\n", .{ver});
|
||||
|
||||
const device = ljack.Labjack.autodetect();
|
||||
RotCtl.run(allocator) catch |err| {
|
||||
log.err("rotator controller ceased unexpectedly! {s}", .{@errorName(err)});
|
||||
return 1;
|
||||
};
|
||||
|
||||
const in = try device.analogReadOne(.{ .channel = .diff_01, .gain = 2 });
|
||||
std.debug.print("Read voltage: {d}. Overvolt: {}\n", .{ in.voltage, in.over_voltage });
|
||||
try device.digitalWriteOne(.{ .channel = .{ .io = 0 }, .level = true });
|
||||
|
||||
const sample = try device.readAnalogWriteDigital(
|
||||
2,
|
||||
.{ .{ .channel = .diff_01, .gain = 2 }, .{ .channel = .diff_23, .gain = 2 } },
|
||||
.{false} ** 4,
|
||||
true,
|
||||
);
|
||||
|
||||
for (sample, 0..) |input, idx| {
|
||||
std.debug.print(" channel {d}: {d} V\n", .{ idx, input.voltage });
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
Loading…
x
Reference in New Issue
Block a user