MIT SPIne 测试5

今天又进行了更细致的测试，添加了一个简单的通信协议，有CRC校验。结果如下：

spine_cmd_t: 132
spine_state_t: 84
[RT SPI] Open
0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x9D 0xFA 0xA8 0xC2

0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x9D 0xFA 0xA8 0xC2

0x00 0x00 0x80 0x3F 0x00 0x00 0x00 0x40 0x00 0x00 0x40 0x40 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x55 0xF4 0x72 0xB7
[CRC] crc correct
leg: 0 id: 0 : 1, 2, 3
leg: 0 id: 1 : 0, 0, 0
leg: 0 id: 2 : 0, 0, 0
leg: 1 id: 0 : 0, 0, 0
leg: 1 id: 1 : 0, 0, 0
leg: 1 id: 2 : 0, 0, 0

废话不多说，直接上代码：

定义的数据结构：

#pragma once

#include <stdint.h>

#define ABAD 0
#define HIP 1
#define KNEE 2

#define RF 0
#define LF 1
#define RH 2
#define LH 3

#pragma pack(1)

// 24 byte
typedef struct
{
    float p_des;
    float v_des;
    float kp;
    float kd;
    float t_ff;
} motor_cmd_t;

// 12 byte
typedef struct 
{
    float p;
    float v;
    float t;
} motor_data_t;

// 64 byte
typedef struct
{
    motor_cmd_t cmd[3];
    int32_t flag;
} leg_cmd_t;

// 40 byte
typedef struct
{
    motor_data_t state[3];
    int32_t flag;
} leg_state_t;

// 132 byte
typedef struct
{
    leg_cmd_t cmd[2];
    uint32_t crc;
} spine_cmd_t;

// 84 byte
typedef struct
{
    leg_state_t state[2];
    uint32_t crc;
} spine_state_t;

#pragma pack()

typedef struct
{
    leg_cmd_t cmd[4];
} robot_cmd_t;

typedef struct
{
    leg_state_t state[4];
} robot_state_t;

STM32的中断回调函数：

spine_cmd_t   g_cmd;
spine_state_t g_state;

int state = 1;
uint8_t tx[140] = {0};
uint8_t rx[140] = {0};

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
  if(GPIO_Pin == SPI_CS_Pin)
  {
    HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
    HAL_SPI_TransmitReceive(&hspi1, tx, rx, 132, 100);

    hspi1.Instance->DR = 0x00;

    // decode rx
    memcpy(&g_cmd, rx, sizeof(spine_cmd_t));
    uint32_t crc = calculate((uint8_t*)&g_cmd, sizeof(spine_cmd_t) - 4);
    if(crc != g_cmd.crc)
    {
      //HAL_UART_Transmit(&huart2, "crc error", 10, 10000);
      HAL_UART_Transmit(&huart2, (uint8_t*) &g_cmd, sizeof(spine_cmd_t), 10000);
    }
    // copy data to tx
    g_state.state[0].state[0].p = 1;
    g_state.state[0].state[0].v = 2;
    g_state.state[0].state[0].t = 3;
    g_state.crc = calculate((uint8_t*)&g_state, sizeof(spine_state_t) - 4);
    memcpy(tx, &g_state, sizeof(spine_state_t));

    HAL_UART_Transmit(&huart2, tx, sizeof(spine_state_t), 10000);
    // trigger event to main task
  }
}

上位机的测试代码：

#include "rt_spi.h"
#include "motor_control.h"
#include "crc.h"

#include <cmath>
#include <iostream>
#include <unistd.h>

int spi_1_fd = -1;
int spi_2_fd = -1;

const char* name1 = "/dev/spidev0.0";
const char* name2 = "/dev/spidev0.1";

spine_cmd_t   cmd[2];
spine_state_t state[2];

using namespace std;

