项目踩坑:串口通讯(Arduino)

问题描述:

arduino和电脑上的c++程序通讯,数据为包含18个float浮点数的数组,数据带包头'S'和包尾'E'。通讯过程中电脑能够顺利接收到来自arduino的数组,但是arduino无法接收来自电脑的。

问题定位:

调试发现该数组加上包头包尾共74个字节,但是arduino默认串口缓存大小为64字节,因此每次都有溢出,无法读到完整的数据。

问题解决:

方法一:修改arduino的串口缓存大小

如果是在VSCode里用platformIO创建的项目

则在platformio.ini文件内配置串口缓存大小:
 

build_flags =
    -D SERIAL_RX_BUFFER_SIZE=256
    -D SERIAL_TX_BUFFER_SIZE=256

举例,我的arduino mega2560板子对应的platformio.ini完整内容为:

[env:megaatmega2560]
platform = atmelavr
board = megaatmega2560
framework = arduino
monitor_speed = 115200
build_flags =
    -D SERIAL_RX_BUFFER_SIZE=256
    -D SERIAL_TX_BUFFER_SIZE=256

 编译后重新写入arduino进行测试

如果是在Aduino IDE里创建的项目

则在 HardwareSerial.h 文件中进行修改。

对于标准的Arduino IDE安装,该文件路径通常如下:

  • Windows: C:\Program Files (x86)\Arduino\hardware\arduino\avr\cores\arduino\HardwareSerial.h
  • macOS: /Applications/Arduino.app/Contents/Java/hardware/arduino/avr/cores/arduino/HardwareSerial.h
  • Linux: /usr/share/arduino/hardware/arduino/avr/cores/arduino/HardwareSerial.h
修改方式
  1.  打开 HardwareSerial.h 文件。
  2. 找到如下的定义:
    #if !defined(SERIAL_TX_BUFFER_SIZE)
    #define SERIAL_TX_BUFFER_SIZE 64
    #endif
    #if !defined(SERIAL_RX_BUFFER_SIZE)
    #define SERIAL_RX_BUFFER_SIZE 64
    #endif
    
  3. 将 SERIAL_RX_BUFFER_SIZE 和 SERIAL_TX_BUFFER_SIZE 修改为更大的数值,例如 256:
    #define SERIAL_TX_BUFFER_SIZE 256
    #define SERIAL_RX_BUFFER_SIZE 256
    

修改后重新编译上传后测试

方法二:将过长的数据分段进行收发

没试,可在arduino和电脑上的代码里都增加分段和合并的函数,约定好分段的格式。

附测试代码:

arduino:

#include <Arduino.h>
#include <stdlib.h>

const int FLOATS_COUNT = 18;
const int BUFFER_SIZE = FLOATS_COUNT * sizeof(float) + 2; // 12 floats + 'S' + 'E'
char buffer[BUFFER_SIZE];
int bufferIndex = 0;
bool receiving = false;
float receivedData[FLOATS_COUNT];
float sendData[FLOATS_COUNT];

const uint8_t led_1 = 22;
const uint8_t led_2 = 24;
const uint8_t led_3 = 26;

void senddata(float* data) {
  Serial.write('S');
  Serial.write((byte*)data, FLOATS_COUNT * sizeof(float));
  Serial.write('E');
}

void setup() {
  pinMode(led_1, OUTPUT);
  pinMode(led_2, OUTPUT);
  pinMode(led_3, OUTPUT);
  digitalWrite(led_1, LOW);
  digitalWrite(led_2, LOW);
  digitalWrite(led_3, LOW);

  sendData[1] = 1.45;

  Serial.begin(115200);
  while (!Serial) {} // 等待串行端口准备好
}


void loop() {
    if (Serial.available() >= BUFFER_SIZE) { // S (1 byte) + 12 floats (72 bytes) + E (1 byte) = 74 bytes
      digitalWrite(led_1, !digitalRead(led_1));
        if (Serial.read() == 'S') {
            digitalWrite(led_2, !digitalRead(led_2));            
            Serial.readBytes((char*)receivedData, sizeof(receivedData)); // 读取浮点数组

            for (int i = 0; i < FLOATS_COUNT; ++i) {
                sendData[i] = receivedData[i]+0.001; // 每个float加上0.1
            }

            if(Serial.read() == 'E'){
                digitalWrite(led_3, !digitalRead(led_3));
                senddata(sendData);
            }
        }
    }
}

