fix speed and steering conversion on CAN input

osgar/platforms/test_yuhesen.py

@@ -21,9 +21,15 @@ def test_control_speed(self):
         robot.on_can([0x18c4d2ef, bytes.fromhex('0100700000102041'), 1])
         bus.publish.assert_called_with('can', [0x18C4D2D0, bytes.fromhex('843801000000209d'), 1])
 
+        robot.on_can([0x18c4d2ef, bytes.fromhex('843801000000209d'), 1])
+        self.assertEqual(robot.last_speed, 5000)
+
     def test_control_steering(self):
         bus = MagicMock()
         robot = FR07(bus=bus, config={})
         robot.desired_steering_angle_deg = -25.0  # deg (from user manual)
         robot.on_can([0x18c4d2ef, bytes.fromhex('0100700000102041'), 1])
         bus.publish.assert_called_with('can', [0x18C4D2D0, bytes.fromhex('0400c0630f002088'), 1])
+
+        robot.on_can([0x18c4d2ef, bytes.fromhex('0400c0630f002088'), 1])
+        self.assertEqual(robot.last_steering, -2500)

osgar/platforms/yuhesen.py

@@ -58,12 +58,12 @@ def on_can(self, data):
             # 0100e0ff0f20 d0e1
             target_gear = payload[0] & 0xF
             assert target_gear in [1, 2, 3, 4], target_gear  # P, R, N, D
-            speed = ((payload[0] & 0xF0) << 8) + (payload[1] << 4) + (payload[2] & 0x0F)  # 0.001 m/s
+            speed = ((payload[0] & 0xF0) >> 4) + (payload[1] << 4) + ((payload[2] & 0x0F) << 12)  # 0.001 m/s
             if self.last_speed != speed:
 #                print(self.time, f'speed = {speed}')
                 self.last_speed = speed
-            steering = ((payload[2] & 0xF0) << 8) + (payload[3] << 4) + (payload[4] & 0x0F)  # 0.01 deg
-
+            uint16_steering = ((payload[2] & 0xF0) >> 4) + (payload[3] << 4) + ((payload[4] & 0x0F) << 12)  # 0.01 deg
+            steering = struct.unpack('<h', struct.pack('H', uint16_steering))[0]
             # trigger command in response
 
             desired_speed_int = int(self.desired_speed * 1000)