四足机器人伺服驱动可靠性提升?优化SPI-DMA通信

1 原因(实时的驱动通讯才能保证实物与仿真的一致性)

众所周知对于四足机器人的控制系统来说,首当其冲的是底层驱动与控制软硬件部分,由于四足机器人需要进行试试的力矩控制,关节自由度也很多因此其对底层控制带宽有严格的要求,目前常用的底层驱动频率是1Khz,采用如此高评论的原因是:

(1)更高的控制频率利于摆动关节的控制,由于目前四足机器人采用的电机大多是准支驱(减速比再9以内),虽然减速比不高但是要对其进行精确的摆动控制如果控制频率不够高那其响应和关节刚度可能达不到预期;

(2)更高的控制频率意味这更高的力控带宽,提供机器人在高动态和扰动下的稳定性;

(3)1Khz的底层驱动频率保证了关节状态采集的连续和实时性,对后续滤波处理提供良好的数据基础;

那么高的控制频率意味着高容量、高速、高实时的底层通信控制,目前来说要实现稳定的底层通信伺服控制方案还是很难的而且没有很多可参考的例子。目前国内许多四足机器人参考的伺服驱动方案都是MIT开源的Spine SPI-CAN转换方案,其采用两个STM32F4来实现4路CAN的驱动,每路CAN驱动一条腿的3个电机,两个STM32之间采用内部SPI通信,主STM32采用另一路SPI与Upboard通信,MOCO四足机器人参考了MIT的底层驱动结构但是考虑到实用性仅使用了一个STM32下挂两路CAN来实现底层驱动,同时结合飞控的相关经历,把IMU和一些扩接口也集成到MOCO-PIhat上,相应资料可以参考:

回到正文本片文章记录了我开发SPI与Linux驱动的过程,在上半年中采用正点原子的SPI驱动例程我首先开发了基于轮询的方案,但是由于SPI中断太频繁,导致其总体通信频率不是太高,对比与早期采用STM32直接控制的效果其在姿态控制精度有明显变化;因此,后续又研究了基于DMA实现的全双工通信,将SPI的通信速率大大提供,每0.4ms传输了100个字节的数据,将机器人需要的姿态、角速度、加速度、各关节角度、力矩反馈、系统状态和扩展数据实现的高速的传输,至此基于Linux实时控制器的效果基本与单片机版本一致,但实际可以看出很多时候还是有些微的差距,这可能还是由于RT patch的软实时性还是有问题造成的,这个版本的驱动资料如下:

上图为MOCO-PIhat的原理图,通过SPI通信实现对底层电机的驱动,但在实际实验中发现该方案虽然保证了机器人较好的控制效果,但在通信中首先会不定时出现协议校验码错误,另外当连续出现一段时间后SPI就会卡死导致底层通信线程故障,该问题出现的频率不确定有时候可能连续运行10min也不会出现,而有时候可能机器人一站立就出现导致其摔倒,为此我进行了一系列的测试但是由于该故障出现时没有任何的错误提示导致无法判断问题在哪,因此同期开展了基于USB虚拟串口的通信,即采用VCP虚拟串口与Linux控制器通信,采用如下的Seril库实现了1ms的通信(我也移植了ROS下的serial库但是其无法实现1ms的读写):

/*************************************************************
    FileName : serialport.c
    FileFunc : 定义实现文件
    Version  : V0.1
    Author   : Sunrier
    Date     : 2012-06-13
    Descp    : Linux下实现串口库
*************************************************************/
/*#include "serialport.h"*/
#include <stdio.h>
#include <strings.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>

int open_port(int iPortNumber)
{
    int fd = -1;
    char *pDev[]={"/dev/ttyACM0","/dev/ttyS1","/dev/ttyS2","/dev/ttyS3",
                                "/dev/ttyS4","/dev/ttyS5","/dev/ttyS6","/dev/ttyS7",
                                "/dev/ttyS8","/dev/ttyS9","/dev/ttyS10","/dev/ttyS11"};

    switch( iPortNumber )
    {
        case	1:
        case	2:
        case	3:
        case	4:
        case	5:
        case  6:
        case  7:
        case  8:
        case  9:
        case  10:
        case  11:
        case  12:
        fd = open(pDev[iPortNumber-1],O_RDWR|O_NOCTTY|O_NDELAY);
        if( fd<0 )
        {
            perror("Can't Open Serial Port !");
            return (-1);
        }
        else
        {
            printf("Open ttyS%d ......\n",iPortNumber-1);
        }
        break;
        default:
                        /*perror("Don't exist iPortNumber !");*/
                        printf("Don't exist iPortNumber%d under /dev/? !\n",iPortNumber);
                        return (-1);
    }

    if( fcntl(fd,F_SETFL,0)<0 )/*恢复串口的状态为阻塞状态,用于等待串口数据的读入*/
    {
        printf("fcntl failed !\n");
        return (-1);
    }
    else
    {
        printf("fcntl = %d !\n",fcntl(fd,F_SETFL,0));
    }

    /*测试打开的文件描述符是否应用一个终端设备,以进一步确认串口是否正确打开*/
    if( !isatty(STDIN_FILENO) )
    {
        printf("Standard input isn't a terminal device !\n");
        return (-1);
    }
    else
    {
        printf("It's a serial terminal device!\n");
    }

    printf("open_port file ID= %d !\n",fd);

    return fd;

}

int set_port(int fd,int iBaudRate,int iDataSize,char cParity,int iStopBit)
{
    int iResult = 0;
    struct termios oldtio,newtio;


    iResult = tcgetattr(fd,&oldtio);/*保存原先串口配置*/
    if( iResult )
    {
        perror("Can't get old terminal description !");
        return (-1);
    }


    bzero(&newtio,sizeof(newtio));
    newtio.c_cflag |= CLOCAL | CREAD;/*设置本地连接和接收使用*/

    /*设置输入输出波特率*/
    switch( iBaudRate )
    {
        case 2400:
                            cfsetispeed(&newtio,B2400);
                            cfsetospeed(&newtio,B2400);
                            break;
        case 4800:
                            cfsetispeed(&newtio,B4800);
                            cfsetospeed(&newtio,B4800);
                            break;
        case 9600:
                            cfsetispeed(&newtio,B9600);
                            cfsetospeed(&newtio,B9600);
                            break;
        case 19200:
                            cfsetispeed(&newtio,B19200);
                            cfsetospeed(&newtio,B19200);
                            break;
        case 38400:
                            cfsetispeed(&newtio,B38400);
                            cfsetospeed(&newtio,B38400);
                            break;
        case 57600:
                            cfsetispeed(&newtio,B57600);
                            cfsetospeed(&newtio,B57600);
                            break;
        case 115200:
                            cfsetispeed(&newtio,B115200);
                            cfsetospeed(&newtio,B115200);
                            break;
        case 460800:
                            cfsetispeed(&newtio,B460800);
                            cfsetospeed(&newtio,B460800);
                            break;
        case 1000000:
                            cfsetispeed(&newtio,B1000000);
                            cfsetospeed(&newtio,B1000000);
                            break;
        case 2000000:
                            cfsetispeed(&newtio,B4000000);
                            cfsetospeed(&newtio,B4000000);
                            break;
        default		:
                            /*perror("Don't exist iBaudRate !");*/
                            printf("Don't exist iBaudRate %d !\n",iBaudRate);
                            return (-1);
    }

    /*设置数据位*/
    newtio.c_cflag &= (~CSIZE);
    switch( iDataSize )
    {
        case	7:
                        newtio.c_cflag |= CS7;
                        break;
        case	8:
                        newtio.c_cflag |= CS8;
                        break;
        default:
                        /*perror("Don't exist iDataSize !");*/
                        printf("Don't exist iDataSize %d !\n",iDataSize);
                        return (-1);
    }

    /*设置校验位*/
    switch( cParity )
    {
        case	'N':					/*无校验*/
                            newtio.c_cflag &= (~PARENB);
                            break;
        case	'O':					/*奇校验*/
                            newtio.c_cflag |= PARENB;
                            newtio.c_cflag |= PARODD;
                            newtio.c_iflag |= (INPCK | ISTRIP);
                            break;
        case	'E':					/*偶校验*/
                            newtio.c_cflag |= PARENB;
                            newtio.c_cflag &= (~PARODD);
                            newtio.c_iflag |= (INPCK | ISTRIP);
                            break;
        default:
                            /*perror("Don't exist cParity  !");*/
                            printf("Don't exist cParity %c !\n",cParity);
                            return (-1);
    }

    /*设置停止位*/
    switch( iStopBit )
    {
        case	1:
                        newtio.c_cflag &= (~CSTOPB);
                        break;
        case	2:
                        newtio.c_cflag |= CSTOPB;
                        break;
        default:
                        /*perror("Don't exist iStopBit !");*/
                        printf("Don't exist iStopBit %d !\n",iStopBit);
                        return (-1);
    }

    newtio.c_cc[VTIME] = 1;	/*设置等待时间*/
    newtio.c_cc[VMIN] = 1;	/*设置最小字符*/
    tcflush(fd,TCIFLUSH);		/*刷新输入队列(TCIOFLUSH为刷新输入输出队列)*/
    iResult = tcsetattr(fd,TCSANOW,&newtio);	/*激活新的设置使之生效,参数TCSANOW表示更改立即发生*/

    if( iResult )
    {
        perror("Set new terminal description error !");
        return (-1);
    }

    printf("set_port success !\n");

    return 0;
}

int read_port(int fd,void *buf,int iByte)
{
    int iLen = 0;
    if( !iByte )
    {
        printf("Read byte number error !\n");
        return iLen;
    }

    iLen = read(fd,buf,iByte);

    return iLen;
}

int write_port(int fd,void *buf,int iByte)
{
    int iLen = 0;
    if( !iByte )
    {
        printf("Write byte number error !\n");
        return iLen;
    }

    iLen = write(fd,buf,iByte);

    return iLen;
}


int close_port(int fd)
{
    int iResult = -1;

    iResult = close(fd);

    return iResult;
}

/*!
 * \file serial/serial.h
 * \author  William Woodall <wjwwood@gmail.com>
 * \author  John Harrison   <ash.gti@gmail.com>
 * \version 0.1
 *
 * \section LICENSE
 *
 * The MIT License
 *
 * Copyright (c) 2012 William Woodall
 *
 * Permission is hereby granted, free of charge, to any person obtaining a
 * copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
 * DEALINGS IN THE SOFTWARE.
 *
 * \section DESCRIPTION
 *
 * This provides a cross platform interface for interacting with Serial Ports.
 */

#ifndef SERIAL_H
#define SERIAL_H

#include <limits>
#include <vector>
#include <string>
#include <cstring>
#include <sstream>
#include <exception>
#include <stdexcept>
#include <serial/v8stdint.h>

#define THROW(exceptionClass, message) throw exceptionClass(__FILE__, \
__LINE__, (message) )

namespace serial {

/*!
 * Enumeration defines the possible bytesizes for the serial port.
 */
typedef enum {
  fivebits = 5,
  sixbits = 6,
  sevenbits = 7,
  eightbits = 8
} bytesize_t;

/*!
 * Enumeration defines the possible parity types for the serial port.
 */
typedef enum {
  parity_none = 0,
  parity_odd = 1,
  parity_even = 2,
  parity_mark = 3,
  parity_space = 4
} parity_t;

/*!
 * Enumeration defines the possible stopbit types for the serial port.
 */
typedef enum {
  stopbits_one = 1,
  stopbits_two = 2,
  stopbits_one_point_five
} stopbits_t;

/*!
 * Enumeration defines the possible flowcontrol types for the serial port.
 */
typedef enum {
  flowcontrol_none = 0,
  flowcontrol_software,
  flowcontrol_hardware
} flowcontrol_t;

/*!
 * Structure for setting the timeout of the serial port, times are
 * in milliseconds.
 *
 * In order to disable the interbyte timeout, set it to Timeout::max().
 */
struct Timeout {
#ifdef max
# undef max
#endif
  static uint32_t max() {return std::numeric_limits<uint32_t>::max();}
  /*!
   * Convenience function to generate Timeout structs using a
   * single absolute timeout.
   *
   * \param timeout A long that defines the time in milliseconds until a
   * timeout occurs after a call to read or write is made.
   *
   * \return Timeout struct that represents this simple timeout provided.
   */
  static Timeout simpleTimeout(uint32_t timeout) {
    return Timeout(max(), timeout, 0, timeout, 0);
  }

