mirror of
https://github.com/azaion/autopilot.git
synced 2026-06-21 08:11:09 +00:00
chore: cargo fmt --all (gimbal_controller hygiene)
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -103,8 +103,8 @@ impl CentreOnTarget {
|
||||
let cy = (bbox.y_min + bbox.y_max) * 0.5;
|
||||
let err_x = cx - 0.5;
|
||||
let err_y = cy - 0.5;
|
||||
let on_target =
|
||||
err_x.abs() <= self.config.centre_half_width && err_y.abs() <= self.config.centre_half_width;
|
||||
let on_target = err_x.abs() <= self.config.centre_half_width
|
||||
&& err_y.abs() <= self.config.centre_half_width;
|
||||
|
||||
// Effective FOV shrinks as zoom grows; the same pixel error
|
||||
// therefore corresponds to a smaller angular error at high
|
||||
@@ -177,7 +177,9 @@ mod tests {
|
||||
let mut on_target_after = None;
|
||||
for tick_idx in 0..3 {
|
||||
let out = ctrl.tick(Some(bbox), yaw, pitch, zoom);
|
||||
let cmd = out.command.expect("loop should emit a command on every tick with bbox");
|
||||
let cmd = out
|
||||
.command
|
||||
.expect("loop should emit a command on every tick with bbox");
|
||||
let dy = cmd.yaw_deg - yaw;
|
||||
let dp = cmd.pitch_deg - pitch;
|
||||
yaw = cmd.yaw_deg;
|
||||
@@ -232,20 +234,35 @@ mod tests {
|
||||
let out5 = ctrl.tick(None, 0.0, 0.0, 1.0);
|
||||
|
||||
// Assert
|
||||
assert!(out3.target_lost_signal, "target_lost did not fire at tick 3");
|
||||
assert!(!out4.target_lost_signal, "target_lost re-fired during sustained loss");
|
||||
assert!(!out5.target_lost_signal, "target_lost re-fired during sustained loss");
|
||||
assert!(
|
||||
out3.target_lost_signal,
|
||||
"target_lost did not fire at tick 3"
|
||||
);
|
||||
assert!(
|
||||
!out4.target_lost_signal,
|
||||
"target_lost re-fired during sustained loss"
|
||||
);
|
||||
assert!(
|
||||
!out5.target_lost_signal,
|
||||
"target_lost re-fired during sustained loss"
|
||||
);
|
||||
|
||||
// Act 4: bbox returns → loss state clears, new streak can re-fire
|
||||
let recovered = ctrl.tick(Some(bbox_at(0.5, 0.5, 0.1, 0.1)), 0.0, 0.0, 1.0);
|
||||
assert!(recovered.command.is_some(), "recovery tick must emit command");
|
||||
assert!(
|
||||
recovered.command.is_some(),
|
||||
"recovery tick must emit command"
|
||||
);
|
||||
assert!(!recovered.target_lost_signal);
|
||||
|
||||
for _ in 0..2 {
|
||||
assert!(!ctrl.tick(None, 0.0, 0.0, 1.0).target_lost_signal);
|
||||
}
|
||||
let lost_again = ctrl.tick(None, 0.0, 0.0, 1.0);
|
||||
assert!(lost_again.target_lost_signal, "second loss streak did not fire");
|
||||
assert!(
|
||||
lost_again.target_lost_signal,
|
||||
"second loss streak did not fire"
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
|
||||
@@ -220,7 +220,11 @@ mod tests {
|
||||
match step {
|
||||
NextStep::Emit(cmd) => {
|
||||
let diff = (cmd.yaw_deg - 15.0).abs();
|
||||
assert!(diff < 0.01, "yaw at t=500ms was {}, want ~15.0", cmd.yaw_deg);
|
||||
assert!(
|
||||
diff < 0.01,
|
||||
"yaw at t=500ms was {}, want ~15.0",
|
||||
cmd.yaw_deg
|
||||
);
|
||||
}
|
||||
NextStep::Throttled => panic!("first emission should not be throttled"),
|
||||
}
|
||||
|
||||
@@ -29,9 +29,7 @@ pub use internal::centre_on_target::{
|
||||
CentreOnTarget, CentreOnTargetConfig, CentreOnTargetOutput, DEFAULT_CENTRE_WINDOW,
|
||||
DEFAULT_MAX_MISSED_TICKS, DEFAULT_TARGET_GAIN,
|
||||
};
|
||||
pub use internal::smooth_pan::{
|
||||
ExecutorStats, NextStep, PlanExecutor, DEFAULT_MIN_CMD_INTERVAL,
|
||||
};
|
||||
pub use internal::smooth_pan::{ExecutorStats, NextStep, PlanExecutor, DEFAULT_MIN_CMD_INTERVAL};
|
||||
pub use internal::sweep::{SweepConfig, SweepEngine, SweepPattern};
|
||||
pub use internal::transport::{
|
||||
A40Error, A40Transport, VendorFaults, VendorFaultsSnapshot, DEFAULT_COMMAND_DEADLINE,
|
||||
@@ -104,9 +102,12 @@ impl GimbalControllerHandle {
|
||||
/// vendor has acknowledged via a T1_F1_B1_D1 reply (its standard
|
||||
/// angle-feedback frame) or the bounded retry budget exhausts.
|
||||
pub async fn set_pose(&self, command: GimbalCommand) -> Result<()> {
|
||||
let transport = self.transport.as_ref().ok_or(AutopilotError::NotImplemented(
|
||||
"gimbal_controller::set_pose: no transport wired",
|
||||
))?;
|
||||
let transport = self
|
||||
.transport
|
||||
.as_ref()
|
||||
.ok_or(AutopilotError::NotImplemented(
|
||||
"gimbal_controller::set_pose: no transport wired",
|
||||
))?;
|
||||
let data = build_a1_angles(command.yaw_deg, command.pitch_deg);
|
||||
let _reply = transport
|
||||
.send_with_response(FrameId::A1, &data, FrameId::T1F1B1D1)
|
||||
@@ -129,9 +130,12 @@ impl GimbalControllerHandle {
|
||||
/// protocol. The continuous-rate C1 ZOOM_IN / ZOOM_OUT pair is
|
||||
/// reserved for AZ-654's sweep primitive.
|
||||
pub async fn zoom(&self, level: f32) -> Result<()> {
|
||||
let transport = self.transport.as_ref().ok_or(AutopilotError::NotImplemented(
|
||||
"gimbal_controller::zoom: no transport wired",
|
||||
))?;
|
||||
let transport = self
|
||||
.transport
|
||||
.as_ref()
|
||||
.ok_or(AutopilotError::NotImplemented(
|
||||
"gimbal_controller::zoom: no transport wired",
|
||||
))?;
|
||||
let data = build_c2_set_zoom(level);
|
||||
// C2 SET_EO_ZOOM ack arrives as a T1_F1_B1_D1 (the vendor's
|
||||
// generic angle/status feedback frame).
|
||||
|
||||
@@ -78,9 +78,22 @@ async fn az656_set_pose_publishes_monotonic_timestamp() {
|
||||
}
|
||||
|
||||
// Assert
|
||||
assert!(timestamps[0] > 0, "initial stamp should be > 0 after first set_pose");
|
||||
assert!(timestamps[1] > timestamps[0], "ts not monotonic: {} → {}", timestamps[0], timestamps[1]);
|
||||
assert!(timestamps[2] > timestamps[1], "ts not monotonic: {} → {}", timestamps[1], timestamps[2]);
|
||||
assert!(
|
||||
timestamps[0] > 0,
|
||||
"initial stamp should be > 0 after first set_pose"
|
||||
);
|
||||
assert!(
|
||||
timestamps[1] > timestamps[0],
|
||||
"ts not monotonic: {} → {}",
|
||||
timestamps[0],
|
||||
timestamps[1]
|
||||
);
|
||||
assert!(
|
||||
timestamps[2] > timestamps[1],
|
||||
"ts not monotonic: {} → {}",
|
||||
timestamps[1],
|
||||
timestamps[2]
|
||||
);
|
||||
}
|
||||
|
||||
/// AZ-655 integration — load a plan and exercise the executor against
|
||||
|
||||
Reference in New Issue
Block a user