pc c++:

#include <rclcpp/rclcpp.hpp>
#include <serial/serial.h>
#include <chrono>
#include <thread>
#include <vector>
#include <memory>
#include <iostream>

std::vector<float> receive_data(18);
std::vector<float> send_data(18);

void receive_serial_data(serial::Serial& serial_port) {
    while (rclcpp::ok()) {
        receive_data = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
        if (serial_port.available() >= sizeof(float) * receive_data.size() + 2) {
            // 读取包头
            char start;
            serial_port.read(reinterpret_cast<uint8_t*>(&start), 1);
            
            if (start == 'S') {
                serial_port.read(reinterpret_cast<uint8_t*>(receive_data.data()), sizeof(float) * receive_data.size());
                
                // 读取包尾
                char end;
                serial_port.read(reinterpret_cast<uint8_t*>(&end), 1);
                
                if (end == 'E') {
                    std::cout << "Received data: ";
                    for (const auto& data : receive_data) {
                        std::cout << data << " ";
                    }
                    std::cout << std::endl;

                    for (size_t i = 0; i < receive_data.size(); i++) {
                        send_data[i] = receive_data[i] + 0.1;
                    }
                }
            }
        }

        // Sleep for a short duration to avoid high CPU usage
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
}


std::vector<char> packData(const std::vector<float>& values) {
    size_t dataSize = values.size() * sizeof(float);
    std::vector<char> packet;
    packet.reserve(1 + dataSize + 1); // 要保存包头和包尾

    // 添加包头
    packet.push_back('S');

    // 添加浮点数数据
    for (const float value : values) {
        char data[sizeof(float)];
        std::memcpy(data, &value, sizeof(float));
        packet.insert(packet.end(), data, data + sizeof(float));
    }

    // 添加包尾
    packet.push_back('E');

    return packet;
}

int main(int argc, char * argv[]) {
    (void)argc;
    (void)argv;
    // Create a ROS 2 node
    rclcpp::init(0, nullptr);
    auto node = std::make_shared<rclcpp::Node>("serial_communication");

    std::string port_name[] = {"/dev/ttyUSB0", "/dev/ttyUSB1"};
    int current_port_index = 0;
    serial::Serial serial_port;

    // Create a serial port object
    while (rclcpp::ok()) {
        try {
            serial_port.setPort(port_name[current_port_index]);
            serial_port.setBaudrate(115200);
            std::cout << "hhh" << std::endl;
            serial::Timeout timeout = serial::Timeout::simpleTimeout(1000);
            std::cout << "iii" << std::endl;
            serial_port.setTimeout(timeout);
            std::cout << "jjj" << std::endl;
            serial_port.open();
            if (serial_port.isOpen()) {
                std::cout << "Serial port: " << port_name[current_port_index] << " is open." << std::endl;
                break;
            }
        } catch (const std::exception& e) {
            current_port_index = (current_port_index + 1) % 2;
            std::cout << "change port name to: " << port_name[current_port_index] << std::endl;
        }

        // Sleep for 500 milliseconds
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
    }

    // 创建一个线程来处理接收数据
    std::thread receive_thread(receive_serial_data, std::ref(serial_port));

    // 初始化发送数据
    send_data = {0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0, 1.1, 1.2, 1.3, 1.4, 1.5, 1.6, 1.7, 1.8};

    // 主线程处理发送数据
    rclcpp::Rate loop_rate(1); // 10 Hz
    while (rclcpp::ok()) {
        // 发送数据包
        std::vector<char> send_data_packed = packData(send_data);
        serial_port.write(reinterpret_cast<const uint8_t*>(send_data_packed.data()), send_data_packed.size());

        std::cout << "Packed Data: ";
        for (char c : send_data_packed) {
            std::cout << std::hex << static_cast<int>(static_cast<unsigned char>(c)) << " ";
        }
        std::cout << std::endl;
        std::cout<< send_data_packed.size() << std::endl;

        loop_rate.sleep();
    }

    // 等待接收线程结束
    if (receive_thread.joinable()) {
        receive_thread.join();
    }

    rclcpp::shutdown();
    return 0;
}

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值