  /*! Number of milliseconds between bytes received to timeout on. */
  uint32_t inter_byte_timeout;
  /*! A constant number of milliseconds to wait after calling read. */
  uint32_t read_timeout_constant;
  /*! A multiplier against the number of requested bytes to wait after
   *  calling read.
   */
  uint32_t read_timeout_multiplier;
  /*! A constant number of milliseconds to wait after calling write. */
  uint32_t write_timeout_constant;
  /*! A multiplier against the number of requested bytes to wait after
   *  calling write.
   */
  uint32_t write_timeout_multiplier;

  explicit Timeout (uint32_t inter_byte_timeout_=0,
                    uint32_t read_timeout_constant_=0,
                    uint32_t read_timeout_multiplier_=0,
                    uint32_t write_timeout_constant_=0,
                    uint32_t write_timeout_multiplier_=0)
  : inter_byte_timeout(inter_byte_timeout_),
    read_timeout_constant(read_timeout_constant_),
    read_timeout_multiplier(read_timeout_multiplier_),
    write_timeout_constant(write_timeout_constant_),
    write_timeout_multiplier(write_timeout_multiplier_)
  {}
};

/*!
 * Class that provides a portable serial port interface.
 */
class Serial {
public:
  /*!
   * Creates a Serial object and opens the port if a port is specified,
   * otherwise it remains closed until serial::Serial::open is called.
   *
   * \param port A std::string containing the address of the serial port,
   *        which would be something like 'COM1' on Windows and '/dev/ttyS0'
   *        on Linux.
   *
   * \param baudrate An unsigned 32-bit integer that represents the baudrate
   *
   * \param timeout A serial::Timeout struct that defines the timeout
   * conditions for the serial port. \see serial::Timeout
   *
   * \param bytesize Size of each byte in the serial transmission of data,
   * default is eightbits, possible values are: fivebits, sixbits, sevenbits,
   * eightbits
   *
   * \param parity Method of parity, default is parity_none, possible values
   * are: parity_none, parity_odd, parity_even
   *
   * \param stopbits Number of stop bits used, default is stopbits_one,
   * possible values are: stopbits_one, stopbits_one_point_five, stopbits_two
   *
   * \param flowcontrol Type of flowcontrol used, default is
   * flowcontrol_none, possible values are: flowcontrol_none,
   * flowcontrol_software, flowcontrol_hardware
   *
   * \throw serial::PortNotOpenedException
   * \throw serial::IOException
   * \throw std::invalid_argument
   */
  Serial (const std::string &port = "",
          uint32_t baudrate = 9600,
          Timeout timeout = Timeout(),
          bytesize_t bytesize = eightbits,
          parity_t parity = parity_none,
          stopbits_t stopbits = stopbits_one,
          flowcontrol_t flowcontrol = flowcontrol_none);

  /*! Destructor */
  virtual ~Serial ();

  /*!
   * Opens the serial port as long as the port is set and the port isn't
   * already open.
   *
   * If the port is provided to the constructor then an explicit call to open
   * is not needed.
   *
   * \see Serial::Serial
   *
   * \throw std::invalid_argument
   * \throw serial::SerialException
   * \throw serial::IOException
   */
  void
  open ();

  /*! Gets the open status of the serial port.
   *
   * \return Returns true if the port is open, false otherwise.
   */
  bool
  isOpen () const;

  /*! Closes the serial port. */
  void
  close ();

  /*! Return the number of characters in the buffer. */
  size_t
  available ();

  /*! Block until there is serial data to read or read_timeout_constant
   * number of milliseconds have elapsed. The return value is true when
   * the function exits with the port in a readable state, false otherwise
   * (due to timeout or select interruption). */
  bool
  waitReadable ();

  /*! Block for a period of time corresponding to the transmission time of
   * count characters at present serial settings. This may be used in con-
   * junction with waitReadable to read larger blocks of data from the
   * port. */
  void
  waitByteTimes (size_t count);

