Skip to content
Snippets Groups Projects
Commit de28947d authored by Paul-Winpenny's avatar Paul-Winpenny
Browse files

Removed even more comments!

parent c9359a98
No related branches found
No related tags found
No related merge requests found
...@@ -676,18 +676,29 @@ class UWBNode(Node): ...@@ -676,18 +676,29 @@ class UWBNode(Node):
dy = calib_end_position[1] - self.calib_start_position[1] dy = calib_end_position[1] - self.calib_start_position[1]
uwb_heading = math.atan2(dy, dx) uwb_heading = math.atan2(dy, dx)
offset = calib_end_yaw - uwb_heading # Convert UWB heading to degrees
self.uwb_to_imu_offset = offset uwb_heading_deg = math.degrees(uwb_heading)
# Normalize to [0, 360)
uwb_heading_deg_normalized = (uwb_heading_deg + 360) % 360
self.get_logger().info("CALIBRATION COMPLETE!") # Convert IMU yaw to degrees
self.get_logger().info(f" Start Pos (UWB): {self.calib_start_position}") calib_end_yaw_deg = math.degrees(calib_end_yaw)
self.get_logger().info(f" End Pos (UWB): {calib_end_position}")
self.get_logger().info(f" UWB heading: {uwb_heading:.3f} rad")
self.get_logger().info(f" IMU yaw: {calib_end_yaw:.3f} rad")
self.get_logger().info(f" --> OFFSET: {offset:.3f} rad")
# Calculate offset
offset_deg = calib_end_yaw_deg - uwb_heading_deg_normalized
# Normalize offset to [0, 360)
offset_deg_normalized = (offset_deg + 360) % 360
# Logging information
print("CALIBRATION COMPLETE!")
print(f" Start Pos (UWB): {self.calib_start_position}")
print(f" End Pos (UWB): {calib_end_position}")
print(f" UWB heading: {uwb_heading:.3f} rad ({uwb_heading_deg_normalized:.3f} deg)")
print(f" IMU yaw: {calib_end_yaw:.3f} rad ({calib_end_yaw_deg:.3f} deg)")
print(f" --> OFFSET: {offset_deg:.3f} deg (normalized to {offset_deg_normalized:.3f} deg)")
self.calibration_active = False self.calibration_active = False
self.mode = "Manual" self.mode = "Manual"
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment