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:
@@ -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
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user