  /*! Read a given amount of bytes from the serial port into a given buffer.
   *
   * The read function will return in one of three cases:
   *  * The number of requested bytes was read.
   *    * In this case the number of bytes requested will match the size_t
   *      returned by read.
   *  * A timeout occurred, in this case the number of bytes read will not
   *    match the amount requested, but no exception will be thrown.  One of
   *    two possible timeouts occurred:
   *    * The inter byte timeout expired, this means that number of
   *      milliseconds elapsed between receiving bytes from the serial port
   *      exceeded the inter byte timeout.
   *    * The total timeout expired, which is calculated by multiplying the
   *      read timeout multiplier by the number of requested bytes and then
   *      added to the read timeout constant.  If that total number of
   *      milliseconds elapses after the initial call to read a timeout will
   *      occur.
   *  * An exception occurred, in this case an actual exception will be thrown.
   *
   * \param buffer An uint8_t array of at least the requested size.
   * \param size A size_t defining how many bytes to be read.
   *
   * \return A size_t representing the number of bytes read as a result of the
   *         call to read.
   *
   * \throw serial::PortNotOpenedException
   * \throw serial::SerialException
   */
  size_t
  read (uint8_t *buffer, size_t size);

  /*! Read a given amount of bytes from the serial port into a give buffer.
   *
   * \param buffer A reference to a std::vector of uint8_t.
   * \param size A size_t defining how many bytes to be read.
   *
   * \return A size_t representing the number of bytes read as a result of the
   *         call to read.
   *
   * \throw serial::PortNotOpenedException
   * \throw serial::SerialException
   */
  size_t
  read (std::vector<uint8_t> &buffer, size_t size = 1);

  /*! Read a given amount of bytes from the serial port into a give buffer.
   *
   * \param buffer A reference to a std::string.
   * \param size A size_t defining how many bytes to be read.
   *
   * \return A size_t representing the number of bytes read as a result of the
   *         call to read.
   *
   * \throw serial::PortNotOpenedException
   * \throw serial::SerialException
   */
  size_t
  read (std::string &buffer, size_t size = 1);

  /*! Read a given amount of bytes from the serial port and return a string
   *  containing the data.
   *
   * \param size A size_t defining how many bytes to be read.
   *
   * \return A std::string containing the data read from the port.
   *
   * \throw serial::PortNotOpenedException
   * \throw serial::SerialException
   */
  std::string
  read (size_t size = 1);

  /*! Reads in a line or until a given delimiter has been processed.
   *
   * Reads from the serial port until a single line has been read.
   *
   * \param buffer A std::string reference used to store the data.
   * \param size A maximum length of a line, defaults to 65536 (2^16)
   * \param eol A string to match against for the EOL.
   *
   * \return A size_t representing the number of bytes read.
   *
   * \throw serial::PortNotOpenedException
   * \throw serial::SerialException
   */
  size_t
  readline (std::string &buffer, size_t size = 65536, std::string eol = "\n");

  /*! Reads in a line or until a given delimiter has been processed.
   *
   * Reads from the serial port until a single line has been read.
   *
   * \param size A maximum length of a line, defaults to 65536 (2^16)
   * \param eol A string to match against for the EOL.
   *
   * \return A std::string containing the line.
   *
   * \throw serial::PortNotOpenedException
   * \throw serial::SerialException
   */
  std::string
  readline (size_t size = 65536, std::string eol = "\n");

  /*! Reads in multiple lines until the serial port times out.
   *
   * This requires a timeout > 0 before it can be run. It will read until a
   * timeout occurs and return a list of strings.
   *
   * \param size A maximum length of combined lines, defaults to 65536 (2^16)
   *
   * \param eol A string to match against for the EOL.
   *
   * \return A vector<string> containing the lines.
   *
   * \throw serial::PortNotOpenedException
   * \throw serial::SerialException
   */
  std::vector<std::string>
  readlines (size_t size = 65536, std::string eol = "\n");

  /*! Write a string to the serial port.
   *
   * \param data A const reference containing the data to be written
   * to the serial port.
   *
   * \param size A size_t that indicates how many bytes should be written from
   * the given data buffer.
   *
   * \return A size_t representing the number of bytes actually written to
   * the serial port.
   *
   * \throw serial::PortNotOpenedException
   * \throw serial::SerialException
   * \throw serial::IOException
   */
  size_t
  write (const uint8_t *data, size_t size);

  /*! Write a string to the serial port.
   *
   * \param data A const reference containing the data to be written
   * to the serial port.
   *
   * \return A size_t representing the number of bytes actually written to
   * the serial port.
   *
   * \throw serial::PortNotOpenedException
   * \throw serial::SerialException
   * \throw serial::IOException
   */
  size_t
  write (const std::vector<uint8_t> &data);

