无人水面艇 (USV) 的硬件接口部分的功能模块程序简单思路

kulataxi

硬件配置上一般会涉及这些（限于实验品）：一、常用传感器

• GPS: 通常通过UART或USB连接。
• 声纳传感器: 可能通过UART或I2C连接。
• IMU (Inertial Measurement Unit): 通常通过I2C或SPI连接。
  

二、常用执行器

• 电机驱动器: 通常通过PWM信号控制。
• 舵机: 通常通过PWM信号控制。
  

kulataxi

考虑到以上硬件配置，我们一般可以按照以下情况进行接口程序设计。
#include <iostream>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <atomic>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <string>
#include <vector>
#include <wiringPi.h>
#include <linux/i2c-dev.h>
#include <sys/ioctl.h>

// 定义传感器数据结构体
struct GPSData {
    double latitude;
    double longitude;
};

struct SonarData {
    double distance;
};

struct IMUData {
    double roll;
    double pitch;
    double yaw;
};

// 定义执行器命令结构体
struct MotorCommand {
    int dutyCycle; // 电机占空比
};

struct SteeringCommand {
    int angle; // 方向角
};

// 硬件接口类
class USVHardwareInterface {
private:
    std::atomic<bool> running_;
    std::mutex mtx_;
    std::condition_variable cv_;

    int gps_fd_;
    int sonar_fd_;
    int imu_fd_;
    int motor_pin_;
    int steering_pin_;

    GPSData gps_data_;
    SonarData sonar_data_;
    IMUData imu_data_;

public:
    USVHardwareInterface()
        : running_(true),
          gps_fd_(-1),
          sonar_fd_(-1),
          imu_fd_(-1),
          motor_pin_(0),
          steering_pin_(1) {}

    ~USVHardwareInterface() {
        close(gps_fd_);
        close(sonar_fd_);
        close(imu_fd_);
    }

    // 初始化传感器和执行器
    bool initialize() {
        if (!initSensor("gps", gps_fd_)) return false;
        if (!initSensor("sonar", sonar_fd_)) return false;
        if (!initIMU("/dev/i2c-1", 0x68, imu_fd_)) return false;
        if (!initActuator(motor_pin_, "motor")) return false;
        if (!initActuator(steering_pin_, "steering")) return false;
        return true;
    }