int main()
{
    cout << "spine_cmd_t: " << sizeof(spine_cmd_t) << endl;
    cout << "spine_state_t: " << sizeof(spine_state_t) << endl;

    create_lookup_table();

    spi_open(spi_1_fd, name1);

    uint8_t tx[sizeof(spine_cmd_t)] = {0};
    uint8_t rx[sizeof(spine_cmd_t)] = {0};

    uint16_t* tx_buf = (uint16_t *)tx;
    uint16_t* rx_buf = (uint16_t *)rx;

    uint16_t *cmd_d = (uint16_t *)&cmd[0];
    uint16_t *data_d = (uint16_t *)&state[0];

    cmd[0].crc = calculate((uint8_t*)&cmd[0], sizeof(spine_cmd_t) - 4);

    auto str = hex2str((uint8_t*)&cmd[0], sizeof(spine_cmd_t));
    cout << str << endl << endl;

    memcpy(tx, &cmd[0], sizeof(spine_cmd_t));
    //for (int i = 0; i < sizeof(spine_cmd_t) / 2; i++)
    //    tx_buf[i] = (cmd_d[i] >> 8) + ((cmd_d[i] & 0xff) << 8);

    str = hex2str(tx, sizeof(spine_cmd_t));
    cout << str << endl << endl;

    int rv = transfer(spi_1_fd, tx, rx, sizeof(spine_cmd_t));
    (void)rv;

    memcpy(&state[0], rx, sizeof(spine_state_t));
    //for (int i = 0; i < sizeof(spine_state_t) / 2; i++)
    //    data_d[i] = (rx_buf[i] >> 8) + ((rx_buf[i] & 0xff) << 8);

    str = hex2str((uint8_t*)&state[0], sizeof(spine_state_t));
    cout << str << endl;

    uint32_t crc = calculate((uint8_t*)&state[0], sizeof(spine_state_t) - 4);

    if(crc == state[0].crc)
    {
        cout << "[CRC] crc correct" << endl;
    }
    else
    {
        cout << "[CRC] crc error" << endl;
    }

    for(int i = 0; i < 2; ++i)
    {
        leg_state_t& s = state[0].state[i];
        for(int j = 0; j < 3; ++j)
        {
            motor_data_t& m = s.state[j];
            cout << "leg: " << i << " id: " << j << " : " << m.p << ", " << m.v << ", " << m.t << endl;
        }
    }

    spi_close(spi_1_fd);

    return 0;
}

以及最后的CRC校验函数：

#include "crc.h"

#include <stdlib.h>
#include <stdio.h>
#include <string.h>

#define LUT_SIZE 256

static uint32_t lookup_table[LUT_SIZE] = {0};
static uint32_t polynomial = 0x04C11DB7;
static uint32_t init_value = 0xFFFFFFFF;
static uint32_t xor_output = 0xFFFFFFFF;
static uint8_t  reflect_input = 1;
static uint8_t  reflect_output = 1;
static uint8_t  use_lut = 1;