  /*! Write a string to the serial port.
   *
   * \param data A const reference containing the data to be written
   * to the serial port.
   *
   * \return A size_t representing the number of bytes actually written to
   * the serial port.
   *
   * \throw serial::PortNotOpenedException
   * \throw serial::SerialException
   * \throw serial::IOException
   */
  size_t
  write (const std::string &data);

  /*! Sets the serial port identifier.
   *
   * \param port A const std::string reference containing the address of the
   * serial port, which would be something like 'COM1' on Windows and
   * '/dev/ttyS0' on Linux.
   *
   * \throw std::invalid_argument
   */
  void
  setPort (const std::string &port);

  /*! Gets the serial port identifier.
   *
   * \see Serial::setPort
   *
   * \throw std::invalid_argument
   */
  std::string
  getPort () const;

  /*! Sets the timeout for reads and writes using the Timeout struct.
   *
   * There are two timeout conditions described here:
   *  * The inter byte timeout:
   *    * The inter_byte_timeout component of serial::Timeout defines the
   *      maximum amount of time, in milliseconds, between receiving bytes on
   *      the serial port that can pass before a timeout occurs.  Setting this
   *      to zero will prevent inter byte timeouts from occurring.
   *  * Total time timeout:
   *    * The constant and multiplier component of this timeout condition,
   *      for both read and write, are defined in serial::Timeout.  This
   *      timeout occurs if the total time since the read or write call was
   *      made exceeds the specified time in milliseconds.
   *    * The limit is defined by multiplying the multiplier component by the
   *      number of requested bytes and adding that product to the constant
   *      component.  In this way if you want a read call, for example, to
   *      timeout after exactly one second regardless of the number of bytes
   *      you asked for then set the read_timeout_constant component of
   *      serial::Timeout to 1000 and the read_timeout_multiplier to zero.
   *      This timeout condition can be used in conjunction with the inter
   *      byte timeout condition with out any problems, timeout will simply
   *      occur when one of the two timeout conditions is met.  This allows
   *      users to have maximum control over the trade-off between
   *      responsiveness and efficiency.
   *
   * Read and write functions will return in one of three cases.  When the
   * reading or writing is complete, when a timeout occurs, or when an
   * exception occurs.
   *
   * \param timeout A serial::Timeout struct containing the inter byte
   * timeout, and the read and write timeout constants and multipliers.
   *
   * \see serial::Timeout
   */
  void
  setTimeout (Timeout &timeout);

  /*! Sets the timeout for reads and writes. */
  void
  setTimeout (uint32_t inter_byte_timeout, uint32_t read_timeout_constant,
              uint32_t read_timeout_multiplier, uint32_t write_timeout_constant,
              uint32_t write_timeout_multiplier)
  {
    Timeout timeout(inter_byte_timeout, read_timeout_constant,
                    read_timeout_multiplier, write_timeout_constant,
                    write_timeout_multiplier);
    return setTimeout(timeout);
  }

  /*! Gets the timeout for reads in seconds.
   *
   * \return A Timeout struct containing the inter_byte_timeout, and read
   * and write timeout constants and multipliers.
   *
   * \see Serial::setTimeout
   */
  Timeout
  getTimeout () const;

  /*! Sets the baudrate for the serial port.
   *
   * Possible baudrates depends on the system but some safe baudrates include:
   * 110, 300, 600, 1200, 2400, 4800, 9600, 14400, 19200, 28800, 38400, 56000,
   * 57600, 115200
   * Some other baudrates that are supported by some comports:
   * 128000, 153600, 230400, 256000, 460800, 921600
   *
   * \param baudrate An integer that sets the baud rate for the serial port.
   *
   * \throw std::invalid_argument
   */
  void
  setBaudrate (uint32_t baudrate);

  /*! Gets the baudrate for the serial port.
   *
   * \return An integer that sets the baud rate for the serial port.
   *
   * \see Serial::setBaudrate
   *
   * \throw std::invalid_argument
   */
  uint32_t
  getBaudrate () const;

  /*! Sets the bytesize for the serial port.
   *
   * \param bytesize Size of each byte in the serial transmission of data,
   * default is eightbits, possible values are: fivebits, sixbits, sevenbits,
   * eightbits
   *
   * \throw std::invalid_argument
   */
  void
  setBytesize (bytesize_t bytesize);

