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);