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) data = json.loads(body)
mode = data.get('mode', '') mode = data.get('mode', '')
except json.JSONDecodeError: except json.JSONDecodeError:
self.send_response(400) self._json_response(400, {'code': -1, 'message': 'invalid json'})
self.end_headers()
self.wfile.write(b'{"error":"invalid json"}')
return return
valid_modes = ('auto', 'manual', 'debug') valid_modes = ('auto', 'manual', 'debug')
if mode not in valid_modes: if mode not in valid_modes:
self.send_response(400) self._json_response(400, {
self.end_headers() 'code': -1,
self.wfile.write( 'message': f'unknown mode: {mode}',
f'{{"error":"invalid mode, must be one of {valid_modes}"}}' })
.encode()
)
return return
if _mode_publisher is not None: if _mode_publisher is not None:
@@ -55,10 +51,7 @@ class _APIHandler(BaseHTTPRequestHandler):
msg.data = mode msg.data = mode
_mode_publisher.publish(msg) _mode_publisher.publish(msg)
self.send_response(200) self._json_response(200, {'code': 0, 'message': 'ok'})
self.send_header('Content-type', 'application/json')
self.end_headers()
self.wfile.write(f'{{"status":"ok","mode":"{mode}"}}'.encode())
def _handle_laser(self): def _handle_laser(self):
content_length = int(self.headers.get('Content-Length', 0)) content_length = int(self.headers.get('Content-Length', 0))
@@ -67,9 +60,7 @@ class _APIHandler(BaseHTTPRequestHandler):
data = json.loads(body) data = json.loads(body)
action = data.get('action', '') action = data.get('action', '')
except json.JSONDecodeError: except json.JSONDecodeError:
self.send_response(400) self._json_response(400, {'code': -1, 'message': 'invalid json'})
self.end_headers()
self.wfile.write(b'{"error":"invalid json"}')
return return
if action == 'on': if action == 'on':
@@ -77,9 +68,10 @@ class _APIHandler(BaseHTTPRequestHandler):
elif action == 'off': elif action == 'off':
mode = 'laser_off' mode = 'laser_off'
else: else:
self.send_response(400) self._json_response(400, {
self.end_headers() 'code': -1,
self.wfile.write(b'{"error":"action must be on or off"}') 'message': 'action must be on or off',
})
return return
if _mode_publisher is not None: if _mode_publisher is not None:
@@ -87,10 +79,13 @@ class _APIHandler(BaseHTTPRequestHandler):
msg.data = mode msg.data = mode
_mode_publisher.publish(msg) _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.send_header('Content-type', 'application/json')
self.end_headers() 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): def handle_error(self, request, client_address):
pass pass
@@ -66,15 +66,15 @@ class GimbalNode(Node):
self.get_logger().error(f'Serial write error: {e}') self.get_logger().error(f'Serial write error: {e}')
def _cmd_gimbal_callback(self, msg: Twist): def _cmd_gimbal_callback(self, msg: Twist):
yaw_deg = msg.angular.z yaw_vel = float(msg.angular.z)
pitch_deg = msg.angular.y pitch_vel = float(msg.angular.y)
self.get_logger().debug( 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 = bytearray()
data.append(0xA6) data.append(0xA6)
data.append(0) # zero_force = 0 data.extend(struct.pack('<ff', yaw_vel, pitch_vel))
data.extend(struct.pack('<ff', float(yaw_deg), float(pitch_deg)))
data.append(_checksum(data)) data.append(_checksum(data))
self._write_frame(data) self._write_frame(data)
@@ -95,10 +95,10 @@ class GimbalNode(Node):
self.get_logger().error(f'Invalid PID JSON: {e}') self.get_logger().error(f'Invalid PID JSON: {e}')
return return
v_yaw = pid.get('vision_yaw', {}) v_yaw = pid.get('visual_yaw', {})
v_pitch = pid.get('vision_pitch', {}) v_pitch = pid.get('visual_pitch', {})
m_yaw = pid.get('motor_yaw', {}) m_yaw = pid.get('angle_yaw', {})
m_pitch = pid.get('motor_pitch', {}) m_pitch = pid.get('angle_pitch', {})
self.get_logger().info('Sending PID debug tuning frame') self.get_logger().info('Sending PID debug tuning frame')
data = bytearray() data = bytearray()
@@ -52,10 +52,9 @@ def test_manual_control_frame():
yaw, pitch = 30.0, -15.0 yaw, pitch = 30.0, -15.0
data = bytearray() data = bytearray()
data.append(0xA6) data.append(0xA6)
data.append(0)
data.extend(struct.pack('<ff', yaw, pitch)) data.extend(struct.pack('<ff', yaw, pitch))
data.append(_checksum(data)) data.append(_checksum(data))
assert len(data) == 11 assert len(data) == 10
assert data[0] == 0xA6 assert data[0] == 0xA6