mirror of
https://github.com/azaion/autopilot.git
synced 2026-06-21 23:31:10 +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 cy = (bbox.y_min + bbox.y_max) * 0.5;
|
||||||
let err_x = cx - 0.5;
|
let err_x = cx - 0.5;
|
||||||
let err_y = cy - 0.5;
|
let err_y = cy - 0.5;
|
||||||
let on_target =
|
let on_target = err_x.abs() <= self.config.centre_half_width
|
||||||
err_x.abs() <= self.config.centre_half_width && err_y.abs() <= self.config.centre_half_width;
|
&& err_y.abs() <= self.config.centre_half_width;
|
||||||
|
|
||||||
// Effective FOV shrinks as zoom grows; the same pixel error
|
// Effective FOV shrinks as zoom grows; the same pixel error
|
||||||
// therefore corresponds to a smaller angular error at high
|
// therefore corresponds to a smaller angular error at high
|
||||||
@@ -177,7 +177,9 @@ mod tests {
|
|||||||
let mut on_target_after = None;
|
let mut on_target_after = None;
|
||||||
for tick_idx in 0..3 {
|
for tick_idx in 0..3 {
|
||||||
let out = ctrl.tick(Some(bbox), yaw, pitch, zoom);
|
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 dy = cmd.yaw_deg - yaw;
|
||||||
let dp = cmd.pitch_deg - pitch;
|
let dp = cmd.pitch_deg - pitch;
|
||||||
yaw = cmd.yaw_deg;
|
yaw = cmd.yaw_deg;
|
||||||
@@ -232,20 +234,35 @@ mod tests {
|
|||||||
let out5 = ctrl.tick(None, 0.0, 0.0, 1.0);
|
let out5 = ctrl.tick(None, 0.0, 0.0, 1.0);
|
||||||
|
|
||||||
// Assert
|
// Assert
|
||||||
assert!(out3.target_lost_signal, "target_lost did not fire at tick 3");
|
assert!(
|
||||||
assert!(!out4.target_lost_signal, "target_lost re-fired during sustained loss");
|
out3.target_lost_signal,
|
||||||
assert!(!out5.target_lost_signal, "target_lost re-fired during sustained loss");
|
"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
|
// 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);
|
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);
|
assert!(!recovered.target_lost_signal);
|
||||||
|
|
||||||
for _ in 0..2 {
|
for _ in 0..2 {
|
||||||
assert!(!ctrl.tick(None, 0.0, 0.0, 1.0).target_lost_signal);
|
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);
|
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]
|
#[test]
|
||||||
|
|||||||
@@ -220,7 +220,11 @@ mod tests {
|
|||||||
match step {
|
match step {
|
||||||
NextStep::Emit(cmd) => {
|
NextStep::Emit(cmd) => {
|
||||||
let diff = (cmd.yaw_deg - 15.0).abs();
|
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"),
|
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,
|
CentreOnTarget, CentreOnTargetConfig, CentreOnTargetOutput, DEFAULT_CENTRE_WINDOW,
|
||||||
DEFAULT_MAX_MISSED_TICKS, DEFAULT_TARGET_GAIN,
|
DEFAULT_MAX_MISSED_TICKS, DEFAULT_TARGET_GAIN,
|
||||||
};
|
};
|
||||||
pub use internal::smooth_pan::{
|
pub use internal::smooth_pan::{ExecutorStats, NextStep, PlanExecutor, DEFAULT_MIN_CMD_INTERVAL};
|
||||||
ExecutorStats, NextStep, PlanExecutor, DEFAULT_MIN_CMD_INTERVAL,
|
|
||||||
};
|
|
||||||
pub use internal::sweep::{SweepConfig, SweepEngine, SweepPattern};
|
pub use internal::sweep::{SweepConfig, SweepEngine, SweepPattern};
|
||||||
pub use internal::transport::{
|
pub use internal::transport::{
|
||||||
A40Error, A40Transport, VendorFaults, VendorFaultsSnapshot, DEFAULT_COMMAND_DEADLINE,
|
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
|
/// vendor has acknowledged via a T1_F1_B1_D1 reply (its standard
|
||||||
/// angle-feedback frame) or the bounded retry budget exhausts.
|
/// angle-feedback frame) or the bounded retry budget exhausts.
|
||||||
pub async fn set_pose(&self, command: GimbalCommand) -> Result<()> {
|
pub async fn set_pose(&self, command: GimbalCommand) -> Result<()> {
|
||||||
let transport = self.transport.as_ref().ok_or(AutopilotError::NotImplemented(
|
let transport = self
|
||||||
"gimbal_controller::set_pose: no transport wired",
|
.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 data = build_a1_angles(command.yaw_deg, command.pitch_deg);
|
||||||
let _reply = transport
|
let _reply = transport
|
||||||
.send_with_response(FrameId::A1, &data, FrameId::T1F1B1D1)
|
.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
|
/// protocol. The continuous-rate C1 ZOOM_IN / ZOOM_OUT pair is
|
||||||
/// reserved for AZ-654's sweep primitive.
|
/// reserved for AZ-654's sweep primitive.
|
||||||
pub async fn zoom(&self, level: f32) -> Result<()> {
|
pub async fn zoom(&self, level: f32) -> Result<()> {
|
||||||
let transport = self.transport.as_ref().ok_or(AutopilotError::NotImplemented(
|
let transport = self
|
||||||
"gimbal_controller::zoom: no transport wired",
|
.transport
|
||||||
))?;
|
.as_ref()
|
||||||
|
.ok_or(AutopilotError::NotImplemented(
|
||||||
|
"gimbal_controller::zoom: no transport wired",
|
||||||
|
))?;
|
||||||
let data = build_c2_set_zoom(level);
|
let data = build_c2_set_zoom(level);
|
||||||
// C2 SET_EO_ZOOM ack arrives as a T1_F1_B1_D1 (the vendor's
|
// C2 SET_EO_ZOOM ack arrives as a T1_F1_B1_D1 (the vendor's
|
||||||
// generic angle/status feedback frame).
|
// generic angle/status feedback frame).
|
||||||
|
|||||||
@@ -78,9 +78,22 @@ async fn az656_set_pose_publishes_monotonic_timestamp() {
|
|||||||
}
|
}
|
||||||
|
|
||||||
// Assert
|
// Assert
|
||||||
assert!(timestamps[0] > 0, "initial stamp should be > 0 after first set_pose");
|
assert!(
|
||||||
assert!(timestamps[1] > timestamps[0], "ts not monotonic: {} → {}", timestamps[0], timestamps[1]);
|
timestamps[0] > 0,
|
||||||
assert!(timestamps[2] > timestamps[1], "ts not monotonic: {} → {}", timestamps[1], timestamps[2]);
|
"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
|
/// AZ-655 integration — load a plan and exercise the executor against
|
||||||
|
|||||||
Reference in New Issue
Block a user