fix: align gimbal protocol and API responses with updated docs

- 0xA6 frame: remove zero_force byte, use angular velocity directly (10 bytes)
- PID keys: vision_* -> visual_*, motor_* -> angle_*
- HTTP API responses: use {"code": 0, "message": "ok"} format
This commit is contained in:
dev01
2026-06-02 18:07:57 +08:00
parent ede16af126
commit a2acd62e9d
3 changed files with 26 additions and 32 deletions
@@ -35,19 +35,15 @@ class _APIHandler(BaseHTTPRequestHandler):
data = json.loads(body)
mode = data.get('mode', '')
except json.JSONDecodeError:
self.send_response(400)
self.end_headers()
self.wfile.write(b'{"error":"invalid json"}')
self._json_response(400, {'code': -1, 'message': 'invalid json'})
return
valid_modes = ('auto', 'manual', 'debug')
if mode not in valid_modes:
self.send_response(400)
self.end_headers()
self.wfile.write(
f'{{"error":"invalid mode, must be one of {valid_modes}"}}'
.encode()
)
self._json_response(400, {
'code': -1,
'message': f'unknown mode: {mode}',
})
return
if _mode_publisher is not None:
@@ -55,10 +51,7 @@ class _APIHandler(BaseHTTPRequestHandler):
msg.data = mode
_mode_publisher.publish(msg)
self.send_response(200)
self.send_header('Content-type', 'application/json')
self.end_headers()
self.wfile.write(f'{{"status":"ok","mode":"{mode}"}}'.encode())
self._json_response(200, {'code': 0, 'message': 'ok'})
def _handle_laser(self):
content_length = int(self.headers.get('Content-Length', 0))
@@ -67,9 +60,7 @@ class _APIHandler(BaseHTTPRequestHandler):
data = json.loads(body)
action = data.get('action', '')
except json.JSONDecodeError:
self.send_response(400)
self.end_headers()
self.wfile.write(b'{"error":"invalid json"}')
self._json_response(400, {'code': -1, 'message': 'invalid json'})
return
if action == 'on':
@@ -77,9 +68,10 @@ class _APIHandler(BaseHTTPRequestHandler):
elif action == 'off':
mode = 'laser_off'
else:
self.send_response(400)
self.end_headers()
self.wfile.write(b'{"error":"action must be on or off"}')
self._json_response(400, {
'code': -1,
'message': 'action must be on or off',
})
return
if _mode_publisher is not None:
@@ -87,10 +79,13 @@ class _APIHandler(BaseHTTPRequestHandler):
msg.data = mode
_mode_publisher.publish(msg)
self.send_response(200)
self._json_response(200, {'code': 0, 'message': 'ok'})
def _json_response(self, status, data):
self.send_response(status)
self.send_header('Content-type', 'application/json')
self.end_headers()
self.wfile.write(f'{{"status":"ok","laser":"{action}"}}'.encode())
self.wfile.write(json.dumps(data).encode())
def handle_error(self, request, client_address):
pass
@@ -66,15 +66,15 @@ class GimbalNode(Node):
self.get_logger().error(f'Serial write error: {e}')
def _cmd_gimbal_callback(self, msg: Twist):
yaw_deg = msg.angular.z
pitch_deg = msg.angular.y
yaw_vel = float(msg.angular.z)
pitch_vel = float(msg.angular.y)
self.get_logger().debug(
f'Manual control: yaw={yaw_deg:.2f}, pitch={pitch_deg:.2f}'
f'Manual control: yaw_vel={yaw_vel:.2f} deg/s, '
f'pitch_vel={pitch_vel:.2f} deg/s'
)
data = bytearray()
data.append(0xA6)
data.append(0) # zero_force = 0
data.extend(struct.pack('<ff', float(yaw_deg), float(pitch_deg)))
data.extend(struct.pack('<ff', yaw_vel, pitch_vel))
data.append(_checksum(data))
self._write_frame(data)
@@ -95,10 +95,10 @@ class GimbalNode(Node):
self.get_logger().error(f'Invalid PID JSON: {e}')
return
v_yaw = pid.get('vision_yaw', {})
v_pitch = pid.get('vision_pitch', {})
m_yaw = pid.get('motor_yaw', {})
m_pitch = pid.get('motor_pitch', {})
v_yaw = pid.get('visual_yaw', {})
v_pitch = pid.get('visual_pitch', {})
m_yaw = pid.get('angle_yaw', {})
m_pitch = pid.get('angle_pitch', {})
self.get_logger().info('Sending PID debug tuning frame')
data = bytearray()
@@ -52,10 +52,9 @@ def test_manual_control_frame():
yaw, pitch = 30.0, -15.0
data = bytearray()
data.append(0xA6)
data.append(0)
data.extend(struct.pack('<ff', yaw, pitch))
data.append(_checksum(data))
assert len(data) == 11
assert len(data) == 10
assert data[0] == 0xA6