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.
This commit is contained in:
parent
b08f819bdc
commit
bd465af30d
SSH Key Fingerprint:
SHA256:nCrXefBNo6EbjNSQhv0nXmEg/VuNq3sMF5b8zETw3Tk
@ -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 {
|
||||
|
||||
166
src/RotCtl.zig
166
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'
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user