linux+cpp，串口转usb，控制失灵

省流：python脚本调用正常，写了个python脚本再用cpp调用

---

具体来说，使用的是wheeltech轮趣家的二自由度云台，串口转usb，里头应该是rs485，驱动用ch340，板子说是stm32、用商家的自带控制程序。用第一款云台（他家的内部名字大概是模块化款）cpp和python都没啥问题，用第二款云台（他家内部名字大概是c06款）python能用cpp就不行了。二者使用ubuntu的串口助手cutecom表现相同。分享代码如下，有差别的自行调整：

cpp部分（set_opt，_System函数也是网上抄的，出处懒得找了，大概也是csdn）（changedegree函数是原始的cpp直接控制，changedegree2函数是调用python脚本）：

#include <errno.h>
#include <fcntl.h>
#include <iostream>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <string>
#include <sys/select.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <sys/types.h>
#include <termios.h>
#include <unistd.h>

using namespace std;

float YY = 1960;
float pantiltRotatingCenterHeight = 2060;
int set_opt(int fd, int nSpeed, int nBits, char nEvent, int nStop)
{
    struct termios newtio, oldtio;
    if (tcgetattr(fd, &oldtio) != 0) {
        perror("SetupSerial 1");
        return -1;
    }
    bzero(&newtio, sizeof(newtio));
    newtio.c_cflag |= CLOCAL | CREAD;
    newtio.c_cflag &= ~CSIZE;

    switch (nBits) {
    case 7:
        newtio.c_cflag |= CS7;
        break;
    case 8:
        newtio.c_cflag |= CS8;
        break;
    }

    switch (nEvent) {
    case 'O': // ji
        newtio.c_cflag |= PARENB;
        newtio.c_cflag |= PARODD;
        newtio.c_cflag |= (INPCK | ISTRIP);
        break;
    case 'E': // ou
        newtio.c_cflag |= PARENB;
        newtio.c_cflag |= ~PARODD;
        newtio.c_cflag |= (INPCK | ISTRIP);
        break;
    case 'N': // no
        newtio.c_cflag |= ~PARENB;
        break;
    }

    switch (nSpeed) {
    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 115200:
        cfsetispeed(&newtio, B115200);
        cfsetospeed(&newtio, B115200);
        break;
    default:
        cfsetispeed(&newtio, B9600);
        cfsetospeed(&newtio, B9600);
        break;
    }
    if (nStop == 1)
        newtio.c_cflag &= ~CSTOPB;
    else if (nStop == 2)
        newtio.c_cflag &= CSTOPB;
    newtio.c_cc[VTIME] = 0;
    newtio.c_cc[VMIN] = 0;
    tcflush(fd, TCSANOW);
    if ((tcsetattr(fd, TCSANOW, &newtio) != 0)) {
        perror("com set error");
        return -1;
    }
    printf("set done!\n");
    return 0;
}

int _System(const char* cmd, char* pRetMsg, int msg_len)
{
    FILE* fp;
    int res = -1;
    if (cmd == NULL || pRetMsg == NULL || msg_len < 0) {
        printf("Param Error!\n");
        return -1;
    }
    if ((fp = popen(cmd, "r")) == NULL) {
        printf("Popen Error!\n");
        return -2;
    } else {
        memset(pRetMsg, 0, msg_len);
        // get lastest result
        while (fgets(pRetMsg, msg_len, fp) != NULL) {
            // printf("Msg:%s",pRetMsg); //print all info
        }

        if ((res = pclose(fp)) == -1) {
            printf("close popenerror!\n");
            return -3;
        }
        pRetMsg[strlen(pRetMsg) - 1] = '\0';
        return 0;
    }
}
// cmd num range:8~180
// real num:40——down 45 degrees, 120——up 4 degrees
// camera num:82(down 60),-90(up 42)
inline float pantiltNum2DownDegree(short num) { return 45 - 49.0 / 80.0 * (num - 40); }
short oriNum = 70;

