Unitree 4. 遥控器解码

black0moonlight

已于 2025-02-10 17:55:49 修改

阅读量229

点赞数 3

分类专栏： Unitree 文章标签：嵌入式硬件 python

于 2025-02-10 17:16:02 首次发布

本文链接：https://blog.youkuaiyun.com/black0moonlight/article/details/145554212

版权

Unitree 专栏收录该内容

8 篇文章

订阅专栏

查看官方库说明：

/*****************************************************************
Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved.
*****************************************************************/
#ifndef _UNITREE_LEGGED_JOYSTICK_H_
#define _UNITREE_LEGGED_JOYSTICK_H_

#include <stdint.h>
// 16b
typedef union {
  struct {
    uint8_t R1 : 1;
    uint8_t L1 : 1;
    uint8_t start : 1;
    uint8_t select : 1;
    uint8_t R2 : 1;
    uint8_t L2 : 1;
    uint8_t F1 : 1;
    uint8_t F2 : 1;
    uint8_t A : 1;
    uint8_t B : 1;
    uint8_t X : 1;
    uint8_t Y : 1;
    uint8_t up : 1;
    uint8_t right : 1;
    uint8_t down : 1;
    uint8_t left : 1;
  } components;
  uint16_t value;
} xKeySwitchUnion;

// 40 Byte (now used 24B)
typedef struct {
  uint8_t head[2];
  xKeySwitchUnion btn;
  float lx;
  float rx;
  float ry;
  float L2;
  float ly;

  uint8_t idle[16];
} xRockerBtnDataStruct;

#endif  // _UNITREE_LEGGED_JOYSTICK_H_

首先可以确定遥控器有以下按键：

class wirelessRemote:
    buf = []
    l1_switch = 0
    l2_switch = 0
    r1_switch = 0
    r2_switch = 0
    start_switch = 0
    select_switch = 0
    f1_switch = 0
    f2_switch = 0
    left_cross_switch = [0, 0, 0, 0]
    right_cross_switch = [0, 0, 0, 0]
    left_stick = [0., 0.]
    right_stick = [0., 0.]

读取state.wirelessRemote可以得到一个40字节的数据

[85, 81, 0, 0, 64, 49, 173, 186, 30, 155, 58, 186, 97, 226, 125, 187, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 6, 0]

0，1两位为帧头，2到24位为数据帧，后续为空数据，最后两位应该是校验和。但是我没找到对应的校验方式。

其余位的数据解析如下：

        if state.wirelessRemote[:2] == [85, 81]:
            buf = bytes(state.wirelessRemote)
            ps.buf = buf

            switch_data = struct.unpack("BB", buf[2:4])
            ps.l1_switch = (switch_data[0] >> 1) & 1
            ps.l2_switch = (switch_data[0] >> 5) & 1
            ps.r1_switch = (switch_data[0] >> 0) & 1
            ps.r2_switch = (switch_data[0] >> 4) & 1
            ps.start_switch = (switch_data[0] >> 2) & 1
            ps.select_switch = (switch_data[0] >> 3) & 1
            ps.f1_switch = (switch_data[0] >> 6) & 1
            ps.f2_switch = (switch_data[0] >> 7) & 1
            ps.left_cross_switch = [(switch_data[1] >> 4) & 1, (switch_data[1] >> 6) & 1, (switch_data[1] >> 5) & 1, (switch_data[1] >> 7) & 1]
            ps.right_cross_switch = [(switch_data[1] >> 3) & 1, (switch_data[1] >> 0) & 1, (switch_data[1] >> 1) & 1, (switch_data[1] >> 1) & 1]

            ps.left_stick[0] = struct.unpack('<f', buf[4:8])[0]
            ps.right_stick[0] = struct.unpack('<f', buf[8:12])[0]
            ps.right_stick[1] = struct.unpack('<f', buf[12:16])[0]
            ps.left_stick[1] = struct.unpack('<f', buf[20:24])[0]

            # print(f"Stamp: {stamp}")
            # print(f"Left Stick: {ps.left_stick[0]:.2f}, {ps.left_stick[1]:.2f}")
            # print(f"Right Stick: {ps.right_stick[0]:.2f}, {ps.right_stick[1]:.2f}")
            # print(f"Switches: {ps.l1_switch}, {ps.l2_switch}, {ps.r1_switch}, {ps.r2_switch}")
            # print(f"Switches: {ps.left_cross_switch}, {ps.right_cross_switch}")
            # print()