static const uint8_t REFLECT_BIT_ORDER_TABLE[256] = {
    0x00, 0x80, 0x40, 0xC0, 0x20, 0xA0, 0x60, 0xE0, 0x10, 0x90, 0x50, 0xD0, 0x30, 0xB0, 0x70, 0xF0, 0x08, 0x88, 0x48, 0xC8, 0x28, 0xA8, 0x68, 0xE8, 0x18, 0x98,
    0x58, 0xD8, 0x38, 0xB8, 0x78, 0xF8, 0x04, 0x84, 0x44, 0xC4, 0x24, 0xA4, 0x64, 0xE4, 0x14, 0x94, 0x54, 0xD4, 0x34, 0xB4, 0x74, 0xF4, 0x0C, 0x8C, 0x4C, 0xCC,
    0x2C, 0xAC, 0x6C, 0xEC, 0x1C, 0x9C, 0x5C, 0xDC, 0x3C, 0xBC, 0x7C, 0xFC, 0x02, 0x82, 0x42, 0xC2, 0x22, 0xA2, 0x62, 0xE2, 0x12, 0x92, 0x52, 0xD2, 0x32, 0xB2,
    0x72, 0xF2, 0x0A, 0x8A, 0x4A, 0xCA, 0x2A, 0xAA, 0x6A, 0xEA, 0x1A, 0x9A, 0x5A, 0xDA, 0x3A, 0xBA, 0x7A, 0xFA, 0x06, 0x86, 0x46, 0xC6, 0x26, 0xA6, 0x66, 0xE6,
    0x16, 0x96, 0x56, 0xD6, 0x36, 0xB6, 0x76, 0xF6, 0x0E, 0x8E, 0x4E, 0xCE, 0x2E, 0xAE, 0x6E, 0xEE, 0x1E, 0x9E, 0x5E, 0xDE, 0x3E, 0xBE, 0x7E, 0xFE, 0x01, 0x81,
    0x41, 0xC1, 0x21, 0xA1, 0x61, 0xE1, 0x11, 0x91, 0x51, 0xD1, 0x31, 0xB1, 0x71, 0xF1, 0x09, 0x89, 0x49, 0xC9, 0x29, 0xA9, 0x69, 0xE9, 0x19, 0x99, 0x59, 0xD9,
    0x39, 0xB9, 0x79, 0xF9, 0x05, 0x85, 0x45, 0xC5, 0x25, 0xA5, 0x65, 0xE5, 0x15, 0x95, 0x55, 0xD5, 0x35, 0xB5, 0x75, 0xF5, 0x0D, 0x8D, 0x4D, 0xCD, 0x2D, 0xAD,
    0x6D, 0xED, 0x1D, 0x9D, 0x5D, 0xDD, 0x3D, 0xBD, 0x7D, 0xFD, 0x03, 0x83, 0x43, 0xC3, 0x23, 0xA3, 0x63, 0xE3, 0x13, 0x93, 0x53, 0xD3, 0x33, 0xB3, 0x73, 0xF3,
    0x0B, 0x8B, 0x4B, 0xCB, 0x2B, 0xAB, 0x6B, 0xEB, 0x1B, 0x9B, 0x5B, 0xDB, 0x3B, 0xBB, 0x7B, 0xFB, 0x07, 0x87, 0x47, 0xC7, 0x27, 0xA7, 0x67, 0xE7, 0x17, 0x97,
    0x57, 0xD7, 0x37, 0xB7, 0x77, 0xF7, 0x0F, 0x8F, 0x4F, 0xCF, 0x2F, 0xAF, 0x6F, 0xEF, 0x1F, 0x9F, 0x5F, 0xDF, 0x3F, 0xBF, 0x7F, 0xFF};

uint32_t reflect(uint32_t value)
{
    uint32_t reflected = 0;
    for (int i = 0; i < 32; i++)
    {
        if (value & 0x01)
            reflected |= (1 << ((32 - 1) - i));
        value = (value >> 1);
    }
    return reflected;
}

void create_lookup_table()
{
    memset(lookup_table, 0, LUT_SIZE * sizeof(uint32_t));
    uint32_t x = 0;
    for (int i = 0; i < LUT_SIZE; i++)
    {
        x = (uint32_t)(i << 24);
        for (int j = 0; j < 8; j++)
        {
            x = x & 0x80000000 ? (x << 1) ^ polynomial : x << 1;
        }
        lookup_table[i] = x;
    }
}

uint32_t calculate(const uint8_t* data, const int length)
{
    uint32_t crc  = init_value;
    uint8_t  byte = 0;

    if (use_lut)
    {
        for (size_t i = 0; i < length; i++)
        {
            byte = reflect_input ? REFLECT_BIT_ORDER_TABLE[data[i]] : data[i];
            crc  = (crc << 8) ^ lookup_table[(crc >> 24) ^ byte];
        }
    }
    else
    {
        for (size_t i = 0; i < length; i++)
        {
            byte = reflect_input ? REFLECT_BIT_ORDER_TABLE[data[i]] : data[i];
            crc ^= (byte << 24);
            for (int j = 0; j < 8; j++)
            {
                crc = crc & 0x80000000 ? (crc << 1) ^ polynomial : crc << 1;
            }
        }
    }

    if (reflect_output)
    {
        crc = reflect(crc);
    }

    crc = (crc ^ xor_output) & 0xFFFFFFFF;
    return crc;
}

如果觉得文章对你有用，请随意赞赏

MIT SPIne 测试5

https://rocketsky.top/archives/mitspine-ce-shi-5

作者

Rocket Sky

发布于

2023-05-15

更新于

2023-06-13

许可协议

CC BY 4.0