Compare commits

..

No commits in common. "38a286da11f28a11923f935d4b1e9507bd60050f" and "153dde40aa39084235cef2ca58007f6567de9553" have entirely different histories.

4 changed files with 89 additions and 182 deletions

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 = true,
},
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

@ -33,7 +33,7 @@ pub fn run(allocator: std.mem.Allocator) !void {
.rotator = try YaesuController.init(allocator),
};
while (interface.running) {
while (true) {
const client = try server.accept();
defer {
log.info("disconnecting client", .{});
@ -42,6 +42,8 @@ pub fn run(allocator: std.mem.Allocator) !void {
}
interface.writer = .{ .unbuffered_writer = client.stream.writer() };
interface.running = true;
defer interface.running = false;
log.info("client connected from {}", .{client.address});
@ -58,11 +60,6 @@ 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();
}
}
}

View File

@ -36,16 +36,19 @@ 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.* = 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();
return .{
.control_thread = try std.Thread.spawn(.{}, runController, .{controller}),
.lock = &controller.lock,
.lock = lock,
.controller = controller,
};
}
@ -98,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 {
@ -151,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,
@ -230,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),
@ -244,10 +187,6 @@ 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();
@ -294,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,
@ -321,35 +263,29 @@ 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}",
.{
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 {
@ -357,58 +293,61 @@ const Controller = struct {
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 => {
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.requested_state = self.current_state;
},
.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;
},
};
}
};

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) {