float changeDegree2(short newNum)
{
    short NumMaxLimit = 180;
    short NumMinLimit = 0;
    float radius = 52.0f + 26.0f / 2; // 5.2cm: pantilt rorating radius, 2.6cm: camara height
    float extendingPartLength = 125.4f - 63.4f - 5.0f; // approximate distance from hinge point to depth camera
    float newDownDegree = pantiltNum2DownDegree(newNum);

    if (newNum > NumMaxLimit) {
        return pantiltNum2DownDegree(NumMaxLimit);
    }
    if (newNum < NumMinLimit) {
        return pantiltNum2DownDegree(NumMinLimit);
    }
    char tmp[256] = { 0 };
    string cmd1 = "ls /dev|grep USB";
    if (_System(cmd1.c_str(), tmp, sizeof(tmp)) != 0) {
        exit(-1);
    }
    string uartPath = "/dev/" + string(tmp);
    cout << uartPath << endl;

    char newNum_str[256];
    sprintf(newNum_str, "%d", newNum);

    string cmd2 = "python3 gimbal.py " + string(newNum_str) + " " + string(tmp);
    cout << cmd2 << endl;
    char tmp2[256] = { 0 };
    if (_System(cmd2.c_str(), tmp2, sizeof(tmp2)) != 0) {
        exit(-1);
    }

    YY = pantiltRotatingCenterHeight - radius * cos(newDownDegree) - extendingPartLength * sin(newDownDegree);

    return newDownDegree;
}

float changeDegree(short newNum)
{
    short NumMaxLimit = 180;
    short NumMinLimit = 0;
    float radius = 52.0f + 26.0f / 2; // 5.2cm: pantilt rorating radius, 2.6cm: camara height
    float extendingPartLength = 125.4f - 63.4f - 5.0f; // approximate distance from hinge point to depth camera
    float newDownDegree = pantiltNum2DownDegree(newNum);

    if (newNum > NumMaxLimit) {
        return pantiltNum2DownDegree(NumMaxLimit);
    }
    if (newNum < NumMinLimit) {
        return pantiltNum2DownDegree(NumMinLimit);
    }
    unsigned char cmd[] = { 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
    cmd[3] = cmd[9] = newNum;

    char tmp[256] = { 0 };
    string cmd1 = "ls /dev|grep USB";
    if (_System(cmd1.c_str(), tmp, sizeof(tmp)) != 0) {
        exit(-1);
    }
    string uartPath = "/dev/" + string(tmp);
    cout << uartPath << endl;
    int fd = 1;

    if ((fd = open(uartPath.c_str(), O_RDWR | O_NOCTTY | O_NDELAY)) < 0) {
        printf("open %s is failed\n", uartPath.c_str());
        exit(-1);
    } else {
        set_opt(fd, 115200, 8, 'N', 1);
        write(fd, cmd, 10);
        close(fd);
    }
    YY = pantiltRotatingCenterHeight - radius * cos(newDownDegree) - extendingPartLength * sin(newDownDegree);

    return newDownDegree;
}
int main(int argc, char* argv[])
{
    if (argc == 2) {
        oriNum = atoi(argv[1]);
    }
    float a = changeDegree2(oriNum);
    cout << "num:" << oriNum << ", down degree:" << a << ", YY:" << YY << ", rotating height:" << pantiltRotatingCenterHeight << endl;
}

python部分（设置十次写入或者读取云台回复的部分是和云台本身有关的，参考可以不看）（运行之前pip3 install pyserial装一下依赖库）：

import serial,sys
send_limit=10

a=int(sys.argv[1])
b=hex(a)[2:]
cmd="FFFE00"+b+"0000000000"+b
uart=sys.argv[2]

print(a,b,cmd,uart)
cmd_b=bytes.fromhex(cmd)
ser = serial.Serial('/dev/'+uart, 115200, bytesize=8, stopbits=1, parity='N',timeout=1)

for i in range(send_limit):

    ser.write(cmd_b)
    ret=ser.read(80)
    try:
        ret_str=str(ret, encoding='gbk', errors='ignore')
    except:
        print(ret)
        ret_str=str(ret[1:], encoding='gbk', errors='ignore')
    ok=False
    for line in ret_str.split():
        if line == "舵机B角度："+str(a):
            ok=True
    if ok:break
    else:
        print(f"send try {i} failed!{ret_str.split()}")

ser.close()