Desmond-Dong commited on
Commit
ad7b805
·
1 Parent(s): 1b0c406

fix: Invert DOA yaw direction - was turning opposite way

Browse files
Files changed (1) hide show
  1. reachy_mini_ha_voice/satellite.py +6 -1
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
- yaw_rad = angle_rad - math.pi / 2
 
 
 
 
 
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°",