From c85fc5067b092b68f8fa1c4ab47990c830636f49 Mon Sep 17 00:00:00 2001 From: zhangxusong <328180668@qq.com> Date: Thu, 18 Sep 2025 09:05:01 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E8=BD=AC=E5=8F=91=E7=9A=84?= =?UTF-8?q?=E4=B8=9C=E5=A4=A9=E5=8D=97=E4=BD=8D=E7=BD=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- MavLinkNode/mavlinknode.cpp | 118 ++++++++++++++++++++++++------------ sh.exe.stackdump | 11 ++++ 2 files changed, 90 insertions(+), 39 deletions(-) create mode 100644 sh.exe.stackdump diff --git a/MavLinkNode/mavlinknode.cpp b/MavLinkNode/mavlinknode.cpp index 7d6ac66..bbdb25f 100644 --- a/MavLinkNode/mavlinknode.cpp +++ b/MavLinkNode/mavlinknode.cpp @@ -7,48 +7,88 @@ * 包含了各个函数、参数、状态机等及其说明 **/ -// 地球半径 (单位: 米) -const double R = 6371000.0; +// WGS-84 椭球参数 +constexpr double a = 6378137.0; // 长半轴 (m) +constexpr double f = 1.0 / 298.257223563; // 扁率 +constexpr double b = a * (1 - f); // 短半轴 +constexpr double e2 = (a * a - b * b) / (a * a); // 第一偏心率平方 -// 将角度转换为弧度 -double degToRad(double degree) -{ - return degree * M_PI / 180.0; +struct Vec3 { + double x, y, z; +}; + +// 经纬度高程 -> ECEF (X,Y,Z) +Vec3 llaToEcef(double latDeg, double lonDeg, double h) { + double lat = latDeg * M_PI / 180.0; + double lon = lonDeg * M_PI / 180.0; + + double sinLat = std::sin(lat); + double cosLat = std::cos(lat); + double sinLon = std::sin(lon); + double cosLon = std::cos(lon); + + double N = a / std::sqrt(1.0 - e2 * sinLat * sinLat); + + double x = (N + h) * cosLat * cosLon; + double y = (N + h) * cosLat * sinLon; + double z = (N * (1 - e2) + h) * sinLat; + + return {x, y, z}; } -double calculateEastPosition(double lat0, double lon0, double lat, double lon) -{ - // 将经纬度从度转换为弧度 - lat0 = degToRad(lat0); - lon0 = degToRad(lon0); - lat = degToRad(lat); - lon = degToRad(lon); - - // 计算经度差 - double deltaLon = lon - lon0; - - // 计算东向位置（单位：米） - double eastPosition = R * deltaLon * cos(lat0); - - return eastPosition; +// 计算 ΔECEF = target - ref +Vec3 deltaEcef(double refLat, double refLon, double refH, + double tgtLat, double tgtLon, double tgtH) { + Vec3 ref = llaToEcef(refLat, refLon, refH); + Vec3 tgt = llaToEcef(tgtLat, tgtLon, tgtH); + return {tgt.x - ref.x, tgt.y - ref.y, tgt.z - ref.z}; } -// 计算南向位置 -double calculateSouthPosition(double lat0, double lat) -{ - // 将经纬度从度转换为弧度 - lat0 = degToRad(lat0); - lat = degToRad(lat); +// 计算 East 分量 +double calcEast(double refLat, double refLon, double refH, + double tgtLat, double tgtLon, double tgtH) { + Vec3 d = deltaEcef(refLat, refLon, refH, tgtLat, tgtLon, tgtH); - // 计算纬度差 - double deltaLat = lat - lat0; + double lon = refLon * M_PI / 180.0; + double sinLon = std::sin(lon); + double cosLon = std::cos(lon); - // 计算南向位置（单位：米） - double southPosition = R * deltaLat; - - return southPosition; + return -sinLon * d.x + cosLon * d.y; } +// 计算 North 分量 +double calcNorth(double refLat, double refLon, double refH, + double tgtLat, double tgtLon, double tgtH) { + Vec3 d = deltaEcef(refLat, refLon, refH, tgtLat, tgtLon, tgtH); + + double lat = refLat * M_PI / 180.0; + double lon = refLon * M_PI / 180.0; + + double sinLat = std::sin(lat); + double cosLat = std::cos(lat); + double sinLon = std::sin(lon); + double cosLon = std::cos(lon); + + return -sinLat * cosLon * d.x - sinLat * sinLon * d.y + cosLat * d.z; +} + +// 计算 Up 分量 +double calcUp(double refLat, double refLon, double refH, + double tgtLat, double tgtLon, double tgtH) { + Vec3 d = deltaEcef(refLat, refLon, refH, tgtLat, tgtLon, tgtH); + + double lat = refLat * M_PI / 180.0; + double lon = refLon * M_PI / 180.0; + + double sinLat = std::sin(lat); + double cosLat = std::cos(lat); + double sinLon = std::sin(lon); + double cosLon = std::cos(lon); + + return cosLat * cosLon * d.x + cosLat * sinLon * d.y + sinLat * d.z; +} + + // 计算合速度 double calculateTotalVelocity(double northVelocity, double eastVelocity, double downwardVelocity) { @@ -1410,9 +1450,9 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) info.id = infoExportID;//1 //info.time = (int32_t)vehicle.ins1.time;//(second+min*60+hour*3600)*1000 info.t = 0; - info.pe = (int32_t)(calculateEastPosition(refPoint.lat, refPoint.lon, vehicle.ins1.lat, vehicle.ins1.lon) * 8); - info.pu = (int32_t)(refPoint.hight - vehicle.ins1.alt) * 8; - info.ps = (int32_t)(calculateSouthPosition(refPoint.lat, vehicle.ins1.lat) * 8); + info.pe = (int32_t)(calcEast(refPoint.lat,refPoint.lon, refPoint.hight, vehicle.ins1.lat, vehicle.ins1.lon, vehicle.ins1.alt)* 8); + info.pu = (int32_t)(calcUp(refPoint.lat,refPoint.lon, refPoint.hight, vehicle.ins1.lat, vehicle.ins1.lon, vehicle.ins1.alt) * 8); + info.ps = (int32_t)( (-calcNorth(refPoint.lat,refPoint.lon, refPoint.hight, vehicle.ins1.lat, vehicle.ins1.lon, vehicle.ins1.alt) ) * 8); info.ve = (int32_t)(vehicle.ins1.v_east * 1024); info.vu = (int32_t)(vehicle.ins1.v_up * 1024); info.vs = (int32_t)(-vehicle.ins1.v_north * 1024); @@ -1534,9 +1574,9 @@ void MavLinkNode::StatusParse(mavlink_message_t msg) info.id = infoExportID;//1 //info.time = (int32_t)vehicle.ins1.time;//(second+min*60+hour*3600)*1000 info.t = 0; - info.pe = (int32_t)(calculateEastPosition(refPoint.lat, refPoint.lon, vehicle.ins2.lat, vehicle.ins2.lon) * 8); - info.pu = (int32_t)(refPoint.hight - vehicle.ins2.alt) * 8; - info.ps = (int32_t)(calculateSouthPosition(refPoint.lat, vehicle.ins2.lat) * 8); + info.pe = (int32_t)(calcEast(refPoint.lat,refPoint.lon, refPoint.hight, vehicle.ins1.lat, vehicle.ins1.lon, vehicle.ins1.alt)* 8); + info.pu = (int32_t)(calcUp(refPoint.lat,refPoint.lon, refPoint.hight, vehicle.ins1.lat, vehicle.ins1.lon, vehicle.ins1.alt) * 8); + info.ps = (int32_t)( (-calcNorth(refPoint.lat,refPoint.lon, refPoint.hight, vehicle.ins1.lat, vehicle.ins1.lon, vehicle.ins1.alt) ) * 8); info.ve = (int32_t)(vehicle.ins2.v_east * 1024); info.vu = (int32_t)(vehicle.ins2.v_up * 1024); info.vs = (int32_t)(-vehicle.ins2.v_north * 1024); diff --git a/sh.exe.stackdump b/sh.exe.stackdump new file mode 100644 index 0000000..9c7115c --- /dev/null +++ b/sh.exe.stackdump @@ -0,0 +1,11 @@ +Stack trace: +Frame Function Args +000FFFFCD30 00210062B0E (00210284F90, 00210275E51, 000FFFFCD30, 000FFFFB9D0) +000FFFFCD30 0021004846A (00100002000, 00800000010, 00800000010, 00000000000) +000FFFFCD30 002100484A2 (00000000000, 002103591F8, 000FFFFCD30, 0021027619F) +000FFFFCD30 0021005BAF9 (2E5C345C305C2E5C, 89BCEF5C385C325C, 635C6F5C645C2E5C, BC0000000000785C) +000FFFFCD30 0021005BB86 (00000000000, 002101FEC10, FFFFFFFFFFFFFF78, 00000000000) +000FFFFCD30 00210048C0C (00000000000, 00000000000, 00000000000, 00000000000) +000FFFFFFF0 00210047716 (00000000000, 00000000000, 00000000000, 00000000000) +000FFFFFFF0 002100477C4 (00000000000, 00000000000, 00000000000, 00000000000) +End of stack trace