Commit ·
ad7b805
1
Parent(s): 1b0c406
fix: Invert DOA yaw direction - was turning opposite way
Browse files
reachy_mini_ha_voice/satellite.py
CHANGED
|
@@ -683,7 +683,12 @@ class VoiceSatelliteProtocol(APIServer):
|
|
| 683 |
# In robot frame: Y+ is left, so yaw = -atan2(dir_y, dir_x)
|
| 684 |
# But since dir_x = sin(doa), dir_y = cos(doa):
|
| 685 |
# yaw = -atan2(cos(doa), sin(doa)) = -(π/2 - doa) = doa - π/2
|
| 686 |
-
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 687 |
yaw_deg = math.degrees(yaw_rad)
|
| 688 |
|
| 689 |
_LOGGER.info("DOA direction: x=%.2f, y=%.2f, yaw=%.1f°",
|
|
|
|
| 683 |
# In robot frame: Y+ is left, so yaw = -atan2(dir_y, dir_x)
|
| 684 |
# But since dir_x = sin(doa), dir_y = cos(doa):
|
| 685 |
# yaw = -atan2(cos(doa), sin(doa)) = -(π/2 - doa) = doa - π/2
|
| 686 |
+
#
|
| 687 |
+
# CORRECTION: The above was inverted. Testing shows:
|
| 688 |
+
# - Sound on left → robot turns right (wrong)
|
| 689 |
+
# - Sound on right → robot turns left (wrong)
|
| 690 |
+
# So we need to negate the yaw: yaw = π/2 - doa
|
| 691 |
+
yaw_rad = math.pi / 2 - angle_rad
|
| 692 |
yaw_deg = math.degrees(yaw_rad)
|
| 693 |
|
| 694 |
_LOGGER.info("DOA direction: x=%.2f, y=%.2f, yaw=%.1f°",
|