    // 初始化传感器
    bool initSensor(const std::string& sensor_name, int& fd) {
        fd = open(sensor_name.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
        if (fd == -1) {
            std::cerr << "无法打开传感器: " << sensor_name << std::endl;
            return false;
        }
        setSerialAttributes(fd);
        return true;
    }

    // 初始化IMU传感器
    bool initIMU(const std::string& device, int address, int& fd) {
        fd = open(device.c_str(), O_RDWR);
        if (fd == -1) {
            std::cerr << "无法打开IMU设备: " << device << std::endl;
            return false;
        }
        if (ioctl(fd, I2C_SLAVE, address) < 0) {
            std::cerr << "无法设置I2C地址" << std::endl;
            close(fd);
            return false;
        }
        return true;
    }

    // 初始化执行器
    bool initActuator(int pin, const std::string& actuator_name) {
        wiringPiSetup();
        pinMode(pin, PWM_OUTPUT);
        pwmSetMode(PWM_MODE_MS);
        pwmSetRange(1024);
        pwmSetClock(384); // 设置频率为50Hz
        return true;
    }

    // 设置串口属性
    void setSerialAttributes(int fd) {
        struct termios tty;
        memset(&tty, 0, sizeof(tty));
        cfsetospeed(&tty, B9600);
        cfsetispeed(&tty, B9600);

        tty.c_cflag &= ~PARENB;     // No parity bit
        tty.c_cflag &= ~CSTOPB;     // Only need 1 stop bit
        tty.c_cflag &= ~CSIZE;
        tty.c_cflag |= CS8;         // 8 bits per byte
        tty.c_cflag &= ~CRTSCTS;    // No hardware flowcontrol
        tty.c_iflag &= ~(IXON | IXOFF | IXANY); // Turn off s/w flow ctrl
        tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // Make raw
        tty.c_oflag &= ~OPOST;      // Make raw

        tcsetattr(fd, TCSANOW, &tty);
    }

    // 读取传感器数据
    void readSensors() {
        while (running_) {
            readGPSSensor();
            readSonarSensor();
            readIMUSensor();
            std::this_thread::sleep_for(std::chrono::milliseconds(100)); // 每100ms读取一次
        }
    }

    // 读取GPS传感器
    void readGPSSensor() {
        char buffer[256];
        ssize_t bytes_read = read(gps_fd_, buffer, sizeof(buffer));
        if (bytes_read > 0) {
            std::string data(buffer, bytes_read);
            parseGPSSensorData(data);
        }
    }

    // 解析GPS传感器数据
    void parseGPSSensorData(const std::string& data) {
        // 假设数据格式为 "lat:xx.xxxx lon:yy.yyyy"
        size_t latPos = data.find("lat:");
        size_t lonPos = data.find("lon:");

        if (latPos != std::string::npos && lonPos != std::string::npos) {
            latPos += 4;
            lonPos += 4;
            double lat = std::stod(data.substr(latPos, data.find(' ', latPos) - latPos));
            double lon = std::stod(data.substr(lonPos, data.find('\n', lonPos) - lonPos));

            std::lock_guard<std::mutex> lock(mtx_);
            gps_data_.latitude = lat;
            gps_data_.longitude = lon;
            cv_.notify_one();
        }
    }

    // 读取声纳传感器
    void readSonarSensor() {
        char buffer[256];
        ssize_t bytes_read = read(sonar_fd_, buffer, sizeof(buffer));
        if (bytes_read > 0) {
            std::string data(buffer, bytes_read);
            parseSonarSensorData(data);
        }
    }

    // 解析声纳传感器数据
    void parseSonarSensorData(const std::string& data) {
        // 假设数据格式为 "distance:xx.xx"
        size_t distPos = data.find("distance:");

        if (distPos != std::string::npos) {
            distPos += 9;
            double distance = std::stod(data.substr(distPos, data.find('\n', distPos) - distPos));

            std::lock_guard<std::mutex> lock(mtx_);
            sonar_data_.distance = distance;
            cv_.notify_one();
        }
    }

    // 读取IMU传感器
    void readIMUSensor() {
        unsigned char buf[14];

        // 读取MPU6050数据
        write(imu_fd_, "\x3B\x47", 2); // 从寄存器0x3B开始读取14个字节
        if (read(imu_fd_, buf, 14) != 14) {
            std::cerr << "无法读取IMU数据" << std::endl;
            return;
        }

        int16_t acc_x = static_cast<int16_t>(buf[0] << 8 | buf[1]);
        int16_t acc_y = static_cast<int16_t>(buf[2] << 8 | buf[3]);
        int16_t acc_z = static_cast<int16_t>(buf[4] << 8 | buf[5]);
        int16_t temp = static_cast<int16_t>(buf[6] << 8 | buf[7]);
        int16_t gyro_x = static_cast<int16_t>(buf[8] << 8 | buf[9]);
        int16_t gyro_y = static_cast<int16_t>(buf[10] << 8 | buf[11]);
        int16_t gyro_z = static_cast<int16_t>(buf[12] << 8 | buf[13]);

        // 假设IMU校准因子和转换公式
        double ax = acc_x / 16384.0;
        double ay = acc_y / 16384.0;
        double az = acc_z / 16384.0;
        double gx = gyro_x / 131.0;
        double gy = gyro_y / 131.0;
        double gz = gyro_z / 131.0;

        // 简单的欧拉角度计算（仅用于示例）
        static double roll = 0.0;
        static double pitch = 0.0;
        static double yaw = 0.0;
        static double dt = 0.01; // 时间间隔

        double roll_acc = atan2(ay, az) * 180 / M_PI;
        double pitch_acc = atan2(-ax, sqrt(ay * ay + az * az)) * 180 / M_PI;

        roll = 0.98 * (roll + gx * dt) + 0.02 * roll_acc;
        pitch = 0.98 * (pitch + gy * dt) + 0.02 * pitch_acc;
        yaw += gz * dt;

        std::lock_guard<std::mutex> lock(mtx_);
        imu_data_.roll = roll;
        imu_data_.pitch = pitch;
        imu_data_.yaw = yaw;
        cv_.notify_one();
    }

    // 发送电机命令
    void sendMotorCommand(int dutyCycle) {
        pwmWrite(motor_pin_, dutyCycle);
    }

    // 发送舵机命令
    void sendSteeringCommand(int angle) {
        int dutyCycle = map(angle, 0, 180, 0, 1024); // 映射角度到PWM占空比
        pwmWrite(steering_pin_, dutyCycle);
    }

    // 获取GPS数据
    GPSData getGPSData() {
        std::unique_lock<std::mutex> lock(mtx_);
        cv_.wait(lock, [this] { return !running_ || gps_data_.latitude != 0.0 || gps_data_.longitude != 0.0; });
        return gps_data_;
    }

    // 获取声纳数据
    SonarData getSonarData() {
        std::unique_lock<std::mutex> lock(mtx_);
        cv_.wait(lock, [this] { return !running_ || sonar_data_.distance != 0.0; });
        return sonar_data_;
    }

    // 获取IMU数据
    IMUData getIMUData() {
        std::unique_lock<std::mutex> lock(mtx_);
        cv_.wait(lock, [this] { return !running_ || imu_data_.roll != 0.0 || imu_data_.pitch != 0.0 || imu_data_.yaw != 0.0; });
        return imu_data_;
    }

    // 停止运行
    void stop() {
        running_.store(false);
        cv_.notify_all();
    }

private:
    // 辅助函数：映射值
    int map(int value, int fromLow, int fromHigh, int toLow, int toHigh) {
        return (value - fromLow) * (toHigh - toLow) / (fromHigh - fromLow) + toLow;
    }
};

int main() {
    USVHardwareInterface usv;

    if (!usv.initialize()) {
        std::cerr << "初始化失败" << std::endl;
        return 1;
    }

    std::thread sensorThread([&usv]() {
        usv.readSensors();
    });

    try {
        while (true) {
            GPSData gps = usv.getGPSData();
            SonarData sonar = usv.getSonarData();
            IMUData imu = usv.getIMUData();

            std::cout << "GPS Data - Latitude: " << gps.latitude << ", Longitude: " << gps.longitude << std::endl;
            std::cout << "Sonar Data - Distance: " << sonar.distance << std::endl;
            std::cout << "IMU Data - Roll: " << imu.roll << ", Pitch: " << imu.pitch << ", Yaw: " << imu.yaw << std::endl;

            // 示例：发送电机和舵机命令
            usv.sendMotorCommand(512); // 占空比50%
            usv.sendSteeringCommand(90); // 中间位置

            std::this_thread::sleep_for(std::chrono::seconds(1)); // 每秒读取一次
        }
    } catch (const std::exception& e) {
        std::cerr << "异常: " << e.what() << std::endl;
    }

    usv.stop();
    sensorThread.join();

    return 0;
}

kulataxi

说明：
一、传感器数据采集——
GPS， 使用UART串口读取数据，并解析成经纬度
声纳传感器， 使用UART串口读取数据，并解析成距离
IMU (MPU6050)，使用I2C读取加速度计和陀螺仪数据，并计算简单的欧拉角度
二、通信接口——
串口通信: 使用termios库设置串口参数
I2C通信: 使用Linux的i2c-dev库进行I2C通信
三、执行器控制
电机驱动器: 使用wiringPi库控制PWM输出
舵机: 使用wiringPi库控制PWM输出，并将角度映射到PWM占空比
除了以上几方面，有时很容易忽略掉错误处理机制有关内容设计考虑，这样无法确保系统的稳定性，有问题也不容易查找分析，只能凭个人经验。
————
希望以上这些对有的朋友有所帮助。

kulataxi

既然是USV，那么免不了的是航迹规划。这忒么的是一个大问题，而且与数学关系扯淡太多，与概率关系密切；感兴趣的可以拾起曾经的专业课本翻翻，或者寻找专业企业产品文档以及一些公开的资料参考。这里不多说，只提供肯定能用上的东西，到时自己改改能跑起来就OK的代码。这破玩意儿花了老子近一个月的时间才马马虎虎，如有错误，敬请熟悉的专业朋友谅解！

kulataxi

三横一竖

来自安卓APP客户端