  /*! Gets the bytesize for the serial port.
   *
   * \see Serial::setBytesize
   *
   * \throw std::invalid_argument
   */
  bytesize_t
  getBytesize () const;

  /*! Sets the parity for the serial port.
   *
   * \param parity Method of parity, default is parity_none, possible values
   * are: parity_none, parity_odd, parity_even
   *
   * \throw std::invalid_argument
   */
  void
  setParity (parity_t parity);

  /*! Gets the parity for the serial port.
   *
   * \see Serial::setParity
   *
   * \throw std::invalid_argument
   */
  parity_t
  getParity () const;

  /*! Sets the stopbits for the serial port.
   *
   * \param stopbits Number of stop bits used, default is stopbits_one,
   * possible values are: stopbits_one, stopbits_one_point_five, stopbits_two
   *
   * \throw std::invalid_argument
   */
  void
  setStopbits (stopbits_t stopbits);

  /*! Gets the stopbits for the serial port.
   *
   * \see Serial::setStopbits
   *
   * \throw std::invalid_argument
   */
  stopbits_t
  getStopbits () const;

  /*! Sets the flow control for the serial port.
   *
   * \param flowcontrol Type of flowcontrol used, default is flowcontrol_none,
   * possible values are: flowcontrol_none, flowcontrol_software,
   * flowcontrol_hardware
   *
   * \throw std::invalid_argument
   */
  void
  setFlowcontrol (flowcontrol_t flowcontrol);

  /*! Gets the flow control for the serial port.
   *
   * \see Serial::setFlowcontrol
   *
   * \throw std::invalid_argument
   */
  flowcontrol_t
  getFlowcontrol () const;

  /*! Flush the input and output buffers */
  void
  flush ();

  /*! Flush only the input buffer */
  void
  flushInput ();

  /*! Flush only the output buffer */
  void
  flushOutput ();

  /*! Sends the RS-232 break signal.  See tcsendbreak(3). */
  void
  sendBreak (int duration);

  /*! Set the break condition to a given level.  Defaults to true. */
  void
  setBreak (bool level = true);

  /*! Set the RTS handshaking line to the given level.  Defaults to true. */
  void
  setRTS (bool level = true);

  /*! Set the DTR handshaking line to the given level.  Defaults to true. */
  void
  setDTR (bool level = true);

  /*!
   * Blocks until CTS, DSR, RI, CD changes or something interrupts it.
   *
   * Can throw an exception if an error occurs while waiting.
   * You can check the status of CTS, DSR, RI, and CD once this returns.
   * Uses TIOCMIWAIT via ioctl if available (mostly only on Linux) with a
   * resolution of less than +-1ms and as good as +-0.2ms.  Otherwise a
   * polling method is used which can give +-2ms.
   *
   * \return Returns true if one of the lines changed, false if something else
   * occurred.
   *
   * \throw SerialException
   */
  bool
  waitForChange ();

  /*! Returns the current status of the CTS line. */
  bool
  getCTS ();

  /*! Returns the current status of the DSR line. */
  bool
  getDSR ();

  /*! Returns the current status of the RI line. */
  bool
  getRI ();

  /*! Returns the current status of the CD line. */
  bool
  getCD ();

private:
  // Disable copy constructors
  Serial(const Serial&);
  Serial& operator=(const Serial&);

  // Pimpl idiom, d_pointer
  class SerialImpl;
  SerialImpl *pimpl_;

  // Scoped Lock Classes
  class ScopedReadLock;
  class ScopedWriteLock;

