修改转发的东天南位置
This commit is contained in:
+79
-39
@@ -7,48 +7,88 @@
|
|||||||
* 包含了各个函数、参数、状态机等及其说明
|
* 包含了各个函数、参数、状态机等及其说明
|
||||||
**/
|
**/
|
||||||
|
|
||||||
// 地球半径 (单位: 米)
|
// WGS-84 椭球参数
|
||||||
const double R = 6371000.0;
|
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); // 第一偏心率平方
|
||||||
|
|
||||||
// 将角度转换为弧度
|
struct Vec3 {
|
||||||
double degToRad(double degree)
|
double x, y, z;
|
||||||
{
|
};
|
||||||
return degree * M_PI / 180.0;
|
|
||||||
|
// 经纬度高程 -> 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)
|
// 计算 ΔECEF = target - ref
|
||||||
{
|
Vec3 deltaEcef(double refLat, double refLon, double refH,
|
||||||
// 将经纬度从度转换为弧度
|
double tgtLat, double tgtLon, double tgtH) {
|
||||||
lat0 = degToRad(lat0);
|
Vec3 ref = llaToEcef(refLat, refLon, refH);
|
||||||
lon0 = degToRad(lon0);
|
Vec3 tgt = llaToEcef(tgtLat, tgtLon, tgtH);
|
||||||
lat = degToRad(lat);
|
return {tgt.x - ref.x, tgt.y - ref.y, tgt.z - ref.z};
|
||||||
lon = degToRad(lon);
|
|
||||||
|
|
||||||
// 计算经度差
|
|
||||||
double deltaLon = lon - lon0;
|
|
||||||
|
|
||||||
// 计算东向位置（单位：米）
|
|
||||||
double eastPosition = R * deltaLon * cos(lat0);
|
|
||||||
|
|
||||||
return eastPosition;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// 计算南向位置
|
// 计算 East 分量
|
||||||
double calculateSouthPosition(double lat0, double lat)
|
double calcEast(double refLat, double refLon, double refH,
|
||||||
{
|
double tgtLat, double tgtLon, double tgtH) {
|
||||||
// 将经纬度从度转换为弧度
|
Vec3 d = deltaEcef(refLat, refLon, refH, tgtLat, tgtLon, tgtH);
|
||||||
lat0 = degToRad(lat0);
|
|
||||||
lat = degToRad(lat);
|
|
||||||
|
|
||||||
// 计算纬度差
|
double lon = refLon * M_PI / 180.0;
|
||||||
double deltaLat = lat - lat0;
|
double sinLon = std::sin(lon);
|
||||||
|
double cosLon = std::cos(lon);
|
||||||
|
|
||||||
// 计算南向位置（单位：米）
|
return -sinLon * d.x + cosLon * d.y;
|
||||||
double southPosition = R * deltaLat;
|
|
||||||
|
|
||||||
return southPosition;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// 计算 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)
|
double calculateTotalVelocity(double northVelocity, double eastVelocity, double downwardVelocity)
|
||||||
{
|
{
|
||||||
@@ -1410,9 +1450,9 @@ void MavLinkNode::StatusParse(mavlink_message_t msg)
|
|||||||
info.id = infoExportID;//1
|
info.id = infoExportID;//1
|
||||||
//info.time = (int32_t)vehicle.ins1.time;//(second+min*60+hour*3600)*1000
|
//info.time = (int32_t)vehicle.ins1.time;//(second+min*60+hour*3600)*1000
|
||||||
info.t = 0;
|
info.t = 0;
|
||||||
info.pe = (int32_t)(calculateEastPosition(refPoint.lat, refPoint.lon, vehicle.ins1.lat, vehicle.ins1.lon) * 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)(refPoint.hight - 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)(calculateSouthPosition(refPoint.lat, vehicle.ins1.lat) * 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.ve = (int32_t)(vehicle.ins1.v_east * 1024);
|
||||||
info.vu = (int32_t)(vehicle.ins1.v_up * 1024);
|
info.vu = (int32_t)(vehicle.ins1.v_up * 1024);
|
||||||
info.vs = (int32_t)(-vehicle.ins1.v_north * 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.id = infoExportID;//1
|
||||||
//info.time = (int32_t)vehicle.ins1.time;//(second+min*60+hour*3600)*1000
|
//info.time = (int32_t)vehicle.ins1.time;//(second+min*60+hour*3600)*1000
|
||||||
info.t = 0;
|
info.t = 0;
|
||||||
info.pe = (int32_t)(calculateEastPosition(refPoint.lat, refPoint.lon, vehicle.ins2.lat, vehicle.ins2.lon) * 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)(refPoint.hight - vehicle.ins2.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)(calculateSouthPosition(refPoint.lat, vehicle.ins2.lat) * 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.ve = (int32_t)(vehicle.ins2.v_east * 1024);
|
||||||
info.vu = (int32_t)(vehicle.ins2.v_up * 1024);
|
info.vu = (int32_t)(vehicle.ins2.v_up * 1024);
|
||||||
info.vs = (int32_t)(-vehicle.ins2.v_north * 1024);
|
info.vs = (int32_t)(-vehicle.ins2.v_north * 1024);
|
||||||
|
|||||||
@@ -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
|
||||||
Reference in New Issue
Block a user