yaes/src/YaesuController.zig

const std = @import("std");

const lj = @import("./labjack.zig");
const Config = @import("./Config.zig");
const config = Config.global;

const log = std.log.scoped(.yaesu_controller);

const YaesuController = @This();

control_thread: std.Thread,
lock: *std.Thread.Mutex,
controller: *const Controller,

pub const AzEl = struct {
    azimuth: f64,
    elevation: f64,
};

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);
    // 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,
        .controller = controller,
    };
}

fn inRange(request: f64, comptime dof: enum { azimuth, elevation }) bool {
    return switch (dof) {
        // zig fmt: off
        .azimuth => request >= (
                  config.labjack.feedback_calibration.azimuth.minimum.angle
                + config.controller.angle_offset.azimuth
            ) and request <= (
                  config.labjack.feedback_calibration.azimuth.maximum.angle
                + config.controller.angle_offset.azimuth
        ),
        .elevation => request >= (
                  config.labjack.feedback_calibration.elevation.minimum.angle
                + config.controller.angle_offset.elevation
            ) and request <= (
                  config.labjack.feedback_calibration.elevation.maximum.angle
                + config.controller.angle_offset.elevation
        ),
        // zig fmt: on
    };
}

pub fn setTarget(self: YaesuController, target: AzEl) error{OutOfRange}!void {
    self.lock.lock();
    defer self.lock.unlock();

    const masked_target: AzEl = .{
        .azimuth = target.azimuth,
        .elevation = @min(
            @max(target.elevation, config.controller.elevation_mask),
            180.0 - config.controller.elevation_mask,
        ),
    };

    if (!inRange(masked_target.azimuth, .azimuth) or !inRange(masked_target.elevation, .elevation))
        return error.OutOfRange;

    const controller = @constCast(self.controller);
    controller.target = masked_target;
    controller.requested_state = .running;
}

pub fn currentPosition(self: YaesuController) AzEl {
    self.lock.lock();
    defer self.lock.unlock();

    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 quit(self: YaesuController) void {
    self.lock.lock();
    defer self.lock.unlock();

    const controller = @constCast(self.controller);
    controller.requested_state = .stopped;
}

pub fn stop(self: YaesuController) void {
    self.lock.lock();
    defer self.lock.unlock();

    const controller = @constCast(self.controller);
    controller.target = controller.position;
    controller.requested_state = .idle;
}

pub fn startPark(self: YaesuController) void {
    self.setTarget(config.controller.parking_posture) catch unreachable;
}

fn runController(controller: *Controller) void {
    controller.run() catch {
        log.err(
            "the rotator control loop has terminated unexpectedly!!!!",
            .{},
        );
    };
}

const Controller = struct {
    target: AzEl,
    position: AzEl,

    current_state: ControllerState,
    requested_state: ControllerState,

    lock: *std.Thread.Mutex,
    labjack: lj.Labjack,

    const ControllerState = enum {
        initializing,
        idle,
        calibration,
        running,
        stopped,
    };

    fn init(self: *Controller, lock: *std.Thread.Mutex) void {
        self.* = .{
            .target = .{ .azimuth = 0, .elevation = 0 },
            .position = .{ .azimuth = 0, .elevation = 0 },
            .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),
            },
        };
    }

    fn connectLabjack(self: *Controller) !void {
        const info = try self.labjack.connect();
        try self.labjack.setAllDigitalOutputLow();
        self.labjack.id = info.local_id;
    }

    fn lerpOne(input: f64, cal_points: Config.MinMax) f64 {
        return (input - cal_points.minimum.voltage) * cal_points.slope() + cal_points.minimum.angle;
    }

    fn lerpAndOffsetAngles(input: [2]lj.AnalogReadResult) AzEl {
        return .{
            .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,
        };
    }

    const Sign = enum {
        negative,
        zero,
        positive,

        pub fn symbol(self: Sign) u8 {
            return switch (self) {
                .negative => '-',
                .zero => '=',
                .positive => '+',
            };
        }
    };

    fn signDeadzone(offset: f64, deadzone: f64) Sign {
        return if (@abs(offset) < deadzone)
            .zero
        else if (offset < 0)
            .negative
        else
            .positive;
    }

    fn updateAzEl(self: *const Controller) !AzEl {
        const inputs = .{ config.controller.azimuth_input, config.controller.elevation_input };

        const raw = try self.labjack.readAnalogWriteDigital(
            2,
            inputs,
            .{false} ** 4,
            true,
        );

        return lerpAndOffsetAngles(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;

        const azsign = signDeadzone(
            pos_error.azimuth,
            config.controller.angle_tolerance.azimuth,
        );

        const elsign = signDeadzone(
            pos_error.elevation,
            config.controller.angle_tolerance.elevation,
        );

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

        log.info(
            "az: {d:.1}° ({d:.2} V) {d:.1}° => {c}, el: {d:.1}° ({d:.2} V) {d:.1}° => {c}",
            .{
                angles.azimuth,
                raw[0].voltage,
                pos_error.azimuth,
                azsign.symbol(),
                angles.elevation,
                raw[1].voltage,
                pos_error.elevation,
                elsign.symbol(),
            },
        );
        return angles;
    }

    fn run(self: *Controller) !void {
        self.current_state = .initializing;

        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;
                };

                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,
                    };
                };

                const pos = 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;
            },
        };
    }
};

pub const LoopTimer = struct {
    interval_ns: u64,

    start: i128 = 0,

    pub fn mark(self: *LoopTimer) bool {
        self.start = std.time.nanoTimestamp();
        return true;
    }

    pub fn sleep(self: *LoopTimer) void {
        const now = std.time.nanoTimestamp();
        const elapsed: u64 = @intCast(now - self.start);

        std.time.sleep(self.interval_ns - elapsed);
    }
};