From 2937de6fcda0aa7bfacda250525d6f891a5f8c8a Mon Sep 17 00:00:00 2001
From: torque <torque@epicyclic.dev>
Date: Sat, 6 Jul 2024 13:31:59 -0700
Subject: [PATCH] controller: apply range checking after applying elevation
 mask

This should work better by not causing command errors when trying to
move to postures that would be masked out anyway.
---
 src/LabjackYaesu.zig | 34 ++++++++++++++++++++++++++++++----
 src/RotCtl.zig       | 12 ++++++------
 2 files changed, 36 insertions(+), 10 deletions(-)

diff --git a/src/LabjackYaesu.zig b/src/LabjackYaesu.zig
index b46dab3..bb46b01 100644
--- a/src/LabjackYaesu.zig
+++ b/src/LabjackYaesu.zig
@@ -35,18 +35,44 @@ pub fn init(allocator: std.mem.Allocator) !LabjackYaesu {
     };
 }
 
-pub fn setTarget(self: LabjackYaesu, target: AzEl) void {
+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: LabjackYaesu, target: AzEl) error{OutOfRange}!void {
     self.lock.lock();
     defer self.lock.unlock();
 
-    const controller = @constCast(self.controller);
-    controller.target = .{
+    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;
 }
 
@@ -87,7 +113,7 @@ pub fn stop(self: LabjackYaesu) void {
 }
 
 pub fn startPark(self: LabjackYaesu) void {
-    self.setTarget(config.controller.parking_posture);
+    self.setTarget(config.controller.parking_posture) catch unreachable;
 }
 
 fn runController(controller: *Controller) void {
diff --git a/src/RotCtl.zig b/src/RotCtl.zig
index e348618..de8d633 100644
--- a/src/RotCtl.zig
+++ b/src/RotCtl.zig
@@ -142,19 +142,19 @@ fn setPosition(self: *RotCtl, _: []const u8, tokens: *TokenIter) CommandError!vo
         return self.replyStatus(.invalid_parameter) catch error.BadOutput;
     };
 
-    if (!inRange(azimuth, .azimuth))
-        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;
     };
 
-    if (!inRange(elevation, .elevation))
-        return self.replyStatus(.invalid_parameter) catch error.BadOutput;
+    self.rotator.setTarget(.{
+        .azimuth = azimuth,
+        .elevation = elevation,
+    }) catch |err| switch (err) {
+        error.OutOfRange => return self.replyStatus(.invalid_parameter) catch error.BadOutput,
+    };
 
-    self.rotator.setTarget(.{ .azimuth = azimuth, .elevation = elevation });
     return self.replyStatus(.okay) catch error.BadOutput;
 }