首页 > 分享 > 在日常的使用步行导航地图导航定位时，GPS的vAcc=30多米正常吗，精度够用吗？

在日常的使用步行导航地图导航定位时，GPS的vAcc=30多米正常吗，精度够用吗？

花匠小妙招
2026-04-07 00:12

#include "DroneDataWorker.h" #include <QDateTime> #include <QtGlobal> #include <QDebug> #include <cmath> #include <cstring> #include <random> #include <QThread> #include <QMutexLocker> // ========== 新增：PID参数范围常量定义 ========== // 角度环（俯仰/横滚） const float PITCH_ROLL_KP_MIN = 1.5f; const float PITCH_ROLL_KP_MAX = 3.0f; const float PITCH_ROLL_KI_MIN = 0.05f; const float PITCH_ROLL_KI_MAX = 0.2f; const float PITCH_ROLL_KD_MIN = 0.3f; const float PITCH_ROLL_KD_MAX = 0.8f; // 角速度环（俯仰/横滚） const float PITCH_ROLL_SPEED_KP_MIN = 1.0f; const float PITCH_ROLL_SPEED_KP_MAX = 2.0f; const float PITCH_ROLL_SPEED_KI_MIN = 0.02f; const float PITCH_ROLL_SPEED_KI_MAX = 0.1f; const float PITCH_ROLL_SPEED_KD_MIN = 0.1f; const float PITCH_ROLL_SPEED_KD_MAX = 0.3f; // 偏航角 const float YAW_KP_MIN = 1.0f; const float YAW_KP_MAX = 2.5f; const float YAW_KI_MIN = 0.03f; const float YAW_KI_MAX = 0.15f; const float YAW_KD_MIN = 0.1f; const float YAW_KD_MAX = 0.5f; // 偏航角速度 const float YAW_SPEED_KP_MIN = 0.8f; const float YAW_SPEED_KP_MAX = 1.5f; const float YAW_SPEED_KI_MIN = 0.01f; const float YAW_SPEED_KI_MAX = 0.08f; const float YAW_SPEED_KD_MIN = 0.05f; const float YAW_SPEED_KD_MAX = 0.2f; // 高度环 const float HEIGHT_KP_MIN = 2.0f; const float HEIGHT_KP_MAX = 4.0f; const float HEIGHT_KI_MIN = 0.1f; const float HEIGHT_KI_MAX = 0.3f; const float HEIGHT_KD_MIN = 0.4f; const float HEIGHT_KD_MAX = 0.8f; // 垂直速度 const float VSPEED_KP_MIN = 2.0f; const float VSPEED_KP_MAX = 3.0f; const float VSPEED_KI_MIN = 0.1f; const float VSPEED_KI_MAX = 0.2f; const float VSPEED_KD_MIN = 0.4f; const float VSPEED_KD_MAX = 0.8f; // 垂直加速度 const float VACC_KP_MIN = 0.5f; const float VACC_KP_MAX = 1.5f; const float VACC_KI_MIN = 0.01f; const float VACC_KI_MAX = 0.05f; const float VACC_KD_MIN = 0.05f; const float VACC_KD_MAX = 0.15f; // 系统参数 const float TAKEOFF_HEIGHT_MIN = 0.5f; const float TAKEOFF_HEIGHT_MAX = 5.0f; const float RETURN_HEIGHT_MIN = 0.0f; const float RETURN_HEIGHT_MAX = 50.0f; const float BATTERY_MIN_MIN = 10.0f; const float BATTERY_MIN_MAX = 30.0f; const float MAX_HEIGHT_MIN = 5.0f; const float MAX_HEIGHT_MAX = 100.0f; const float MAX_RADIUS_MIN = 10.0f; const float MAX_RADIUS_MAX = 1000.0f; const float YAW_P_GAIN = 0.15f; // 偏航比例系数 // PID调试端口定义（和地面站保持一致） #define PID_DEBUG_PORT 14551 const float YAW_DAMPING = 0.1f; // 偏航角速度平滑系数（模拟飞控响应延迟） const float YAW_HOLD_THRESHOLD = 0.1f; // 角度保持阈值（小于该值则角速度归0） const float FLOAT_EQUAL_TOLERANCE = 0.1f; // 油门映射参数 const float THROTTLE_MIN = 1000.0f; // 最小油门PWM const float THROTTLE_MAX = 2000.0f; // 最大油门PWM const float THROTTLE_CENTER = 1500.0f; // 油门中位PWM const float THROTTLE_DEADZONE = 20.0f; // 油门死区（中位±20PWM） // 垂直运动物理参数 const float VERTICAL_ACC_MAX = 2.0f; // 最大垂直加速度 (m/s²) const float VERTICAL_SPEED_MAX = 5.0f; // 最大垂直速度 (m/s) const float HEIGHT_SMOOTH_FACTOR = 0.1f; // 高度平滑系数（模拟惯性） const float SPEED_SMOOTH_FACTOR = 0.2f; // 速度平滑系数 const float ACC_SMOOTH_FACTOR = 0.3f; // 加速度平滑系数 const float TAKEOFF_HEIGHT_TOLERANCE = 0.1f; // 起飞高度到达判定阈值 DroneDataWorker::DroneDataWorker(QObject *parent) : QObject(parent) , m_isRunning(false) , isGenerated(false) , m_nextParamIndex(0) , m_unlockState("锁定") // 初始化默认参数值 , m_pitchP((PITCH_ROLL_KP_MIN + PITCH_ROLL_KP_MAX) / 2) , m_pitchI((PITCH_ROLL_KI_MIN + PITCH_ROLL_KI_MAX) / 2) , m_pitchD((PITCH_ROLL_KD_MIN + PITCH_ROLL_KD_MAX) / 2) , m_pitchspeedP((PITCH_ROLL_SPEED_KP_MIN + PITCH_ROLL_SPEED_KP_MAX) / 2) , m_pitchspeedI((PITCH_ROLL_SPEED_KI_MIN + PITCH_ROLL_SPEED_KI_MAX) / 2) , m_pitchspeedD((PITCH_ROLL_SPEED_KD_MIN + PITCH_ROLL_SPEED_KD_MAX) / 2) , m_rollP((PITCH_ROLL_KP_MIN + PITCH_ROLL_KP_MAX) / 2) , m_rollI((PITCH_ROLL_KI_MIN + PITCH_ROLL_KI_MAX) / 2) , m_rollD((PITCH_ROLL_KD_MIN + PITCH_ROLL_KD_MAX) / 2) , m_rollspeedP((PITCH_ROLL_SPEED_KP_MIN + PITCH_ROLL_SPEED_KP_MAX) / 2) , m_rollspeedI((PITCH_ROLL_SPEED_KI_MIN + PITCH_ROLL_SPEED_KI_MAX) / 2) , m_rollspeedD((PITCH_ROLL_SPEED_KD_MIN + PITCH_ROLL_SPEED_KD_MAX) / 2) , m_yawP((YAW_KP_MIN + YAW_KP_MAX) / 2) , m_yawI((YAW_KI_MIN + YAW_KI_MAX) / 2) , m_yawD((YAW_KD_MIN + YAW_KD_MAX) / 2) , m_yawspeedP((YAW_SPEED_KP_MIN + YAW_SPEED_KP_MAX) / 2) , m_yawspeedI((YAW_SPEED_KI_MIN + YAW_SPEED_KI_MAX) / 2) , m_yawspeedD((YAW_SPEED_KD_MIN + YAW_SPEED_KD_MAX) / 2) , m_heightP((HEIGHT_KP_MIN + HEIGHT_KP_MAX) / 2) , m_heightI((HEIGHT_KI_MIN + HEIGHT_KI_MAX) / 2) , m_heightD((HEIGHT_KD_MIN + HEIGHT_KD_MAX) / 2) , m_vspeedP((VSPEED_KP_MIN + VSPEED_KP_MAX) / 2) , m_vspeedI((VSPEED_KI_MIN + VSPEED_KI_MAX) / 2) , m_vspeedD((VSPEED_KD_MIN + VSPEED_KD_MAX) / 2) , m_vaccP((VACC_KP_MIN + VACC_KP_MAX) / 2) , m_vaccI((VACC_KI_MIN + VACC_KI_MAX) / 2) , m_vaccD((VACC_KD_MIN + VACC_KD_MAX) / 2) , m_takeoffheight((TAKEOFF_HEIGHT_MIN + TAKEOFF_HEIGHT_MAX) / 2) , m_returnheight((RETURN_HEIGHT_MIN + RETURN_HEIGHT_MAX) / 2) , m_minbattery((BATTERY_MIN_MIN + BATTERY_MIN_MAX) / 2) , m_maxheight((MAX_HEIGHT_MIN + MAX_HEIGHT_MAX) / 2) , m_maxradius((MAX_RADIUS_MIN + MAX_RADIUS_MAX) / 2) // 状态参数初始化 , m_pitch(0.0f) , m_roll(0.0f) , m_yaw(0.0f) , m_height(0.0f) , m_verticalSpeed(0.0f) , m_verticalAcc(0.0f) , m_imuTemp(30.0f) , m_batteryVoltage(12.6f) , m_baroHeight(0.0f) , m_ultraHeight(0.0f) , m_optFlowP(0.0f) , m_optFlowR(0.0f) , m_longitude(104.0665f) , m_latitude(30.5728f) , m_altitude(0.0f) , m_speedN(0.0f) , m_speedE(0.0f) , m_gpsSatellites(10) , m_ax(0.0f) , m_ay(0.0f) , m_az(980.0f) , m_gx(0.0f) , m_gy(0.0f) , m_gz(0.0f) , m_mx(0.0f) , m_my(0.0f) , m_mz(0.0f) , m_currentWaypointIndex(-1) , m_isMissionActive(false) , m_uavLng(104.0665f) // 初始位置（与地面站家点默认值一致） , m_uavLat(30.5728f) , m_uavAlt(0.0f) , m_isAutoFlying(false) , m_isTakeoffing(false) , m_lastVerticalSpeed(0.0f) , m_targetVerticalAcc(0.0f) , m_throttleNormalized(0.5f) // 初始油门中位 { m_channelPwm[CH1] = PWM_CENTER; m_channelPwm[CH2] = PWM_CENTER; m_channelPwm[CH3] = PWM_CENTER; m_channelPwm[CH4] = PWM_CENTER; // 初始化主定时器（生成状态数据） m_timer = new QTimer(this); m_timer->setInterval(100); // 10Hz，可根据需要调整 // 初始化PID参数波动定时器 m_pidFluctuateTimer = new QTimer(this); m_pidFluctuateTimer->setInterval(200); // PID波动频率 5Hz（可调整） m_pidFluctuateTimer->setSingleShot(false); m_ch1PollTimer = new QTimer(this); m_ch1PollTimer->setInterval(20); // 20ms轮询一次（覆盖地面站高频数据） // 初始化随机数引擎 m_randEngine = std::default_random_engine(std::random_device{}()); m_pidDist = std::normal_distribution<float>(0.0f, 0.01f); // 原有小幅波动 m_pidFluctDist = std::normal_distribution<float>(0.0f, 0.005f); // 写入后额外轻微波动 m_dataDist = std::normal_distribution<float>(0.0f, 0.05f); // 初始化UDP发送器 m_udpsender = new UdpSender(this); // ========== 核心修改：移除m_pidUdpSocket，改用m_udpsender接收数据 ========== // 绑定UdpSender的数据接收信号，处理地面站的PID指令 connect(m_udpsender, &UdpSender::dataReceived, this, &DroneDataWorker::handleReceivedData); // 连接定时器到对应槽函数 connect(m_timer, &QTimer::timeout, this, &DroneDataWorker::generateAndSendData); connect(m_pidFluctuateTimer, &QTimer::timeout, this, &DroneDataWorker::fluctuatePidParams); connect(m_ch1PollTimer, &QTimer::timeout, this, &DroneDataWorker::updateFlightParamsByChannels); m_ch1PollTimer->start(); // 启动轮询 } DroneDataWorker::~DroneDataWorker() { stopGenerating(); delete m_udpsender; } void DroneDataWorker::setUdpConfig(const QHostAddress& localIp, quint16 localPort, const QHostAddress& targetIp, quint16 targetPort) { m_localIp = localIp; m_localPort = localPort; m_targetIp = targetIp; m_targetPort = targetPort; // 设置UdpSender的目标IP/端口 m_udpsender->setTarget(targetIp, targetPort); // 绑定本地IP/端口 bool bindOk = m_udpsender->bindLocalPort(localIp, localPort); if (bindOk) { emit logMessage(QString("UDP配置已生效：本地%1:%2，目标%3:%4") .arg(localIp.toString()).arg(localPort) .arg(targetIp.toString()).arg(targetPort)); } else { emit logMessage(QString("UDP本地端口绑定失败：%1:%2").arg(localIp.toString()).arg(localPort)); } } void DroneDataWorker::startGenerating() { // 加锁保护运行状态，防止重复启动 QMutexLocker locker(&m_runningMutex); if (m_isRunning) { emit logMessage("数据生成已在运行中，无需重复启动"); return; } // 绑定前先确保端口未被绑定 if (m_udpsender) { m_udpsender->unbindLocalPort(); } // 绑定本地UDP端口（同时用于发送状态数据和接收PID指令） if (!m_udpsender->bindLocalPort(m_localIp, m_localPort)) { emit logMessage("本地端口绑定失败！"); return; } // 设置UDP目标（地面站地址） m_udpsender->setTarget(m_targetIp, m_targetPort); m_isRunning = true; // 先停止定时器再启动，防止重复启动 m_timer->stop(); m_timer->start(); emit logMessage(QString("数据生成线程启动，发送数据到 %1:%2，本地监听端口 %3") .arg(m_targetIp.toString()).arg(m_targetPort).arg(m_localPort)); } void DroneDataWorker::stopGenerating() { m_timer->stop(); m_pidFluctuateTimer->stop(); // 停止PID波动定时器 m_isRunning = false; if (m_udpsender) { m_udpsender->unbindLocalPort(); } emit logMessage("数据生成线程已停止"); isGenerated = false; } // ========== 核心修改：统一处理UdpSender接收的所有数据 ========== void DroneDataWorker::handleReceivedData(const QByteArray& data, const QHostAddress& senderIp, quint16 senderPort) { if (data.isEmpty()) { return; } Q_ASSERT(QThread::currentThread() == this->thread()); // 确保在工作线程执行 // 优先处理二进制PID指令 handlePidCommand(data); // 解析MAVLink指令 mavlink_message_t msg; mavlink_status_t status; for (int i = 0; i < data.size(); i++) { uint8_t byte = static_cast<uint8_t>(data.at(i)); if (mavlink_parse_char(MAVLINK_COMM_0, byte, &msg, &status)) { parseMavlinkCommand(msg); } } } void DroneDataWorker::generateAndSendData() { // 生成带随机波动的模拟数据 QMutexLocker locker(&m_pidParamMutex); // ========== PID参数波动并限制在指定范围 ========== // 角度环（俯仰/横滚） m_pitchP = qBound(PITCH_ROLL_KP_MIN, m_pitchP + m_pidDist(m_randEngine), PITCH_ROLL_KP_MAX); m_pitchI = qBound(PITCH_ROLL_KI_MIN, m_pitchI + m_pidDist(m_randEngine), PITCH_ROLL_KI_MAX); m_pitchD = qBound(PITCH_ROLL_KD_MIN, m_pitchD + m_pidDist(m_randEngine), PITCH_ROLL_KD_MAX); m_rollP = qBound(PITCH_ROLL_KP_MIN, m_rollP + m_pidDist(m_randEngine), PITCH_ROLL_KP_MAX); m_rollI = qBound(PITCH_ROLL_KI_MIN, m_rollI + m_pidDist(m_randEngine), PITCH_ROLL_KI_MAX); m_rollD = qBound(PITCH_ROLL_KD_MIN, m_rollD + m_pidDist(m_randEngine), PITCH_ROLL_KD_MAX); // 角速度环（俯仰/横滚） m_pitchspeedP = qBound(PITCH_ROLL_SPEED_KP_MIN, m_pitchspeedP + m_pidDist(m_randEngine), PITCH_ROLL_SPEED_KP_MAX); m_pitchspeedI = qBound(PITCH_ROLL_SPEED_KI_MIN, m_pitchspeedI + m_pidDist(m_randEngine), PITCH_ROLL_SPEED_KI_MAX); m_pitchspeedD = qBound(PITCH_ROLL_SPEED_KD_MIN, m_pitchspeedD + m_pidDist(m_randEngine), PITCH_ROLL_SPEED_KD_MAX); m_rollspeedP = qBound(PITCH_ROLL_SPEED_KP_MIN, m_rollspeedP + m_pidDist(m_randEngine), PITCH_ROLL_SPEED_KP_MAX); m_rollspeedI = qBound(PITCH_ROLL_SPEED_KI_MIN, m_rollspeedI + m_pidDist(m_randEngine), PITCH_ROLL_SPEED_KI_MAX); m_rollspeedD = qBound(PITCH_ROLL_SPEED_KD_MIN, m_rollspeedD + m_pidDist(m_randEngine), PITCH_ROLL_SPEED_KD_MAX); // 偏航角 m_yawP = qBound(YAW_KP_MIN, m_yawP + m_pidDist(m_randEngine), YAW_KP_MAX); m_yawI = qBound(YAW_KI_MIN, m_yawI + m_pidDist(m_randEngine), YAW_KI_MAX); m_yawD = qBound(YAW_KD_MIN, m_yawD + m_pidDist(m_randEngine), YAW_KD_MAX); // 偏航角速度 m_yawspeedP = qBound(YAW_SPEED_KP_MIN, m_yawspeedP + m_pidDist(m_randEngine), YAW_SPEED_KP_MAX); m_yawspeedI = qBound(YAW_SPEED_KI_MIN, m_yawspeedI + m_pidDist(m_randEngine), YAW_SPEED_KI_MAX); m_yawspeedD = qBound(YAW_SPEED_KD_MIN, m_yawspeedD + m_pidDist(m_randEngine), YAW_SPEED_KD_MAX); // 高度环 m_heightP = qBound(HEIGHT_KP_MIN, m_heightP + m_pidDist(m_randEngine), HEIGHT_KP_MAX); m_heightI = qBound(HEIGHT_KI_MIN, m_heightI + m_pidDist(m_randEngine), HEIGHT_KI_MAX); m_heightD = qBound(HEIGHT_KD_MIN, m_heightD + m_pidDist(m_randEngine), HEIGHT_KD_MAX); // 垂直速度 m_vspeedP = qBound(VSPEED_KP_MIN, m_vspeedP + m_pidDist(m_randEngine), VSPEED_KP_MAX); m_vspeedI = qBound(VSPEED_KI_MIN, m_vspeedI + m_pidDist(m_randEngine), VSPEED_KI_MAX); m_vspeedD = qBound(VSPEED_KD_MIN, m_vspeedD + m_pidDist(m_randEngine), VSPEED_KD_MAX); // 垂直加速度 m_vaccP = qBound(VACC_KP_MIN, m_vaccP + m_pidDist(m_randEngine), VACC_KP_MAX); m_vaccI = qBound(VACC_KI_MIN, m_vaccI + m_pidDist(m_randEngine), VACC_KI_MAX); m_vaccD = qBound(VACC_KD_MIN, m_vaccD + m_pidDist(m_randEngine), VACC_KD_MAX); // 系统参数 m_takeoffheight = qBound(TAKEOFF_HEIGHT_MIN, m_takeoffheight + m_pidDist(m_randEngine), TAKEOFF_HEIGHT_MAX); m_returnheight = qBound(RETURN_HEIGHT_MIN, m_returnheight + m_pidDist(m_randEngine), RETURN_HEIGHT_MAX); m_minbattery = qBound(BATTERY_MIN_MIN, m_minbattery + m_pidDist(m_randEngine), BATTERY_MIN_MAX); m_maxheight = qBound(MAX_HEIGHT_MIN, m_maxheight + m_pidDist(m_randEngine), MAX_HEIGHT_MAX); m_maxradius = qBound(MAX_RADIUS_MIN, m_maxradius + m_pidDist(m_randEngine), MAX_RADIUS_MAX); locker.unlock(); // // 无人机状态参数波动（原有逻辑不变） // updateFlightParamsByChannels();//频率不同步，这里使用频率过低 // 保留原有少量随机波动（模拟传感器噪声） m_imuTemp = 30.0f + m_dataDist(m_randEngine) * 0.5f; m_batteryVoltage -= 0.001f * m_timer->interval() / 1000.0f; m_gpsSatellites = qMax(8, 12 + (int)(m_dataDist(m_randEngine) * 2)); m_ax += m_dataDist(m_randEngine) * 0.01f; m_ay += m_dataDist(m_randEngine) * 0.01f; m_az = (m_verticalAcc + 9.81f) * 1000.0f; // 真实垂直加速度 + 重力 → 转换为mg m_az += m_dataDist(m_randEngine) * 0.5f; // 保留噪声 m_gx += m_dataDist(m_randEngine) * 0.01f; m_gy += m_dataDist(m_randEngine) * 0.01f; m_gz += m_dataDist(m_randEngine) * 0.01f; m_mx += m_dataDist(m_randEngine) * 0.01f; m_my += m_dataDist(m_randEngine) * 0.01f; m_mz += m_dataDist(m_randEngine) * 0.01f; // ========== 同步无人机位置到状态参数（供MAVLink发送） ========== m_longitude = m_uavLng; m_latitude = m_uavLat; // m_height = m_uavAlt; // 打包并发送MAVLink消息（仅状态数据，无PID） packMavlinkMessages(); isGenerated = true; emit dataSent(); } // PID参数随时间波动的槽函数 void DroneDataWorker::fluctuatePidParams() { QMutexLocker locker(&m_pidParamMutex); // PID参数缓慢漂移并限制在指定范围 // 角度环（俯仰/横滚） m_pitchP = qBound(PITCH_ROLL_KP_MIN, m_pitchP + m_pidFluctDist(m_randEngine), PITCH_ROLL_KP_MAX); m_pitchI = qBound(PITCH_ROLL_KI_MIN, m_pitchI + m_pidFluctDist(m_randEngine), PITCH_ROLL_KI_MAX); m_pitchD = qBound(PITCH_ROLL_KD_MIN, m_pitchD + m_pidFluctDist(m_randEngine), PITCH_ROLL_KD_MAX); m_rollP = qBound(PITCH_ROLL_KP_MIN, m_rollP + m_pidFluctDist(m_randEngine), PITCH_ROLL_KP_MAX); m_rollI = qBound(PITCH_ROLL_KI_MIN, m_rollI + m_pidFluctDist(m_randEngine), PITCH_ROLL_KI_MAX); m_rollD = qBound(PITCH_ROLL_KD_MIN, m_rollD + m_pidFluctDist(m_randEngine), PITCH_ROLL_KD_MAX); // 角速度环（俯仰/横滚） m_pitchspeedP = qBound(PITCH_ROLL_SPEED_KP_MIN, m_pitchspeedP + m_pidFluctDist(m_randEngine), PITCH_ROLL_SPEED_KP_MAX); m_pitchspeedI = qBound(PITCH_ROLL_SPEED_KI_MIN, m_pitchspeedI + m_pidFluctDist(m_randEngine), PITCH_ROLL_SPEED_KI_MAX); m_pitchspeedD = qBound(PITCH_ROLL_SPEED_KD_MIN, m_pitchspeedD + m_pidFluctDist(m_randEngine), PITCH_ROLL_SPEED_KD_MAX); m_rollspeedP = qBound(PITCH_ROLL_SPEED_KP_MIN, m_rollspeedP + m_pidFluctDist(m_randEngine), PITCH_ROLL_SPEED_KP_MAX); m_rollspeedI = qBound(PITCH_ROLL_SPEED_KI_MIN, m_rollspeedI + m_pidFluctDist(m_randEngine), PITCH_ROLL_SPEED_KI_MAX); m_rollspeedD = qBound(PITCH_ROLL_SPEED_KD_MIN, m_rollspeedD + m_pidFluctDist(m_randEngine), PITCH_ROLL_SPEED_KD_MAX); // 偏航角 m_yawP = qBound(YAW_KP_MIN, m_yawP + m_pidFluctDist(m_randEngine), YAW_KP_MAX); m_yawI = qBound(YAW_KI_MIN, m_yawI + m_pidFluctDist(m_randEngine), YAW_KI_MAX); m_yawD = qBound(YAW_KD_MIN, m_yawD + m_pidFluctDist(m_randEngine), YAW_KD_MAX); // 偏航角速度 m_yawspeedP = qBound(YAW_SPEED_KP_MIN, m_yawspeedP + m_pidFluctDist(m_randEngine), YAW_SPEED_KP_MAX); m_yawspeedI = qBound(YAW_SPEED_KI_MIN, m_yawspeedI + m_pidFluctDist(m_randEngine), YAW_SPEED_KI_MAX); m_yawspeedD = qBound(YAW_SPEED_KD_MIN, m_yawspeedD + m_pidFluctDist(m_randEngine), YAW_SPEED_KD_MAX); // 高度环 m_heightP = qBound(HEIGHT_KP_MIN, m_heightP + m_pidFluctDist(m_randEngine), HEIGHT_KP_MAX); m_heightI = qBound(HEIGHT_KI_MIN, m_heightI + m_pidFluctDist(m_randEngine), HEIGHT_KI_MAX); m_heightD = qBound(HEIGHT_KD_MIN, m_heightD + m_pidFluctDist(m_randEngine), HEIGHT_KD_MAX); // 垂直速度 m_vspeedP = qBound(VSPEED_KP_MIN, m_vspeedP + m_pidFluctDist(m_randEngine), VSPEED_KP_MAX); m_vspeedI = qBound(VSPEED_KI_MIN, m_vspeedI + m_pidFluctDist(m_randEngine), VSPEED_KI_MAX); m_vspeedD = qBound(VSPEED_KD_MIN, m_vspeedD + m_pidFluctDist(m_randEngine), VSPEED_KD_MAX); // 垂直加速度 m_vaccP = qBound(VACC_KP_MIN, m_vaccP + m_pidFluctDist(m_randEngine), VACC_KP_MAX); m_vaccI = qBound(VACC_KI_MIN, m_vaccI + m_pidFluctDist(m_randEngine), VACC_KI_MAX); m_vaccD = qBound(VACC_KD_MIN, m_vaccD + m_pidFluctDist(m_randEngine), VACC_KD_MAX); // 系统参数 m_takeoffheight = qBound(TAKEOFF_HEIGHT_MIN, m_takeoffheight + m_pidFluctDist(m_randEngine), TAKEOFF_HEIGHT_MAX); m_returnheight = qBound(RETURN_HEIGHT_MIN, m_returnheight + m_pidFluctDist(m_randEngine), RETURN_HEIGHT_MAX); m_minbattery = qBound(BATTERY_MIN_MIN, m_minbattery + m_pidFluctDist(m_randEngine), BATTERY_MIN_MAX); m_maxheight = qBound(MAX_HEIGHT_MIN, m_maxheight + m_pidFluctDist(m_randEngine), MAX_HEIGHT_MAX); m_maxradius = qBound(MAX_RADIUS_MIN, m_maxradius + m_pidFluctDist(m_randEngine), MAX_RADIUS_MAX); } void DroneDataWorker::isGenerateData() { if(isGenerated){ emit logMessage("数据生成完成（线程内）"); } } void DroneDataWorker::packMavlinkMessages() { mavlink_message_t msg; uint8_t buf[MAVLINK_MAX_PACKET_LEN] = {0}; uint16_t len = 0; // 定义base_mode（基础模式位掩码） uint8_t base_mode = 0; if (m_flightMode == "手动模式") { base_mode = MAV_MODE_MANUAL_ARMED; // 手动模式（解锁状态） } // 自动模式：设置MAV_MODE_AUTO_ARMED（自动使能） else if (m_flightMode == "自动模式") { base_mode = MAV_MODE_AUTO_ARMED; // 自动模式（解锁状态） } else if(m_flightMode == "未知模式"){ base_mode = 0; } // 1. 心跳消息（无高度相关字段，逻辑不变） uint8_t system_status = MAV_STATE_STANDBY; if (m_unlockState == "解锁") system_status = MAV_STATE_ACTIVE; mavlink_msg_heartbeat_pack( 1, MAV_COMP_ID_AUTOPILOT, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC, base_mode, 0, system_status ); len = mavlink_msg_to_send_buffer(buf, &msg); m_udpsender->sendData(QByteArray((const char*)buf, len)); // 2. 姿态消息（无高度相关字段，逻辑不变） mavlink_msg_attitude_pack( 1, MAV_COMP_ID_AUTOPILOT, &msg, // 统一组件ID为MAV_COMP_ID_AUTOPILOT QDateTime::currentMSecsSinceEpoch(), m_roll * M_PI / 180.0f, m_pitch * M_PI / 180.0f, m_yaw * M_PI / 180.0f, m_gx * M_PI / 180.0f, // rollspeed（横滚角速度，rad/s） m_gy * M_PI / 180.0f, // pitchspeed（俯仰角速度，rad/s） m_gz * M_PI / 180.0f // yawspeed（偏航角速度，rad/s）→ 不再填充垂直加速度 ); len = mavlink_msg_to_send_buffer(buf, &msg); m_udpsender->sendData(QByteArray((const char*)buf, len)); mavlink_msg_global_position_int_pack( 1, MAV_COMP_ID_AUTOPILOT, &msg, QDateTime::currentMSecsSinceEpoch(), // time_boot_ms：系统启动后毫秒数 0, // 纬度（×1e7 度），此处不设置 0, // 经度（×1e7 度），此处不设置 (int32_t)(m_altitude * 1000), // alt：海拔高度（MSL，mm）核心海拔字段 (int32_t)(m_height * 1000), // relative_alt：相对起飞点高度（mm） (int16_t)(m_speedN * 100), // vx：北方向速度（cm/s） (int16_t)(m_speedE * 100), // vy：东方向速度（cm/s） (int16_t)(m_verticalSpeed * 100), // vz：垂直速度（cm/s） 0 // hdg：航向（×100 度，0-36000对应0-360°） ); len = mavlink_msg_to_send_buffer(buf, &msg); m_udpsender->sendData(QByteArray((const char*)buf, len)); // 4. 修改：GPS_RAW_INT消息（移除高度相关字段，仅保留必要非高度字段） // 注意：alt字段置0（不再通过GPS消息传递高度），仅保留定位状态、经纬度、卫星数等 mavlink_msg_gps_raw_int_pack( 1, MAV_COMP_ID_AUTOPILOT, &msg, QDateTime::currentMSecsSinceEpoch() * 1000, // time_usec：微秒时间戳 3, // fix_type：3D定位 (int32_t)(m_latitude * 1e7), // 纬度（×1e7 度） (int32_t)(m_longitude * 1e7), // 经度（×1e7 度） 0, // alt：高度字段置0（不再使用） 100, // eph：水平精度 200, // epv：垂直精度（保留但无实际意义） (uint16_t)(sqrt(m_speedN*m_speedN + m_speedE*m_speedE) * 100), // 地速 0, // cog：航向,此处不上传 m_gpsSatellites, // 可见卫星数 0, 0, 0, 0, 0, MAV_SYS_ID_ALL // 扩展字段全部置0（含高度相关） ); len = mavlink_msg_to_send_buffer(buf, &msg); m_udpsender->sendData(QByteArray((const char*)buf, len)); // 5. 修改：VFR_HUD消息（移除高度相关字段，仅保留必要非高度字段） // alt字段置0，climb字段置0（不再通过VFR HUD传递高度/爬升率） mavlink_msg_vfr_hud_pack( 1, MAV_COMP_ID_AUTOPILOT, &msg, 0.0f, // airspeed：空速 sqrt(m_speedN*m_speedN + m_speedE*m_speedE), // groundspeed：地速 0, // heading：航向，此处不设置 50, // throttle：油门 0.0f, // alt：高度字段置0 m_verticalSpeed // climb：爬升率置0 ); len = mavlink_msg_to_send_buffer(buf, &msg); m_udpsender->sendData(QByteArray((const char*)buf, len)); // 5. 扩展IMU消息（原有逻辑不变） mavlink_msg_scaled_imu2_pack( 1, 1, &msg, QDateTime::currentMSecsSinceEpoch(), (int16_t)(m_ax * 1000), (int16_t)(m_ay * 1000), (int16_t)(m_az), (int16_t)(m_gx * 1000), (int16_t)(m_gy * 1000), (int16_t)(m_gz * 1000), (int16_t)(m_mx * 1000), (int16_t)(m_my * 1000), (int16_t)(m_mz * 1000), (int16_t)(m_imuTemp * 100) ); len = mavlink_msg_to_send_buffer(buf, &msg); m_udpsender->sendData(QByteArray((const char*)buf, len)); } void DroneDataWorker::sendParamValue(const char* paramName, float value) { mavlink_message_t msg; uint8_t buf[MAVLINK_MAX_PACKET_LEN] = {0}; uint16_t len = 0; uint8_t param_type = MAV_PARAM_TYPE_REAL32; union { float f; uint32_t i; } param_value; param_value.f = value; mavlink_msg_param_value_pack( 1, 1, &msg, paramName, param_value.i, param_type, 32, m_nextParamIndex++ ); if (m_nextParamIndex >= 32) { m_nextParamIndex = 0; } len = mavlink_msg_to_send_buffer(buf, &msg); m_udpsender->sendData(QByteArray((const char*)buf, len)); } // 仅响应读取指令时发送所有PID参数 void DroneDataWorker::sendAllPidParams() { QMutexLocker locker(&m_pidParamMutex); // 加锁保护PID参数 QByteArray ackData; ackData.resize(sizeof(uint32_t) + sizeof(uint8_t)); uint32_t ackCmd = 0x50494441; // "PIDA" 标识PID确认 uint8_t ackType = m_lastPidCmdType; // 1=读取成功, 2=写入成功 memcpy(ackData.data(), &ackCmd, sizeof(uint32_t)); memcpy(ackData.data() + sizeof(uint32_t), &ackType, sizeof(uint8_t)); m_udpsender->sendData(ackData); sendParamValue("PITCH_P", m_pitchP); sendParamValue("PITCH_I", m_pitchI); sendParamValue("PITCH_D", m_pitchD); sendParamValue("PITCHSPEED_P", m_pitchspeedP); sendParamValue("PITCHSPEED_I", m_pitchspeedI); sendParamValue("PITCHSPEED_D", m_pitchspeedD); sendParamValue("ROLL_P", m_rollP); sendParamValue("ROLL_I", m_rollI); sendParamValue("ROLL_D", m_rollD); sendParamValue("ROLLSPEED_P", m_rollspeedP); sendParamValue("ROLLSPEED_I", m_rollspeedI); sendParamValue("ROLLSPEED_D", m_rollspeedD); sendParamValue("YAW_P", m_yawP); sendParamValue("YAW_I", m_yawI); sendParamValue("YAW_D", m_yawD); sendParamValue("YAWSPEED_P", m_yawspeedP); sendParamValue("YAWSPEED_I", m_yawspeedI); sendParamValue("YAWSPEED_D", m_yawspeedD); sendParamValue("HEIGHT_P", m_heightP); sendParamValue("HEIGHT_I", m_heightI); sendParamValue("HEIGHT_D", m_heightD); sendParamValue("VSPEED_P", m_vspeedP); sendParamValue("VSPEED_I", m_vspeedI); sendParamValue("VSPEED_D", m_vspeedD); sendParamValue("VACC_P", m_vaccP); sendParamValue("VACC_I", m_vaccI); sendParamValue("VACC_D", m_vaccD); sendParamValue("TAKEOFFHEIGHT", m_takeoffheight); sendParamValue("RETURNHEIGHT", m_returnheight); sendParamValue("MINBATTERY", m_minbattery); sendParamValue("MAXHEIGHT", m_maxheight); sendParamValue("MAXRADIUS", m_maxradius); emit logMessage("已响应读取指令，发送所有PID参数"); } // 核心：处理PID指令（读取/写入/默认） void DroneDataWorker::handlePidCommand(const QByteArray& data) { if (data.isEmpty()) return; // 解析指令类型 uint32_t cmd = 0; const char* dataPtr = data.constData(); memcpy(&cmd, dataPtr, sizeof(uint32_t)); switch (cmd) { case 0x50494452: { // CMD_READ_PARAMS - 读取参数 emit logMessage("收到地面站读取参数指令，准备回传PID参数"); // 方式1：MAVLink格式回传（兼容地面站） m_lastPidCmdType = 1; sendAllPidParams(); // 方式2：二进制数组回传（兼容PID对话框） float params[32] = { m_pitchP, m_pitchI, m_pitchD, m_pitchspeedP, m_pitchspeedI, m_pitchspeedD, m_rollP, m_rollI, m_rollD, m_rollspeedP, m_rollspeedI, m_rollspeedD, m_yawP, m_yawI, m_yawD, m_yawspeedP, m_yawspeedI, m_yawspeedD, m_heightP, m_heightI, m_heightD, m_vspeedP, m_vspeedI, m_vspeedD, m_vaccP, m_vaccI, m_vaccD, m_takeoffheight, m_returnheight, m_minbattery, m_maxheight, m_maxradius }; QByteArray datagram; datagram.resize(32 * sizeof(float)); memcpy(datagram.data(), params, 32 * sizeof(float)); QThread::msleep(5); // 5ms延迟 m_udpsender->sendData(datagram); // 回传给PID对话框 QThread::msleep(10); // 再延迟，确保确认指令先到达 break; } case 0x50494457: { // CMD_WRITE_PARAMS - 写入参数 if (data.size() < sizeof(uint32_t) + 32 * sizeof(float)) { emit logMessage("写入参数数据长度不足"); return; } // 解析参数 const char* paramStart = dataPtr + sizeof(uint32_t); float* params = reinterpret_cast<float*>(const_cast<char*>(paramStart)); // 加锁更新参数（线程安全） QMutexLocker locker(&m_pidParamMutex); // 写入参数时限制在指定范围 // 角度环（俯仰/横滚） m_pitchP = qBound(PITCH_ROLL_KP_MIN, params[0], PITCH_ROLL_KP_MAX); m_pitchI = qBound(PITCH_ROLL_KI_MIN, params[1], PITCH_ROLL_KI_MAX); m_pitchD = qBound(PITCH_ROLL_KD_MIN, params[2], PITCH_ROLL_KD_MAX); // 角速度环（俯仰/横滚） m_pitchspeedP = qBound(PITCH_ROLL_SPEED_KP_MIN, params[3], PITCH_ROLL_SPEED_KP_MAX); m_pitchspeedI = qBound(PITCH_ROLL_SPEED_KI_MIN, params[4], PITCH_ROLL_SPEED_KI_MAX); m_pitchspeedD = qBound(PITCH_ROLL_SPEED_KD_MIN, params[5], PITCH_ROLL_SPEED_KD_MAX); // 角度环（横滚） m_rollP = qBound(PITCH_ROLL_KP_MIN, params[6], PITCH_ROLL_KP_MAX); m_rollI = qBound(PITCH_ROLL_KI_MIN, params[7], PITCH_ROLL_KI_MAX); m_rollD = qBound(PITCH_ROLL_KD_MIN, params[8], PITCH_ROLL_KD_MAX); // 角速度环（横滚） m_rollspeedP = qBound(PITCH_ROLL_SPEED_KP_MIN, params[9], PITCH_ROLL_SPEED_KP_MAX); m_rollspeedI = qBound(PITCH_ROLL_SPEED_KI_MIN, params[10], PITCH_ROLL_SPEED_KI_MAX); m_rollspeedD = qBound(PITCH_ROLL_SPEED_KD_MIN, params[11], PITCH_ROLL_SPEED_KD_MAX); // 偏航角 m_yawP = qBound(YAW_KP_MIN, params[12], YAW_KP_MAX); m_yawI = qBound(YAW_KI_MIN, params[13], YAW_KI_MAX); m_yawD = qBound(YAW_KD_MIN, params[14], YAW_KD_MAX); // 偏航角速度 m_yawspeedP = qBound(YAW_SPEED_KP_MIN, params[15], YAW_SPEED_KP_MAX); m_yawspeedI = qBound(YAW_SPEED_KI_MIN, params[16], YAW_SPEED_KI_MAX); m_yawspeedD = qBound(YAW_SPEED_KD_MIN, params[17], YAW_SPEED_KD_MAX); // 高度环 m_heightP = qBound(HEIGHT_KP_MIN, params[18], HEIGHT_KP_MAX); m_heightI = qBound(HEIGHT_KI_MIN, params[19], HEIGHT_KI_MAX); m_heightD = qBound(HEIGHT_KD_MIN, params[20], HEIGHT_KD_MAX); // 垂直速度 m_vspeedP = qBound(VSPEED_KP_MIN, params[21], VSPEED_KP_MAX); m_vspeedI = qBound(VSPEED_KI_MIN, params[22], VSPEED_KI_MAX); m_vspeedD = qBound(VSPEED_KD_MIN, params[23], VSPEED_KD_MAX); // 垂直加速度 m_vaccP = qBound(VACC_KP_MIN, params[24], VACC_KP_MAX); m_vaccI = qBound(VACC_KI_MIN, params[25], VACC_KI_MAX); m_vaccD = qBound(VACC_KD_MIN, params[26], VACC_KD_MAX); // 系统参数 m_takeoffheight = qBound(TAKEOFF_HEIGHT_MIN, params[27], TAKEOFF_HEIGHT_MAX); m_returnheight = qBound(RETURN_HEIGHT_MIN, params[28], RETURN_HEIGHT_MAX); m_minbattery = qBound(BATTERY_MIN_MIN, params[29], BATTERY_MIN_MAX); m_maxheight = qBound(MAX_HEIGHT_MIN, params[30], MAX_HEIGHT_MAX); m_maxradius = qBound(MAX_RADIUS_MIN, params[31], MAX_RADIUS_MAX); locker.unlock(); emit logMessage("成功接收地面站写入的PID参数，已更新本地参数"); // 启动PID参数波动定时器（如果未启动） if (!m_pidFluctuateTimer->isActive()) { // 随机调整波动定时器间隔（±50ms），模拟时间波动 int baseInterval = m_pidFluctuateTimer->interval(); int randomOffset = (rand() % 101) - 50; // -50 ~ +50ms m_pidFluctuateTimer->setInterval(baseInterval + randomOffset); m_pidFluctuateTimer->start(); } m_lastPidCmdType = 2; // 回传确认：发送更新后的参数 sendAllPidParams(); break; } default: // emit logMessage(QString("未知指令类型：0x%1").arg(cmd, 8, 16, QChar('0'))); break; } } // 1. 实现getChannelPwm方法 int DroneDataWorker::getChannelPwm(int channel) const { if ((channel < 1) || (channel > 4)) { return 1500; // 默认中位 } QMutexLocker locker(&m_channelMutex); return m_channelPwm[channel]; } // 2. 实现SERVO_OUTPUT_RAW解析函数 void DroneDataWorker::parseServoOutputRaw(const mavlink_message_t& msg) { mavlink_servo_output_raw_t servoData; mavlink_msg_servo_output_raw_decode(&msg, &servoData); QMutexLocker locker(&m_channelMutex); m_channelPwm[CH1] = servoData.servo1_raw; m_channelPwm[CH2] = servoData.servo2_raw; m_channelPwm[CH3] = servoData.servo3_raw; m_channelPwm[CH4] = servoData.servo4_raw; // 提取CH1-CH4的PWM值 static int ch1PrintCount = 0; static int ch1Count = 0; static int ch2PrintCount = 0; static int ch2Count = 0; static int msgCount = 0; if(m_channelPwm[CH1] != 1500){ ch1PrintCount++; } if(m_channelPwm[CH2] != 1500){ ch2PrintCount++; } ch1Count++; ch2Count++; // 每接收10条消息，打印一次统计结果 msgCount++; if (msgCount % 10 == 0) { qDebug() << "===== 统计 ====="; qDebug() << "接收MAVLink消息数：" << msgCount; qDebug() << "ch1!=1500打印次数：" << ch1PrintCount; qDebug() << "ch2!=1500打印次数：" << ch2PrintCount; qDebug() << "ch1打印次数：" << ch1Count; qDebug() << "ch2打印次数：" << ch2Count; qDebug() << "================="; } } // 通道PWM解析与参数控制 void DroneDataWorker::updateFlightParamsByChannels() { QMutexLocker channelLocker(&m_channelMutex); // 第一步：仅读取通道值时加锁 int ch1, ch2, ch3, ch4; static int ch1count = 0; static int ch2count = 0; if(m_channelPwm[CH1] != 1500){ ch1count++; qDebug() << "channel1" << ch1count; } if(m_channelPwm[CH2] != 1500){ ch2count++; qDebug() << "channel2" << ch2count; } static int c1count = 0; static int c2count = 0; ch1 = qBound(PWM_MIN, m_channelPwm[CH1], PWM_MAX); if(ch1 != 1500){ c1count++; qDebug() << "ch1" << c1count; } int rollDelta = ch1 - PWM_CENTER; // 死区处理 if (abs(rollDelta) < PWM_DEADZONE) { m_targetRoll = 0.0f; } else { // 线性映射：PWM偏移量→目标横滚角（确保比例正确） float rollRatio = (float)rollDelta / (PWM_MAX - PWM_CENTER); m_targetRoll = rollRatio * ROLL_MAX_ANGLE; } // 极值判断：改用整数判断（避免浮点精度问题） if (abs(rollDelta) >= (PWM_MAX - PWM_CENTER) - 1) { // 允许1个PWM单位误差 // 摇杆拉满，直接赋值（无平滑衰减） m_roll = m_targetRoll; } else { // 非极值，平滑过渡 m_roll = m_roll + (m_targetRoll - m_roll) * ANGLE_DAMPING; } // 最终限幅：确保横滚角在物理范围内 m_roll = qBound(-ROLL_MAX_ANGLE, m_roll, ROLL_MAX_ANGLE); // ========== 2. 俯仰控制（CH2） ========== ch2 = qBound(PWM_MIN, m_channelPwm[CH2], PWM_MAX); int pitchDelta = ch2 - PWM_CENTER; if(ch2 != 1500){ c2count++; qDebug() << "ch2" << c2count; } if (abs(pitchDelta) < PWM_DEADZONE) { m_targetPitch = 0.0f; } else { float pitchRatio = (float)(pitchDelta) / (PWM_MAX - PWM_CENTER); m_targetPitch = pitchRatio * PITCH_MAX_ANGLE; } if (abs(m_targetPitch - ROLL_MAX_ANGLE) < 0.01f || abs(m_targetPitch + ROLL_MAX_ANGLE) < 0.01f) { // 目标是最大/最小横滚角，直接赋值（不再平滑，避免衰减） m_pitch = m_targetPitch; }else{ m_pitch = m_pitch + (m_targetPitch - m_pitch) * ANGLE_DAMPING; } m_pitch = qBound(-PITCH_MAX_ANGLE, m_pitch, PITCH_MAX_ANGLE); // ========== 3. 油门/垂直控制（CH3）- 参考横滚角逻辑 + 线性映射 + 极值锁死 ========== float deltaTime = m_timer->interval() / 1000.0f; // 时间间隔（秒） ch3 = qBound(PWM_MIN, m_channelPwm[CH3], PWM_MAX); // 油门通道值（1000~2000） // 步骤1：计算油门偏移量（参考横滚角的delta逻辑） int throttleDelta = ch3 - THROTTLE_CENTER; // 偏移量：-500~+500 // 步骤2：死区处理（参考横滚角的死区逻辑） if (abs(throttleDelta) < THROTTLE_DEADZONE) { m_targetVerticalAcc = 0.0f; // 死区内目标加速度为0 } else { // 步骤3：CH3与加速度线性映射（-2~2m/s²，严格线性） float throttleRatio = (float)(throttleDelta) / (PWM_MAX - PWM_CENTER); m_targetVerticalAcc = throttleRatio * VERTICAL_ACC_MAX; // 1000→-2，2000→+2 } // 步骤4：参考横滚角逻辑 - 极值直接赋值（锁死最大/最小加速度，避免归零） if (fabs(m_targetVerticalAcc - VERTICAL_ACC_MAX) < 0.01f || fabs(m_targetVerticalAcc + VERTICAL_ACC_MAX) < 0.01f) { // 摇杆拉满（目标加速度是±2m/s²极值）：直接赋值，不衰减、不归零 m_cmdVerticalAcc = m_targetVerticalAcc; } else { // 非极值（摇杆半推）：保留平滑过渡逻辑（可选，也可直接赋值） m_cmdVerticalAcc = m_cmdVerticalAcc + (m_targetVerticalAcc - m_cmdVerticalAcc) * ACC_DAMPING; } // 步骤5：加速度最终限幅（确保不超出-2~2范围） m_cmdVerticalAcc = qBound(-VERTICAL_ACC_MAX, m_cmdVerticalAcc, VERTICAL_ACC_MAX); // 步骤6：速度积分（核心逻辑不变，加速度控速度） float newVerticalSpeed = m_verticalSpeed + m_cmdVerticalAcc * deltaTime; // 步骤7：速度约束（仅限制速度上限，不影响加速度逻辑） newVerticalSpeed = qBound(-VERTICAL_SPEED_MAX, newVerticalSpeed, VERTICAL_SPEED_MAX); // 步骤8：起飞场景特殊处理（原有逻辑保留） if (m_isTakeoffing) { float heightDiff = m_takeoffheight - m_height; if (heightDiff < TAKEOFF_HEIGHT_TOLERANCE) { m_isTakeoffing = false; newVerticalSpeed = 0.0f; m_cmdVerticalAcc = 0.0f; } else { newVerticalSpeed = qBound(0.0f, newVerticalSpeed, VERTICAL_SPEED_MAX * 0.5f); } } // 步骤9：高度积分 + 状态更新（原有逻辑保留） float newHeight = m_height + newVerticalSpeed * deltaTime; newHeight = qBound(0.0f, newHeight, m_maxheight); m_verticalSpeed = newVerticalSpeed; m_height = newHeight; m_verticalAcc = m_cmdVerticalAcc; // 实际加速度=目标加速度 // 步骤10：IMU加速度模拟（原有逻辑保留） m_imuVerticalAcc = m_verticalAcc + 9.81f; m_imuVerticalAcc += m_dataDist(m_randEngine) * 0.05f; m_az = m_imuVerticalAcc * 1000.0f; // 4. 偏航控制（CH4）- 严格匹配真实无人机逻辑 float m_currentYawRate; ch4 = qBound(PWM_MIN, m_channelPwm[CH4], PWM_MAX); int yawDelta = ch4 - PWM_CENTER; // 步骤1：死区处理 - 摇杆回中则角速度归0 if (abs(yawDelta) < PWM_DEADZONE) { m_targetYawRate = 0.0f; } else { // 步骤2：摇杆偏移量映射为偏航角速度（真实无人机逻辑） float yawRatio = (float)(yawDelta) / (PWM_MAX - PWM_CENTER); m_targetYawRate = yawRatio * YAW_MAX_RATE; // 角速度平滑（模拟飞控响应延迟，避免突变） m_targetYawRate = m_currentYawRate + (m_targetYawRate - m_currentYawRate) * YAW_DAMPING; // 步骤3：角速度限幅（物理极限） m_targetYawRate = qBound(-YAW_MAX_RATE, m_targetYawRate, YAW_MAX_RATE); } // 步骤4：更新当前角速度（摇杆回中后，角速度会立即归0） m_currentYawRate = m_targetYawRate; // 步骤5：更新偏航角（仅当角速度大于阈值时才变化，模拟飞控角度保持） if (fabs(m_currentYawRate) > YAW_HOLD_THRESHOLD) { m_yaw += m_currentYawRate * deltaTime; } // 步骤6：修复负数取模问题，确保航向角始终在 0~360° m_yaw = fmod(m_yaw + 360.0f, 360.0f); // 加360再取模，避免负数 } // 解析MAVLink指令 void DroneDataWorker::parseMavlinkCommand(const mavlink_message_t& msg) { switch (msg.msgid) { case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:{ parseServoOutputRaw(msg); break; } case MAVLINK_MSG_ID_COMMAND_LONG: { mavlink_command_long_t cmd; mavlink_msg_command_long_decode(&msg, &cmd); switch (cmd.command) { // 1. 解锁/上锁指令 case MAV_CMD_COMPONENT_ARM_DISARM: { const float UNLOCK_VALUE = 1.0f; bool unlock = ((fabsf(cmd.param1 - UNLOCK_VALUE) < FLOAT_EQUAL_TOLERANCE)) ; m_unlockState = unlock ? "解锁" : "锁定"; // 发送日志 emit logMessage(QString("收到MAVLink%1指令，无人机状态：%2") .arg(unlock ? "解锁" : "上锁") .arg(m_unlockState)); break; } // 2. 起飞指令 case MAV_CMD_NAV_TAKEOFF: { float takeoffHeight = cmd.param2; emit logMessage(QString("收到MAVLink起飞指令，目标高度：%1m").arg(takeoffHeight)); // 校验并更新起飞高度（加锁保证线程安全） QMutexLocker locker(&m_pidParamMutex); m_takeoffheight = qBound(TAKEOFF_HEIGHT_MIN, takeoffHeight, TAKEOFF_HEIGHT_MAX); locker.unlock(); break; } // 3. 降落指令 case MAV_CMD_NAV_LAND: { emit logMessage("收到MAVLink降落指令"); break; } // 4. 手动模式切换 case MAV_CMD_DO_SET_MODE: { // 判断是否为手动模式 bool isManualMode = ((static_cast<uint8_t>(roundf(cmd.param1)) & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) != 0); // 判断是否为自动模式 bool isAutoMode = ((static_cast<uint8_t>(roundf(cmd.param1)) & MAV_MODE_FLAG_AUTO_ENABLED) != 0); if (isManualMode) { m_flightMode = "手动模式"; emit logMessage("收到MAVLink手动模式切换指令，当前模式：手动模式"); } else if (isAutoMode) { m_flightMode = "自动模式"; emit logMessage("收到MAVLink自动模式切换指令，当前模式：自动模式"); } else { m_flightMode = "未知模式"; emit logMessage(QString("收到未知的模式切换指令，param1=%1, param2=%2").arg(cmd.param1).arg(cmd.param2)); } break; } // 5. 急停/继续指令 case MAV_CMD_DO_EMERGENCY_STOP: { const float EMERGENCY_STOP_VALUE = 1.0f; // 紧急停止的param1值 const float RESUME_VALUE = 0.0f; // 恢复运行的param1值 bool isEmergencyStop = (fabsf(cmd.param1 - EMERGENCY_STOP_VALUE) < FLOAT_EQUAL_TOLERANCE); bool isResume = (fabsf(cmd.param1 - RESUME_VALUE) < FLOAT_EQUAL_TOLERANCE); if (isEmergencyStop) { emit logMessage("收到MAVLink急停指令，无人机已切断动力"); } else if (isResume) { emit logMessage("收到MAVLink继续指令，无人机恢复正常运行"); } break; } // 6. 返航指令 case MAV_CMD_NAV_RETURN_TO_LAUNCH: { emit logMessage("收到MAVLink返航指令，无人机将返回起飞点"); break; } //处理无人机端的读取指令 case 2000: { emit logMessage("收到地面站航点读取请求，准备回传航点数据"); sendWaypointsToGcs(); // 触发航点发送 break; } // 处理无人机端的上传指令（自定义指令ID：2000） case 2001: { // 解析航点数据 int wpCount = static_cast<int>(cmd.param1); // 航点总数 int wpIndex = static_cast<int>(cmd.param2); // 当前航点索引 double lng = cmd.param3; // 经度 double lat = cmd.param4; // 纬度 double alt = cmd.param5; // 高度 // 清空原有航点（首次上传时） if (wpIndex == 0) { m_droneWaypoints.clear(); m_currentWaypointIndex = -1; m_isMissionActive = false; emit logMessage(QString("无人机端准备接收 %1 个航点").arg(wpCount)); } // 添加航点到无人机端列表 if (wpIndex < wpCount) { Waypoint wp; wp.lng = lng; wp.lat = lat; wp.alt = alt; wp.index = wpIndex + 1; // 索引从1开始（排除家点） wp.isHomePoint = false; m_droneWaypoints.append(wp); emit logMessage(QString("无人机端接收航点 %1: (%2, %3, %4m)") .arg(wp.index) .arg(lng, 0, 'f', 6) .arg(lat, 0, 'f', 6) .arg(alt)); } // 最后一个航点上传完成 if (wpIndex == wpCount - 1) { m_isMissionActive = true; m_currentWaypointIndex = 0; if (!m_droneWaypoints.isEmpty()) { m_currentTargetWaypoint = m_droneWaypoints.first(); } emit logMessage(QString("无人机端成功接收 %1 个航点，任务已激活").arg(wpCount)); // 回传确认 sendMissionAck(true); } break; } // ========== 2. 新增：处理地面站的航点到达通知（2003指令） ========== case 2003: { int reachedIndex = static_cast<int>(cmd.param1); emit logMessage(QString("【地面站同步】到达航点 %1").arg(reachedIndex)); break; } case 2004: { // CMD_UAV_POSITION_UPDATE double newLng = cmd.param1; double newLat = cmd.param2; double newAlt = cmd.param3; // 加锁更新无人机位置（线程安全） QMutexLocker locker(&m_pidParamMutex); m_uavLng = newLng; m_uavLat = newLat; m_uavAlt = newAlt; locker.unlock(); break; } default: break; } } case MAVLINK_MSG_ID_COMMAND_ACK: { mavlink_command_ack_t ack; mavlink_msg_command_ack_decode(&msg, &ack); switch (ack.command) { case 2001: { // 航点分片/完成确认 if (ack.result == MAV_RESULT_ACCEPTED) { int wpIndex = ack.progress; // 可扩展progress字段传递航点索引 emit logMessage(QString("地面站确认接收航点 %1 成功").arg(wpIndex)); } else { emit logMessage("地面站接收航点失败，需重发"); // 重发对应航点逻辑 } break; } case 2000: // 航点总数确认 emit logMessage("地面站已确认收到航点总数"); break; case 2003: // 航点到达通知确认 emit logMessage("地面站已同步航点到达状态"); break; } break; } default: { emit logMessage(QString("收到未处理的MAVLink指令，msgid：%1").arg(msg.msgid)); break; } } } // 发送无人机端航点到地面站 void DroneDataWorker::sendWaypointsToGcs() { QMutexLocker locker(&m_pidParamMutex); // 1. 先发送航点总数（指令2000，param1=总数） mavlink_message_t msg; mavlink_msg_command_long_pack( 1, MAV_COMP_ID_AUTOPILOT, &msg, 255, MAV_COMP_ID_MISSIONPLANNER, 2000, // 响应指令ID（与读取请求一致） 0, // 确认标志 m_droneWaypoints.size(), // param1: 航点总数 m_isMissionActive ? 1 : 0, // param2: 任务状态, 0, 0, 0, 0, 0 ); sendMavlinkMessage(msg); QThread::msleep(20); // 间隔避免总数丢失 // 2. 逐个发送航点数据（指令2001，参数修正） for (int i = 0; i < m_droneWaypoints.size(); i++) { Waypoint wp = m_droneWaypoints[i]; mavlink_msg_command_long_pack( 1, MAV_COMP_ID_AUTOPILOT, &msg, 255, MAV_COMP_ID_MISSIONPLANNER, 2001, // 航点数据指令ID 0, m_droneWaypoints.size(), // param1: 航点总数（地面站用于校验） i, // param2: 当前航点索引（从0开始） wp.lng, // param3: 经度 wp.lat, // param4: 纬度 wp.alt, // param5: 高度 wp.isHomePoint ? 1 : 0, // param6: 是否家点 0 ); sendMavlinkMessage(msg); QThread::msleep(10); // 避免数据拥塞 } emit logMessage(QString("已向地面站发送 %1 个航点").arg(m_droneWaypoints.size())); } // 发送任务确认消息 void DroneDataWorker::sendMissionAck(bool success) { mavlink_message_t msg; mavlink_msg_command_ack_pack( 1, MAV_COMP_ID_AUTOPILOT, &msg, 2001, // 指令ID success ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED, 0, 0, 1, 1 ); sendMavlinkMessage(msg); } void DroneDataWorker::sendMissionStatus(bool started) { mavlink_message_t msg; mavlink_msg_command_long_pack( 1, MAV_COMP_ID_AUTOPILOT, &msg, 255, MAV_COMP_ID_MISSIONPLANNER, 2002, 0, started ? 1 : 0, 0, 0, 0, 0, 0, 0 ); sendMavlinkMessage(msg); } // 封装MAVLink消息发送 void DroneDataWorker::sendMavlinkMessage(const mavlink_message_t& msg) { uint8_t buf[MAVLINK_MAX_PACKET_LEN] = {0}; uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); m_udpsender->sendData(QByteArray((const char*)buf, len)); } // 更新单个PID参数（线程安全） void DroneDataWorker::updatePidParam(const QString& paramName, float value) { QMutexLocker locker(&m_pidParamMutex); // 加锁保证线程安全 // 根据参数名限制值的范围 float safeValue = 0.0f; if (paramName == "PITCH_P") safeValue = qBound(PITCH_ROLL_KP_MIN, value, PITCH_ROLL_KP_MAX); else if (paramName == "PITCH_I") safeValue = qBound(PITCH_ROLL_KI_MIN, value, PITCH_ROLL_KI_MAX); else if (paramName == "PITCH_D") safeValue = qBound(PITCH_ROLL_KD_MIN, value, PITCH_ROLL_KD_MAX); else if (paramName == "PITCHSPEED_P") safeValue = qBound(PITCH_ROLL_SPEED_KP_MIN, value, PITCH_ROLL_SPEED_KP_MAX); else if (paramName == "PITCHSPEED_I") safeValue = qBound(PITCH_ROLL_SPEED_KI_MIN, value, PITCH_ROLL_SPEED_KI_MAX); else if (paramName == "PITCHSPEED_D") safeValue = qBound(PITCH_ROLL_SPEED_KD_MIN, value, PITCH_ROLL_SPEED_KD_MAX); else if (paramName == "ROLL_P") safeValue = qBound(PITCH_ROLL_KP_MIN, value, PITCH_ROLL_KP_MAX); else if (paramName == "ROLL_I") safeValue = qBound(PITCH_ROLL_KI_MIN, value, PITCH_ROLL_KI_MAX); else if (paramName == "ROLL_D") safeValue = qBound(PITCH_ROLL_KD_MIN, value, PITCH_ROLL_KD_MAX); else if (paramName == "ROLLSPEED_P") safeValue = qBound(PITCH_ROLL_SPEED_KP_MIN, value, PITCH_ROLL_SPEED_KP_MAX); else if (paramName == "ROLLSPEED_I") safeValue = qBound(PITCH_ROLL_SPEED_KI_MIN, value, PITCH_ROLL_SPEED_KI_MAX); else if (paramName == "ROLLSPEED_D") safeValue = qBound(PITCH_ROLL_SPEED_KD_MIN, value, PITCH_ROLL_SPEED_KD_MAX); else if (paramName == "YAW_P") safeValue = qBound(YAW_KP_MIN, value, YAW_KP_MAX); else if (paramName == "YAW_I") safeValue = qBound(YAW_KI_MIN, value, YAW_KI_MAX); else if (paramName == "YAW_D") safeValue = qBound(YAW_KD_MIN, value, YAW_KD_MAX); else if (paramName == "YAWSPEED_P") safeValue = qBound(YAW_SPEED_KP_MIN, value, YAW_SPEED_KP_MAX); else if (paramName == "YAWSPEED_I") safeValue = qBound(YAW_SPEED_KI_MIN, value, YAW_SPEED_KI_MAX); else if (paramName == "YAWSPEED_D") safeValue = qBound(YAW_SPEED_KD_MIN, value, YAW_SPEED_KD_MAX); else if (paramName == "HEIGHT_P") safeValue = qBound(HEIGHT_KP_MIN, value, HEIGHT_KP_MAX); else if (paramName == "HEIGHT_I") safeValue = qBound(HEIGHT_KI_MIN, value, HEIGHT_KI_MAX); else if (paramName == "HEIGHT_D") safeValue = qBound(HEIGHT_KD_MIN, value, HEIGHT_KD_MAX); else if (paramName == "VSPEED_P") safeValue = qBound(VSPEED_KP_MIN, value, VSPEED_KP_MAX); else if (paramName == "VSPEED_I") safeValue = qBound(VSPEED_KI_MIN, value, VSPEED_KI_MAX); else if (paramName == "VSPEED_D") safeValue = qBound(VSPEED_KD_MIN, value, VSPEED_KD_MAX); else if (paramName == "VACC_P") safeValue = qBound(VACC_KP_MIN, value, VACC_KP_MAX); else if (paramName == "VACC_I") safeValue = qBound(VACC_KI_MIN, value, VACC_KI_MAX); else if (paramName == "VACC_D") safeValue = qBound(VACC_KD_MIN, value, VACC_KD_MAX); else if (paramName == "TAKEOFFHEIGHT") safeValue = qBound(TAKEOFF_HEIGHT_MIN, value, TAKEOFF_HEIGHT_MAX); else if (paramName == "RETURNHEIGHT") safeValue = qBound(RETURN_HEIGHT_MIN, value, RETURN_HEIGHT_MAX); else if (paramName == "MINBATTERY") safeValue = qBound(BATTERY_MIN_MIN, value, BATTERY_MIN_MAX); else if (paramName == "MAXHEIGHT") safeValue = qBound(MAX_HEIGHT_MIN, value, MAX_HEIGHT_MAX); else if (paramName == "MAXRADIUS") safeValue = qBound(MAX_RADIUS_MIN, value, MAX_RADIUS_MAX); else safeValue = value; // 未知参数，保持原值 // 根据参数名更新对应的值 if (paramName == "PITCH_P") m_pitchP = safeValue; else if (paramName == "PITCH_I") m_pitchI = safeValue; else if (paramName == "PITCH_D") m_pitchD = safeValue; else if (paramName == "PITCHSPEED_P") m_pitchspeedP = safeValue; else if (paramName == "PITCHSPEED_I") m_pitchspeedI = safeValue; else if (paramName == "PITCHSPEED_D") m_pitchspeedD = safeValue; else if (paramName == "ROLL_P") m_rollP = safeValue; else if (paramName == "ROLL_I") m_rollI = safeValue; else if (paramName == "ROLL_D") m_rollD = safeValue; else if (paramName == "ROLLSPEED_P") m_rollspeedP = safeValue; else if (paramName == "ROLLSPEED_I") m_rollspeedI = safeValue; else if (paramName == "ROLLSPEED_D") m_rollspeedD = safeValue; else if (paramName == "YAW_P") m_yawP = safeValue; else if (paramName == "YAW_I") m_yawI = safeValue; else if (paramName == "YAW_D") m_yawD = safeValue; else if (paramName == "YAWSPEED_P") m_yawspeedP = safeValue; else if (paramName == "YAWSPEED_I") m_yawspeedI = safeValue; else if (paramName == "YAWSPEED_D") m_yawspeedD = safeValue; else if (paramName == "HEIGHT_P") m_heightP = safeValue; else if (paramName == "HEIGHT_I") m_heightI = safeValue; else if (paramName == "HEIGHT_D") m_heightD = safeValue; else if (paramName == "VSPEED_P") m_vspeedP = safeValue; else if (paramName == "VSPEED_I") m_vspeedI = safeValue; else if (paramName == "VSPEED_D") m_vspeedD = safeValue; else if (paramName == "VACC_P") m_vaccP = safeValue; else if (paramName == "VACC_I") m_vaccI = safeValue; else if (paramName == "VACC_D") m_vaccD = safeValue; else if (paramName == "TAKEOFFHEIGHT") m_takeoffheight = safeValue; else if (paramName == "RETURNHEIGHT") m_returnheight = safeValue; else if (paramName == "MINBATTERY") m_minbattery = safeValue; else if (paramName == "MAXHEIGHT") m_maxheight = safeValue; else if (paramName == "MAXRADIUS") m_maxradius = safeValue; } 这是具体代码，结合代码分析问题