From bd465af30d0ba0495e6de3f8973a46ffa6f0d7ba Mon Sep 17 00:00:00 2001 From: torque Date: Fri, 5 Jul 2024 00:32:42 -0700 Subject: [PATCH] rotctl: hook up remaining interface 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. --- src/LabjackYaesu.zig | 31 +++++--- src/RotCtl.zig | 166 ++++++++++++++++++++++++++++--------------- 2 files changed, 132 insertions(+), 65 deletions(-) diff --git a/src/LabjackYaesu.zig b/src/LabjackYaesu.zig index 593e160..34f5ab7 100644 --- a/src/LabjackYaesu.zig +++ b/src/LabjackYaesu.zig @@ -44,7 +44,7 @@ pub fn setTarget(self: LabjackYaesu, target: AzEl) void { controller.requested_state = .running; } -pub fn position(self: LabjackYaesu) AzEl { +pub fn currentPosition(self: LabjackYaesu) AzEl { self.lock.lock(); defer self.lock.unlock(); @@ -63,12 +63,12 @@ pub fn startCalibration(self: LabjackYaesu) void { _ = self; } -pub fn idle(self: LabjackYaesu) void { +pub fn quit(self: LabjackYaesu) void { self.lock.lock(); defer self.lock.unlock(); const controller = @constCast(self.controller); - controller.requested_state = .idle; + controller.requested_state = .stopped; } pub fn stop(self: LabjackYaesu) void { @@ -76,7 +76,12 @@ pub fn stop(self: LabjackYaesu) void { defer self.lock.unlock(); 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 { @@ -129,10 +134,16 @@ const Controller = struct { 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 .{ - .azimuth = lerpOne(input[0].voltage, config.labjack.feedback_calibration.azimuth), - .elevation = lerpOne(input[1].voltage, config.labjack.feedback_calibration.elevation), + .azimuth = lerpOne( + 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, ); - return lerpAngles(raw); + return lerpAndOffsetAngles(raw); } 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.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); - return lerpAngles(raw); + return lerpAndOffsetAngles(raw); } fn run(self: *Controller) !void { diff --git a/src/RotCtl.zig b/src/RotCtl.zig index 7c4c5ea..d0db0ea 100644 --- a/src/RotCtl.zig +++ b/src/RotCtl.zig @@ -36,6 +36,7 @@ pub fn run(allocator: std.mem.Allocator) !void { const client = try server.accept(); defer { log.info("disconnecting client", .{}); + interface.rotator.stop(); client.stream.close(); } @@ -51,9 +52,12 @@ pub fn run(allocator: std.mem.Allocator) !void { while (interface.running) : (fbs.reset()) { 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), - ); + ) catch break; } } } @@ -72,55 +76,101 @@ fn printReply(self: *RotCtl, comptime fmt: []const u8, args: anytype) !void { 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( self: *RotCtl, command: []const u8, ) !void { + if (command.len == 0) { + return self.replyStatus(.invalid_parameter) catch error.BadOutput; + } + var tokens = std.mem.tokenizeScalar(u8, command, ' '); const first = tokens.next().?; if (first.len == 1 or first[0] == '\\') { switch (first[0]) { - 'q', 'Q' => { - self.running = false; - self.replyStatus(.okay) catch {}; - self.rotator.stop(); - }, - 'P' => { - const pos = self.rotator.position(); - try self.printReply("{d:.1} {d:.1}", .{ pos.azimuth, pos.elevation }); - }, - '\\' => { - try self.parseLongCommand(first[1..], &tokens); - }, + // 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), + 'P' => try self.setPosition(first, &tokens), + '\\' => try self.handleLongCommand(first[1..], &tokens), else => { log.err("unknown short command '{s}'", .{command}); - try self.replyStatus(.not_implemented); + self.replyStatus(.not_supported) catch return error.BadOutput; }, } } else { - try self.parseLongCommand(first, &tokens); + try self.handleLongCommand(first, &tokens); } } -fn parseLongCommand( +fn handleLongCommand( self: *RotCtl, command: []const u8, - tokens: *std.mem.TokenIterator(u8, .scalar), + tokens: *TokenIter, ) !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| { - 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); + return self.replyStatus(.not_supported) catch error.BadOutput; } 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 { short: ?u8 = null, long: ?[]const u8 = null, + callback: CommandCallback, }; const rotctl_commands = [_]HamlibCommand{ - .{ .short = 'q' }, // quit - .{ .short = 'Q' }, // quit - .{ .long = "AOS" }, - .{ .long = "LOS" }, - .{ .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" }, + .{ .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 + .{ .short = 'p', .long = "get_pos", .callback = getPosition }, // return az: f64, el: f64 + .{ .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", .callback = stop }, + .{ .short = 'K', .long = "park", .callback = park }, + .{ .short = 'C', .long = "set_conf", .callback = notSupported }, // token: []const u8, value: []const u8 + .{ .short = 'R', .long = "reset", .callback = notSupported }, // u1 (1 is reset all) + .{ .short = '_', .long = "get_info", .callback = notSupported }, // return Model name + .{ .long = "dump_state", .callback = notSupported }, // ??? + .{ .short = '1', .long = "dump_caps", .callback = notSupported }, // ??? + .{ .short = 'w', .long = "send_cmd", .callback = notSupported }, // []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 = "loc2lonlat", .callback = notSupported }, // the inverse of the above + .{ .short = 'D', .long = "dms2dec", .callback = notSupported }, // deg, min, sec, 0 (positive) or 1 (negative) + .{ .short = 'd', .long = "dec2dms", .callback = notSupported }, + .{ .short = 'E', .long = "dmmm2dec", .callback = notSupported }, + .{ .short = 'e', .long = "dec2dmmm", .callback = notSupported }, + .{ .short = 'B', .long = "grb", .callback = notSupported }, + .{ .short = 'A', .long = "a_sp2a_lp", .callback = notSupported }, + .{ .short = 'a', .long = "d_sp2d_lp", .callback = notSupported }, + .{ .long = "pause", .callback = notSupported }, }; // D, dms2dec 'Degrees' 'Minutes' 'Seconds' 'S/W'