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.
This commit is contained in:
parent
153dde40aa
commit
0ccc65fd10
SSH Key Fingerprint:
SHA256:nCrXefBNo6EbjNSQhv0nXmEg/VuNq3sMF5b8zETw3Tk
@ -127,6 +127,7 @@ 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 };
|
||||
@ -179,6 +180,8 @@ const ControllerConfig = struct {
|
||||
angle_offset: AzEl,
|
||||
elevation_mask: f64,
|
||||
|
||||
feedback_window_samples: u8,
|
||||
|
||||
const OutPair = struct {
|
||||
increase: lj.DigitalOutputChannel,
|
||||
decrease: lj.DigitalOutputChannel,
|
||||
|
||||
@ -36,19 +36,16 @@ 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.init(lock);
|
||||
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 = lock,
|
||||
.lock = &controller.lock,
|
||||
.controller = controller,
|
||||
};
|
||||
}
|
||||
@ -101,16 +98,15 @@ pub fn currentPosition(self: YaesuController) AzEl {
|
||||
return self.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 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 {
|
||||
@ -155,16 +151,77 @@ 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,
|
||||
@ -173,13 +230,13 @@ const Controller = struct {
|
||||
stopped,
|
||||
};
|
||||
|
||||
fn init(self: *Controller, lock: *std.Thread.Mutex) void {
|
||||
self.* = .{
|
||||
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,
|
||||
.lock = lock,
|
||||
.labjack = switch (config.labjack.device) {
|
||||
.autodetect => lj.Labjack.autodetect(),
|
||||
.serial_number => |sn| lj.Labjack.with_serial_number(sn),
|
||||
@ -187,6 +244,10 @@ 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();
|
||||
@ -233,26 +294,23 @@ const Controller = struct {
|
||||
.positive;
|
||||
}
|
||||
|
||||
fn updateAzEl(self: *const Controller) !AzEl {
|
||||
const inputs = .{ config.controller.azimuth_input, config.controller.elevation_input };
|
||||
fn updateFeedback(self: *Controller) !void {
|
||||
const inputs = .{
|
||||
config.controller.azimuth_input,
|
||||
config.controller.elevation_input,
|
||||
};
|
||||
|
||||
const raw = try self.labjack.readAnalogWriteDigital(
|
||||
2,
|
||||
inputs,
|
||||
.{false} ** 4,
|
||||
null,
|
||||
true,
|
||||
);
|
||||
|
||||
return lerpAndOffsetAngles(raw);
|
||||
self.feedback_buffer.push(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;
|
||||
|
||||
fn drive(self: *const Controller, pos_error: AzEl) !void {
|
||||
const azsign = signDeadzone(
|
||||
pos_error.azimuth,
|
||||
config.controller.angle_tolerance.azimuth,
|
||||
@ -263,29 +321,35 @@ 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 = try self.labjack.readAnalogWriteDigital(2, inputs, drive_signal, true);
|
||||
const angles = lerpAndOffsetAngles(raw);
|
||||
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}",
|
||||
.{
|
||||
angles.azimuth,
|
||||
raw[0].voltage,
|
||||
self.position.azimuth,
|
||||
raw.azimuth,
|
||||
pos_error.azimuth,
|
||||
azsign.symbol(),
|
||||
angles.elevation,
|
||||
raw[1].voltage,
|
||||
self.position.elevation,
|
||||
raw.elevation,
|
||||
pos_error.elevation,
|
||||
elsign.symbol(),
|
||||
},
|
||||
);
|
||||
return angles;
|
||||
|
||||
try self.labjack.writeIoLines(drive_signal);
|
||||
}
|
||||
|
||||
fn setPosition(self: *Controller, position: AzEl) void {
|
||||
self.position = position;
|
||||
self.condition.broadcast();
|
||||
}
|
||||
|
||||
fn run(self: *Controller) !void {
|
||||
@ -293,61 +357,58 @@ const Controller = struct {
|
||||
|
||||
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;
|
||||
};
|
||||
|
||||
while (timer.mark()) : (timer.sleep()) {
|
||||
self.updateFeedback() catch {
|
||||
self.lock.lock();
|
||||
defer self.lock.unlock();
|
||||
|
||||
self.position = pos;
|
||||
self.current_state = self.requested_state;
|
||||
},
|
||||
.calibration => {
|
||||
self.lock.lock();
|
||||
defer self.lock.unlock();
|
||||
self.current_state = .stopped;
|
||||
continue;
|
||||
};
|
||||
|
||||
// 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.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();
|
||||
|
||||
break :blk .{
|
||||
.azimuth = self.target.azimuth - self.position.azimuth,
|
||||
.elevation = self.target.elevation - self.position.elevation,
|
||||
// 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.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;
|
||||
},
|
||||
};
|
||||
self.current_state = .stopped;
|
||||
continue;
|
||||
};
|
||||
},
|
||||
.stopped => {
|
||||
// attempt to reset the drive outputs
|
||||
try self.labjack.writeIoLines(.{false} ** 4);
|
||||
break;
|
||||
},
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@ -68,6 +68,30 @@ 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) {
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user