hm/gcs-nf
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修改转发的东天南位置

Browse Source
This commit is contained in:
zhangxusong
2025-09-18 09:05:01 +08:00
parent c9ed9fcb27
commit c85fc5067b
2 changed files with 90 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MavLinkNode/mavlinknode.cpp
+79 -39
View File
@@ -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);
sh.exe.stackdump
+11
View File
@@ -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