  // Read common function
  size_t
  read_ (uint8_t *buffer, size_t size);
  // Write common function
  size_t
  write_ (const uint8_t *data, size_t length);

};

class SerialException : public std::exception
{
  // Disable copy constructors
  SerialException& operator=(const SerialException&);
  std::string e_what_;
public:
  SerialException (const char *description) {
      std::stringstream ss;
      ss << "SerialException " << description << " failed.";
      e_what_ = ss.str();
  }
  SerialException (const SerialException& other) : e_what_(other.e_what_) {}
  virtual ~SerialException() throw() {}
  virtual const char* what () const throw () {
    return e_what_.c_str();
  }
};

class IOException : public std::exception
{
  // Disable copy constructors
  IOException& operator=(const IOException&);
  std::string file_;
  int line_;
  std::string e_what_;
  int errno_;
public:
  explicit IOException (std::string file, int line, int errnum)
    : file_(file), line_(line), errno_(errnum) {
      std::stringstream ss;
#if defined(_WIN32) && !defined(__MINGW32__)
      char error_str [1024];
      strerror_s(error_str, 1024, errnum);
#else
      char * error_str = strerror(errnum);
#endif
      ss << "IO Exception (" << errno_ << "): " << error_str;
      ss << ", file " << file_ << ", line " << line_ << ".";
      e_what_ = ss.str();
  }
  explicit IOException (std::string file, int line, const char * description)
    : file_(file), line_(line), errno_(0) {
      std::stringstream ss;
      ss << "IO Exception: " << description;
      ss << ", file " << file_ << ", line " << line_ << ".";
      e_what_ = ss.str();
  }
  virtual ~IOException() throw() {}
  IOException (const IOException& other) : line_(other.line_), e_what_(other.e_what_), errno_(other.errno_) {}

  int getErrorNumber () const { return errno_; }

  virtual const char* what () const throw () {
    return e_what_.c_str();
  }
};

class PortNotOpenedException : public std::exception
{
  // Disable copy constructors
  const PortNotOpenedException& operator=(PortNotOpenedException);
  std::string e_what_;
public:
  PortNotOpenedException (const char * description)  {
      std::stringstream ss;
      ss << "PortNotOpenedException " << description << " failed.";
      e_what_ = ss.str();
  }
  PortNotOpenedException (const PortNotOpenedException& other) : e_what_(other.e_what_) {}
  virtual ~PortNotOpenedException() throw() {}
  virtual const char* what () const throw () {
    return e_what_.c_str();
  }
};

/*!
 * Structure that describes a serial device.
 */
struct PortInfo {

  /*! Address of the serial port (this can be passed to the constructor of Serial). */
  std::string port;

  /*! Human readable description of serial device if available. */
  std::string description;

  /*! Hardware ID (e.g. VID:PID of USB serial devices) or "n/a" if not available. */
  std::string hardware_id;

};

/* Lists the serial ports available on the system
 *
 * Returns a vector of available serial ports, each represented
 * by a serial::PortInfo data structure:
 *
 * \return vector of serial::PortInfo.
 */
std::vector<PortInfo>
list_ports();

} // namespace serial

#endif

2 结论

但是虽然其与SPI的通信速率一致,但最后运行的效果差很多,机器人会出现剧烈的抖动,这与早期SPI通信速率不够高造成数据实时性较低的现象一致,可见采用虚拟串口是无法实现如此高实时性的需求的,可能采用其他USB的通信方法能实现但受限于嵌入式开发水平无法实现(在此也欢迎了解的同学提供测试方案),因此最终还是回到对DMA-SPI通信的优化,通过各种尝试目前发现通过保证如下的两个机制能大幅度提高SPI通信的可靠性:

  • (1)首先将SPI NSS修改为硬件管理,具体原因不明
  • (2)在通信传输中保证Linux和单片机通信数据量一致,如果协议长度不足,则补充0数据;

通过上述的修改目前机器人SPI通信稳定了很多,但是不是还会出现之前的问题仍需要更多可靠性测试,下面给出了机器人连续行走40分钟未出现故障的打印,相比之前整个过程中不再出现解码错误的提示(上一版本在静止站立时不会误码,而当机器人一段运动起来就会出现),因此底层驱动的可靠性得到了提升这样也实现机器人能真正走到户外,当然对通信协议和通信速度的进一步优化也是重要的方向!

综上,总体来说目前对于个人DIY采用SPI转CAN的方案还是比较合适的,首先其难度较低,实时性和效果也不错,采用串口或USB虚拟串口可能无法满足机器人实时运动控制需求,但由于采集一些其他传感器还是可以的,当然如果能采用PCIE的方案那性能应该可以大幅度提升,当然基于USB本身通信的机制应该也能实现不错的效果但整个驱动开发比较复杂,考虑到实际我们转换的需求还是将Linux的驱动命令以CAN发送出去,目前市面上有很多独立的SPI转CAN模块,如周立功就有好几款,实际其内部应该就是代替了单片机来实现上述工作,可能对于产品开发直接采用这样的模块更合适吧。