factotum/yaes
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

rotctl: hook up remaining interface

Browse Source 
This is missing a little bit of input validation, e.g. we don't
currently check that set_position azimuth and elevation are actually
in the range that the controller can possibly move.

The geodetic north offset configuration value is applied in when
computing the current position, but I think there are still some
slightly fiddly edge cases around it and I haven't actually figured
out which direction I want the sign to be.

The various pieces appear to be functional, so next up will be figuring
out what all the problems are with some hardware in the loop.
This commit is contained in:
torque 2024-07-05 00:32:42 -07:00
parent b08f819bdc
commit bd465af30d
Signed by: torque
SSH Key Fingerprint: SHA256:nCrXefBNo6EbjNSQhv0nXmEg/VuNq3sMF5b8zETw3Tk
2 changed files with 132 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
src/LabjackYaesu.zig
View File

@ -44,7 +44,7 @@ pub fn setTarget(self: LabjackYaesu, target: AzEl) void {
controller.requested_state = .running; controller.requested_state = .running;
} }
pub fn position(self: LabjackYaesu) AzEl { pub fn currentPosition(self: LabjackYaesu) AzEl {
self.lock.lock(); self.lock.lock();
defer self.lock.unlock(); defer self.lock.unlock();
@ -63,12 +63,12 @@ pub fn startCalibration(self: LabjackYaesu) void {
_ = self; _ = self;
} }
pub fn idle(self: LabjackYaesu) void { pub fn quit(self: LabjackYaesu) void {
self.lock.lock(); self.lock.lock();
defer self.lock.unlock(); defer self.lock.unlock();
const controller = @constCast(self.controller); const controller = @constCast(self.controller);
controller.requested_state = .idle; controller.requested_state = .stopped;
} }
pub fn stop(self: LabjackYaesu) void { pub fn stop(self: LabjackYaesu) void {
@ -76,7 +76,12 @@ pub fn stop(self: LabjackYaesu) void {
defer self.lock.unlock(); defer self.lock.unlock();
const controller = @constCast(self.controller); const controller = @constCast(self.controller);
controller.requested_state = .stopped; controller.target = controller.position;
controller.requested_state = .idle;
}
pub fn startPark(self: LabjackYaesu) void {
self.setTarget(config.controller.parking_posture);
} }
fn runController(controller: *Controller) void { fn runController(controller: *Controller) void {
@ -129,10 +134,16 @@ const Controller = struct {
return (input - cal_points.minimum.voltage) * cal_points.slope() + cal_points.minimum.angle; return (input - cal_points.minimum.voltage) * cal_points.slope() + cal_points.minimum.angle;
} }
fn lerpAngles(input: [2]lj.AnalogReadResult) AzEl { fn lerpAndOffsetAngles(input: [2]lj.AnalogReadResult) AzEl {
return .{ return .{
.azimuth = lerpOne(input[0].voltage, config.labjack.feedback_calibration.azimuth), .azimuth = lerpOne(
.elevation = lerpOne(input[1].voltage, config.labjack.feedback_calibration.elevation), 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,
}; };
} }
@ -155,7 +166,7 @@ const Controller = struct {
true, true,
); );
return lerpAngles(raw); return lerpAndOffsetAngles(raw);
} }
fn drive(self: *const Controller, pos_error: AzEl) !AzEl { fn drive(self: *const Controller, pos_error: AzEl) !AzEl {
@ -180,9 +191,11 @@ const Controller = struct {
drive_signal[config.controller.elevation_outputs.increase.io] = elsign == .positive; drive_signal[config.controller.elevation_outputs.increase.io] = elsign == .positive;
drive_signal[config.controller.elevation_outputs.decrease.io] = elsign == .negative; drive_signal[config.controller.elevation_outputs.decrease.io] = elsign == .negative;
log.info("drive: az = {s}, el = {s}. outputs: {any}", .{ @tagName(azsign), @tagName(elsign), drive_signal });
const raw = try self.labjack.readAnalogWriteDigital(2, inputs, drive_signal, true); const raw = try self.labjack.readAnalogWriteDigital(2, inputs, drive_signal, true);
return lerpAngles(raw); return lerpAndOffsetAngles(raw);
} }
fn run(self: *Controller) !void { fn run(self: *Controller) !void {

166
src/RotCtl.zig
View File

@ -36,6 +36,7 @@ pub fn run(allocator: std.mem.Allocator) !void {
const client = try server.accept(); const client = try server.accept();
defer { defer {
log.info("disconnecting client", .{}); log.info("disconnecting client", .{});
interface.rotator.stop();
client.stream.close(); client.stream.close();
} }
@ -51,9 +52,12 @@ pub fn run(allocator: std.mem.Allocator) !void {
while (interface.running) : (fbs.reset()) { while (interface.running) : (fbs.reset()) {
reader.streamUntilDelimiter(fbs.writer(), '\n', readbuffer.len) catch break; reader.streamUntilDelimiter(fbs.writer(), '\n', readbuffer.len) catch break;
try interface.handleHamlibCommand( // note: an error here kills this entire function, which may not be
// desirable. For example, if the client unexpectedly disconnects, we
// probably shouldn't kill the whole runloop.
interface.handleHamlibCommand(
std.mem.trim(u8, fbs.getWritten(), &std.ascii.whitespace), std.mem.trim(u8, fbs.getWritten(), &std.ascii.whitespace),
); ) catch break;
} }
} }
} }
@ -72,55 +76,101 @@ fn printReply(self: *RotCtl, comptime fmt: []const u8, args: anytype) !void {
try self.writer.flush(); try self.writer.flush();
} }
fn quit(self: *RotCtl, _: []const u8, tokens: *TokenIter) CommandError!void {
if (tokens.next() != null) return error.BadInput;
self.running = false;
self.replyStatus(.okay) catch {};
self.rotator.quit();
}
fn stop(self: *RotCtl, _: []const u8, tokens: *TokenIter) CommandError!void {
if (tokens.next() != null) return error.BadInput;
self.rotator.stop();
self.replyStatus(.okay) catch return error.BadOutput;
}
fn park(self: *RotCtl, _: []const u8, tokens: *TokenIter) CommandError!void {
if (tokens.next() != null) return error.BadInput;
self.rotator.startPark();
self.replyStatus(.okay) catch return error.BadOutput;
}
fn blindAck(self: *RotCtl, _: []const u8, _: *TokenIter) CommandError!void {
self.replyStatus(.okay) catch return error.BadOutput;
}
fn notSupported(self: *RotCtl, _: []const u8, _: *TokenIter) CommandError!void {
self.replyStatus(.not_supported) catch return error.BadOutput;
}
fn getPosition(self: *RotCtl, _: []const u8, tokens: *TokenIter) CommandError!void {
if (tokens.next() != null) return error.BadInput;
const pos = self.rotator.currentPosition();
self.printReply("{d:.1}\n{d:.1}", .{ pos.azimuth, pos.elevation }) catch return error.BadOutput;
}
fn setPosition(self: *RotCtl, _: []const u8, tokens: *TokenIter) CommandError!void {
const azimuth = std.fmt.parseFloat(f64, tokens.next() orelse {
return self.replyStatus(.invalid_parameter) catch error.BadOutput;
}) catch {
return self.replyStatus(.invalid_parameter) catch error.BadOutput;
};
const elevation = std.fmt.parseFloat(f64, tokens.next() orelse {
return self.replyStatus(.invalid_parameter) catch error.BadOutput;
}) catch {
return self.replyStatus(.invalid_parameter) catch error.BadOutput;
};
self.rotator.setTarget(.{ .azimuth = azimuth, .elevation = elevation });
return self.replyStatus(.okay) catch error.BadOutput;
}
fn handleHamlibCommand( fn handleHamlibCommand(
self: *RotCtl, self: *RotCtl,
command: []const u8, command: []const u8,
) !void { ) !void {
if (command.len == 0) {
return self.replyStatus(.invalid_parameter) catch error.BadOutput;
}
var tokens = std.mem.tokenizeScalar(u8, command, ' '); var tokens = std.mem.tokenizeScalar(u8, command, ' ');
const first = tokens.next().?; const first = tokens.next().?;
if (first.len == 1 or first[0] == '\\') { if (first.len == 1 or first[0] == '\\') {
switch (first[0]) { switch (first[0]) {
'q', 'Q' => { // NOTE: this is not technically supported by rotctld.
self.running = false; 'q', 'Q' => try self.quit(first, &tokens),
self.replyStatus(.okay) catch {}; 'S' => try self.stop(first, &tokens),
self.rotator.stop(); 'K' => try self.park(first, &tokens),
}, 'p' => try self.getPosition(first, &tokens),
'P' => { 'P' => try self.setPosition(first, &tokens),
const pos = self.rotator.position(); '\\' => try self.handleLongCommand(first[1..], &tokens),
try self.printReply("{d:.1} {d:.1}", .{ pos.azimuth, pos.elevation });
},
'\\' => {
try self.parseLongCommand(first[1..], &tokens);
},
else => { else => {
log.err("unknown short command '{s}'", .{command}); log.err("unknown short command '{s}'", .{command});
try self.replyStatus(.not_implemented); self.replyStatus(.not_supported) catch return error.BadOutput;
}, },
} }
} else { } else {
try self.parseLongCommand(first, &tokens); try self.handleLongCommand(first, &tokens);
} }
} }
fn parseLongCommand( fn handleLongCommand(
self: *RotCtl, self: *RotCtl,
command: []const u8, command: []const u8,
tokens: *std.mem.TokenIterator(u8, .scalar), tokens: *TokenIter,
) !void { ) !void {
_ = tokens; inline for (rotctl_commands) |cmdef|
if (comptime cmdef.long) |long|
if (std.mem.eql(u8, long, command))
return try cmdef.callback(self, command, tokens);
for (rotctl_commands) |check| { return self.replyStatus(.not_supported) catch error.BadOutput;
if (check.long) |long| {
if (command.len >= long.len and std.mem.eql(u8, long, command)) {
log.warn("Unsupported long command {s}", .{command});
break;
}
}
} else {
log.warn("Unknown long command '{s}'", .{command});
}
return self.replyStatus(.not_supported);
} }
const HamlibErrorCode = enum(u8) { const HamlibErrorCode = enum(u8) {
@ -154,38 +204,42 @@ const HamlibErrorCode = enum(u8) {
} }
}; };
const CommandError = error{ BadInput, BadOutput };
const TokenIter: type = std.mem.TokenIterator(u8, .scalar);
const CommandCallback: type = *const fn (self: *RotCtl, command: []const u8, tokens: *TokenIter) CommandError!void;
const HamlibCommand = struct { const HamlibCommand = struct {
short: ?u8 = null, short: ?u8 = null,
long: ?[]const u8 = null, long: ?[]const u8 = null,
callback: CommandCallback,
}; };
const rotctl_commands = [_]HamlibCommand{ const rotctl_commands = [_]HamlibCommand{
.{ .short = 'q' }, // quit .{ .short = 'q', .callback = quit }, // quit
.{ .short = 'Q' }, // quit .{ .short = 'Q', .callback = quit }, // quit
.{ .long = "AOS" }, .{ .long = "AOS", .callback = blindAck },
.{ .long = "LOS" }, .{ .long = "LOS", .callback = blindAck },
.{ .short = 'P', .long = "set_pos" }, // azimuth: f64, elevation: f64 .{ .short = 'P', .long = "set_pos", .callback = setPosition }, // azimuth: f64, elevation: f64
.{ .short = 'p', .long = "get_pos" }, // return az: f64, el: f64 .{ .short = 'p', .long = "get_pos", .callback = getPosition }, // 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 = 'M', .long = "move", .callback = notSupported }, // direction: enum { up=2, down=4, left=8, right=16 }, speed: i8 (0-100 or -1)
.{ .short = 'S', .long = "stop" }, .{ .short = 'S', .long = "stop", .callback = stop },
.{ .short = 'K', .long = "park" }, .{ .short = 'K', .long = "park", .callback = park },
.{ .short = 'C', .long = "set_conf" }, // token: []const u8, value: []const u8 .{ .short = 'C', .long = "set_conf", .callback = notSupported }, // token: []const u8, value: []const u8
.{ .short = 'R', .long = "reset" }, // u1 (1 is reset all) .{ .short = 'R', .long = "reset", .callback = notSupported }, // u1 (1 is reset all)
.{ .short = '_', .long = "get_info" }, // return Model name .{ .short = '_', .long = "get_info", .callback = notSupported }, // return Model name
.{ .short = 'K', .long = "park" }, .{ .long = "dump_state", .callback = notSupported }, // ???
.{ .long = "dump_state" }, // ??? .{ .short = '1', .long = "dump_caps", .callback = notSupported }, // ???
.{ .short = '1', .long = "dump_caps" }, // ??? .{ .short = 'w', .long = "send_cmd", .callback = notSupported }, // []const u8, send serial command directly to the rotator
.{ .short = 'w', .long = "send_cmd" }, // []const u8, send serial command directly to the rotator .{ .short = 'L', .long = "lonlat2loc", .callback = notSupported }, // return Maidenhead locator for given long: f64 and , .callback = notSupportedlat: f64, locator precision: u4 (2-12)
.{ .short = 'L', .long = "lonlat2loc" }, // return Maidenhead locator for given long: f64 and lat: f64, locator precision: u4 (2-12) .{ .short = 'l', .long = "loc2lonlat", .callback = notSupported }, // the inverse of the above
.{ .short = 'l', .long = "loc2lonlat" }, // the inverse of the above .{ .short = 'D', .long = "dms2dec", .callback = notSupported }, // deg, min, sec, 0 (positive) or 1 (negative)
.{ .short = 'D', .long = "dms2dec" }, // deg, min, sec, 0 (positive) or 1 (negative) .{ .short = 'd', .long = "dec2dms", .callback = notSupported },
.{ .short = 'd', .long = "dec2dms" }, .{ .short = 'E', .long = "dmmm2dec", .callback = notSupported },
.{ .short = 'E', .long = "dmmm2dec" }, .{ .short = 'e', .long = "dec2dmmm", .callback = notSupported },
.{ .short = 'e', .long = "dec2dmmm" }, .{ .short = 'B', .long = "grb", .callback = notSupported },
.{ .short = 'B', .long = "grb" }, .{ .short = 'A', .long = "a_sp2a_lp", .callback = notSupported },
.{ .short = 'A', .long = "a_sp2a_lp" }, .{ .short = 'a', .long = "d_sp2d_lp", .callback = notSupported },
.{ .short = 'a', .long = "d_sp2d_lp" }, .{ .long = "pause", .callback = notSupported },
.{ .long = "pause" },
}; };
// D, dms2dec 'Degrees' 'Minutes' 'Seconds' 'S/W' // D, dms2dec 'Degrees' 'Minutes' 'Seconds' 'S/W'