From 6d1ad2780c7c57f8fcb2782f92311990aeddfee6 Mon Sep 17 00:00:00 2001 From: wyc <1104749580@qq.com> Date: Thu, 16 Jul 2026 17:51:11 +0800 Subject: [PATCH] =?UTF-8?q?=E8=BD=A8=E8=BF=B9=E7=82=B9=E4=BF=A1=E6=81=AF?= =?UTF-8?q?=E4=BC=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- SSR/ADS_B/ADS_B.hpp | 10 ++-- SSR/CPR/CPR.h | 86 ++++++++++++++++------------- SSR/Msg.h | 49 ++++++----------- SSR/export.hpp | 131 ++++++++++++++++++++++++++++---------------- 4 files changed, 154 insertions(+), 122 deletions(-) diff --git a/SSR/ADS_B/ADS_B.hpp b/SSR/ADS_B/ADS_B.hpp index 5c85aa6..8c79eb4 100644 --- a/SSR/ADS_B/ADS_B.hpp +++ b/SSR/ADS_B/ADS_B.hpp @@ -72,7 +72,7 @@ struct CPR_Data { std::function(std::string_view, std::string_view, time_t, time_t)> global_parse = nullptr; std::function(std::string_view, double lat, double lon, time_t, time_t)> locale_parse = nullptr; std::function encode = nullptr; - std::optional pos; + std::optional last_parse_pos; CPR::Frame cpr_format = CPR::Frame::Even; }; struct Constraint { @@ -91,7 +91,7 @@ struct CPR_MSG : CPR_Data { std::string current_state(P_S msg); Psc::JSON current_state_json() { Psc::JSON ret = Psc::JSON::object(); - ret.append({"pos", (pos.has_value() ? pos->to_string() : "null")}); + ret.append({"pos", (last_parse_pos.has_value() ? last_parse_pos->to_string() : "null")}); ret.append({"odd_msg", odd_msg.to_json()}); ret.append({"even_msg", even_msg.to_json()}); return ret; @@ -104,15 +104,15 @@ struct CPR_MSG : CPR_Data { } virtual Psc::JSON toJson() { Psc::JSON ret = Psc::JSON::object(); - Ret_J(pos) + Ret_J(last_parse_pos) Ret_J(cpr_format) return ret; } virtual void set(std::string& msg_bin) { set_bin<53, 1>(msg_bin, static_cast(this->cpr_format)); - if (pos.has_value()) { + if (last_parse_pos.has_value()) { set_bin<54, 34>(msg_bin, - encode(this->pos->lat, this->pos->lon, this->cpr_format)); + encode(this->last_parse_pos->lat, this->last_parse_pos->lon, this->cpr_format)); } } private: diff --git a/SSR/CPR/CPR.h b/SSR/CPR/CPR.h index 055bb50..1bded7a 100644 --- a/SSR/CPR/CPR.h +++ b/SSR/CPR/CPR.h @@ -14,20 +14,20 @@ using Time = long double; using D = long double; using I = std::int64_t; struct Position { - Position(D lat, D lon) : lat(lat), lon(lon) {} - D lat; - D lon; - [[nodiscard]] std::string to_string() const; + Position(D lat, D lon) : lat(lat), lon(lon) {} + D lat; + D lon; + [[nodiscard]] std::string to_string() const; }; enum Frame { - Even = 0, // 偶数帧 - ODD = 1 // 奇数帧 + Even = 0, // 偶数帧 + ODD = 1 // 奇数帧 }; using POS = std::optional; const I NZ = 15; template T floor(D x) { - return static_cast(std::floor(x)); + return static_cast(std::floor(x)); } D MOD(D x, D y); I NL(D lat); @@ -45,47 +45,47 @@ POS locale_decode(D lat_s, D lon_s, I YZi, I XZi, I i, I _2, D r = 360.0); static I Nb1 = 19; static I Nb2 = 17; inline std::tuple surface_encode(D lat, D lon, I i) { - auto [YZ, XZ] = encode(lat, lon, Nb1, i); - I base = 2 << (Nb2 - 1); - YZ %= base; - XZ %= base; - return {YZ, XZ}; + auto [YZ, XZ] = encode(lat, lon, Nb1, i); + I base = 2 << (Nb2 - 1); + YZ %= base; + XZ %= base; + return {YZ, XZ}; } inline I zone_index(D lon) { - return (lon + 180.0) / 90.0; + return (lon + 180.0) / 90.0; } inline D bb(D lon, D lon_s) { - auto j = zone_index(lon); - auto i = zone_index(lon_s); - D change = (i - j) * 90.0; - D lon2 = lon + change; - return lon2; + auto j = zone_index(lon); + auto i = zone_index(lon_s); + D change = (i - j) * 90.0; + D lon2 = lon + change; + return lon2; } inline std::optional> surface_global_decode(I YZ0, I YZ1, I XZ0, I XZ1) { - auto poses = global_decode(YZ0, YZ1, XZ0, XZ1, Nb2, 90.0); - // auto pos = global_decode(YZ0, YZ1, XZ0, XZ1, t0, t1,Nb, 360.0); - if (!poses.has_value()) { - return std::nullopt; - } - auto [pos0, pos1] = poses.value(); - return std::tuple(pos0, pos1); + auto poses = global_decode(YZ0, YZ1, XZ0, XZ1, Nb2, 90.0); + // auto pos = global_decode(YZ0, YZ1, XZ0, XZ1, t0, t1,Nb, 360.0); + if (!poses.has_value()) { + return std::nullopt; + } + auto [pos0, pos1] = poses.value(); + return std::tuple(pos0, pos1); } inline std::optional> surface_global_decode_ref(I YZ0, I YZ1, I XZ0, I XZ1, D lat_s, D lon_s) { - auto poses = surface_global_decode(YZ0, YZ1, XZ0, XZ1); - if (!poses.has_value()) return std::nullopt; - auto [pos0, pos1] = poses.value(); - auto p0 = Position{bb(pos0.lat, lat_s), bb(pos0.lon, lon_s)}; - auto p1 = Position{bb(pos1.lat, lat_s), bb(pos1.lon, lon_s)}; - return std::tuple(p0, p1); + auto poses = surface_global_decode(YZ0, YZ1, XZ0, XZ1); + if (!poses.has_value()) return std::nullopt; + auto [pos0, pos1] = poses.value(); + auto p0 = Position{bb(pos0.lat, lat_s), bb(pos0.lon, lon_s)}; + auto p1 = Position{bb(pos1.lat, lat_s), bb(pos1.lon, lon_s)}; + return std::tuple(p0, p1); } inline POS surface_locale_decode(D lat_s, D lon_s, I YZi, I XZi, I i) { - auto pos = locale_decode(lat_s, lon_s, YZi, XZi, i, Nb2, 90.0); - // auto pos = locale_decode(lat_s, lon_s, YZi, XZi, i, Nb, 360.0); - if (!pos.has_value()) { - return std::nullopt; - } - auto [lat, lon] = pos.value(); - return Position{bb(lat, lat_s), bb(lon, lon_s)}; + auto pos = locale_decode(lat_s, lon_s, YZi, XZi, i, Nb2, 90.0); + // auto pos = locale_decode(lat_s, lon_s, YZi, XZi, i, Nb, 360.0); + if (!pos.has_value()) { + return std::nullopt; + } + auto [lat, lon] = pos.value(); + return Position{bb(lat, lat_s), bb(lon, lon_s)}; } // time std::string lat_lon_airborne_34(double latitude, double longitude, @@ -128,3 +128,13 @@ void XYZ_to_WGS84_LBH(double X, double Y, double Z, double& longitude, double& latitude, double& altitude); } // namespace CPR } // namespace SSR +#include +template <> +struct std::formatter { + constexpr auto parse(std::format_parse_context& ctx) { + return ctx.begin(); + } + auto format(const SSR::CPR::Position& pos, std::format_context& ctx) const { + return std::format_to(ctx.out(), "({}, {})", pos.lat, pos.lon); + } +}; diff --git a/SSR/Msg.h b/SSR/Msg.h index 79219ab..38457c0 100644 --- a/SSR/Msg.h +++ b/SSR/Msg.h @@ -5,6 +5,7 @@ #include "convert/DataOutputFormats.h" #include "enum.h" #include +#include #include #define STRINGIFY(x) #x // 硬件接收来的 二进制消息族 @@ -204,17 +205,12 @@ struct Position_3D : CPR::Position { Position_3D(CPR::D lat, CPR::D lon, CPR::D alt) : CPR::Position(lat, lon), alt(alt) {} }; // 这个返回值的意义在于 解码失败后 返回值能反应出是 位置解码失败 -// 还是奇偶报文解码位置失败 +// 还是奇偶报文解码位置失败 轨迹点上会显示的信息 class Pos_Ret { public: CPR::Position pos; - std::shared_ptr that; - using Extra = std::variant, CPR::Position>; - Extra extra; - Pos_Ret(CPR::Position p, std::shared_ptr t, - std::shared_ptr o) : pos(p), that(std::move(t)), extra(std::move(o)) {} - Pos_Ret(CPR::Position p, std::shared_ptr t, CPR::Position old) : pos(p), that(std::move(t)), - extra(old) {} + Psc::JSON info; + Pos_Ret(CPR::Position p, Psc::JSON info) : pos(p), info(std::move(info)) {} }; // 展示出最详细的信息 struct Position_Info : Position_3D { @@ -222,10 +218,8 @@ struct Position_Info : Position_3D { std::uint64_t utc{}; std::uint64_t odd_even_diff_ns{}; Position_Info() : Position_3D(0.0, 0.0, 0.0) {} - Pos_Ret::Extra extra; - std::shared_ptr that; - Position_Info(const Pos_Ret& r, CPR::D alt) : Position_3D(r.pos.lat, r.pos.lon, alt), that(r.that), - extra(r.extra) { + Psc::JSON info; + Position_Info(const Pos_Ret& r, CPR::D alt) : Position_3D(r.pos.lat, r.pos.lon, alt), info(r.info) { using namespace std::chrono; utc = static_cast( duration_cast(system_clock::now().time_since_epoch()).count()); @@ -236,30 +230,14 @@ struct Position_Info : Position_3D { Ret_J(lon) Ret_J(alt) Ret_J(utc) - Ret_J(that->mlat_timestamp) - Ret_J(that->msg_hex) - Ret_J(that->signal_level) - if (auto p = std::get_if< - std::shared_ptr>( - &extra)) { - std::shared_ptr other = *p; - Ret_J(other->mlat_timestamp) - Ret_J(other->msg_hex) - Ret_J(other->signal_level) - return ret; - } - if (auto p = std::get_if(&extra)) { - CPR::Position old = *p; - Ret_J(old) - return ret; - } + ret.append_list(info.children); return ret; } }; struct POS_List { NoneCopyLock mtx; - size_t max_size; - POS_List() {} + size_t max_size{}; + POS_List() = default; void reset() { std::lock_guard g(mtx.mtx); seq = 0; @@ -300,3 +278,12 @@ protected: std::vector buf; }; } // namespace SSR +template <> +struct std::formatter { + constexpr auto parse(std::format_parse_context& ctx) { + return ctx.begin(); + } + auto format(const SSR::Play_Back_Time_Point& value, std::format_context& ctx) const { + return std::format_to(ctx.out(), "{}", value.abs_sec()); + } +}; diff --git a/SSR/export.hpp b/SSR/export.hpp index 8843bc1..d17e30d 100644 --- a/SSR/export.hpp +++ b/SSR/export.hpp @@ -23,31 +23,6 @@ inline CPR_CB cpr_cb = [](CPR_Type t, CPR_Ret_Type err_type, std::string_view lo if (mode_s_logger) mode_s_logger->debug("CPR/" + Psc::to_string(t) + "/" + Psc::to_string(err_type), {}, log); }; namespace ADS_B_T { -// cpr回调 -template -RET CPR_MSG::local_parse_message(CPR::Position old_pos, P_S msg, double time_s, Constraint constraint) { - constraint.cb(SSR::CPR_Type::Local_pos_parse, SSR::CPR_Ret_Type::Base, current_state(msg), msg); - auto diff_time = std::abs(time_s - last_parse_pos_time); - if (constraint.time_space_filter && diff_time > constraint.max_time_s) { - constraint.cb(SSR::CPR_Type::Local_pos_parse, SSR::CPR_Ret_Type::Time_Space_Too_Long, current_state(msg), msg); - return std::nullopt; - } - auto new_pos = locale_parse(msg->msg_bin, old_pos.lat, old_pos.lon, time_s, last_parse_pos_time); - if (!new_pos.has_value()) { - constraint.cb(SSR::CPR_Type::Local_pos_parse, SSR::CPR_Ret_Type::Inter_Error, current_state(msg), msg); - return std::nullopt; - } - auto distance_m = CPR::haversine(old_pos, new_pos.value()); - auto used_time_s = time_s - last_parse_pos_time; - auto max_distance = used_time_s * constraint.max_speed_m_s; - if (constraint.speed_filter && distance_m > max_distance) { - auto var = VAR_STR_6(distance_m, time_s, last_parse_pos_time, used_time_s, constraint.max_speed_m_s, max_distance); - constraint.cb(SSR::CPR_Type::Local_pos_parse, SSR::CPR_Ret_Type::Speed_Error, "本地解码 速度异常：" + var + current_state(msg), msg); - return std::nullopt; - } - constraint.cb(SSR::CPR_Type::Local_pos_parse, SSR::CPR_Ret_Type::Parse_OK, current_state(msg), msg); - return Pos_Ret(new_pos.value(), msg, old_pos); -} template std::string CPR_MSG::current_state(P_S msg) { auto src = msg->source; @@ -63,52 +38,110 @@ std::string CPR_MSG::current_state(P_S msg) { "}}\n", src->get_key(), msg->msg_hex, Psc::to_string(get_cpr_format()), - pos.has_value() ? pos->to_string() : "null", + last_parse_pos.has_value() ? last_parse_pos->to_string() : "null", odd_msg.to_string(), even_msg.to_string(), Psc::to_string(last_parse_pos_time) ); } +// cpr回调 +template +RET CPR_MSG::local_parse_message(CPR::Position old_pos, P_S msg, double time_s, Constraint constraint) { + Psc::JSON info = Psc::JSON::object(); + auto type = SSR::CPR_Type::Local_pos_parse; + info.append({"type", Psc::to_string(type)}); + constraint.cb(type, SSR::CPR_Ret_Type::Base, current_state(msg), msg); + auto diff_time = std::abs(time_s - last_parse_pos_time); + if (constraint.time_space_filter && diff_time > constraint.max_time_s) { + constraint.cb(type, SSR::CPR_Ret_Type::Time_Space_Too_Long, current_state(msg), msg); + return std::nullopt; + } + else { + info.append({"diff_time", std::format("{} = std::abs(time_s:{} - last_parse_pos_time:{})", diff_time, time_s, last_parse_pos_time)}); + info.append({"if", std::format("diff_time:{} <= constraint.max_time_s:{}", diff_time, constraint.max_time_s)}); + } + auto new_pos = locale_parse(msg->msg_bin, old_pos.lat, old_pos.lon, time_s, last_parse_pos_time); + if (!new_pos.has_value()) { + constraint.cb(type, SSR::CPR_Ret_Type::Inter_Error, current_state(msg), msg); + return std::nullopt; + } + else { + info.append({"locale_parse", std::format("msg_hex:{}_{} old_pos:{}", msg->msg_hex, msg->time(), old_pos)}); + } + auto distance_m = CPR::haversine(old_pos, new_pos.value()); + auto used_time_s = time_s - last_parse_pos_time; + auto max_distance = used_time_s * constraint.max_speed_m_s; + if (constraint.speed_filter && distance_m > max_distance) { + auto var = VAR_STR_6(distance_m, time_s, last_parse_pos_time, used_time_s, constraint.max_speed_m_s, max_distance); + constraint.cb(type, SSR::CPR_Ret_Type::Speed_Error, "本地解码 速度异常：" + var + current_state(msg), msg); + return std::nullopt; + } + else { + info.append({"distance_m", std::format("{} = CPR::haversine(pos.value():{}, new_pos.value():{})", distance_m, last_parse_pos.value(), new_pos.value())}); + info.append({"used_time_s", std::format("{} = time_s:{} - last_parse_pos_time:{}", used_time_s, time_s, last_parse_pos_time)}); + info.append({"max_distance", std::format("{} = used_time_s:{} * constraint.max_speed_m_s:{}", max_distance, used_time_s, constraint.max_speed_m_s)}); + info.append({"distance_m", std::format("{} <= max_distance:{}", distance_m, max_distance)}); + } + constraint.cb(type, SSR::CPR_Ret_Type::Parse_OK, current_state(msg), msg); + return Pos_Ret(new_pos.value(), info); +} template RET CPR_MSG::global_parse_message(P_S msg, double time_s, P_S other_msg, double other_time_s, Constraint constraint) { - constraint.cb(SSR::CPR_Type::Global_pos_parse, SSR::CPR_Ret_Type::Base, current_state(msg), msg); + Psc::JSON info = Psc::JSON::object(); + auto type = SSR::CPR_Type::Global_pos_parse; + info.append({"type", Psc::to_string(type)}); + constraint.cb(type, SSR::CPR_Ret_Type::Base, current_state(msg), msg); auto diff_time = std::abs(time_s - other_time_s); if (constraint.time_space_filter && diff_time > constraint.max_time_s) { - constraint.cb(SSR::CPR_Type::Global_pos_parse, SSR::CPR_Ret_Type::Time_Space_Too_Long, current_state(msg), msg); + constraint.cb(type, SSR::CPR_Ret_Type::Time_Space_Too_Long, current_state(msg), msg); return std::nullopt; } + else { + info.append({"diff_time", std::format("{} = std::abs(time_s:{} - other_time_s:{})", diff_time, time_s, other_time_s)}); + info.append({"if", std::format("diff_time:{} <= constraint.max_time_s:{}", diff_time, constraint.max_time_s)}); + } RET new_pos = global_parse(msg->msg_bin, other_msg->msg_bin, time_s, other_time_s); if (!new_pos.has_value()) { - constraint.cb(SSR::CPR_Type::Global_pos_parse, SSR::CPR_Ret_Type::Inter_Error, current_state(msg), msg); + constraint.cb(type, SSR::CPR_Ret_Type::Inter_Error, current_state(msg), msg); return std::nullopt; } - if (constraint.speed_filter && pos.has_value()) { - auto distance_m = CPR::haversine(pos.value(), new_pos.value()); + else { + info.append({"global_parse", std::format("msg:{}_{} other_msg:{}_{}", msg->msg_hex, msg->time(), other_msg->msg_hex, other_msg->time())}); + } + if (constraint.speed_filter && last_parse_pos.has_value()) { + auto distance_m = CPR::haversine(last_parse_pos.value(), new_pos.value()); auto used_time_s = time_s - other_time_s; auto max_distance = used_time_s * constraint.max_speed_m_s; if (distance_m > max_distance) { auto var = VAR_STR_6(distance_m, time_s, last_parse_pos_time, used_time_s, constraint.max_speed_m_s, max_distance); - constraint.cb(SSR::CPR_Type::Global_pos_parse, SSR::CPR_Ret_Type::Speed_Error, "全局解码 速度异常：" + var + current_state(msg), msg); + constraint.cb(type, SSR::CPR_Ret_Type::Speed_Error, "全局解码 速度异常：" + var + current_state(msg), msg); return std::nullopt; } + else { + info.append({"distance_m", std::format("{} = CPR::haversine(pos.value():{}, new_pos.value():{})", distance_m, last_parse_pos.value(), new_pos.value())}); + info.append({"used_time_s", std::format("{} = time_s:{} - other_time_s:{}", used_time_s, time_s, other_time_s)}); + info.append({"max_distance", std::format("{} = used_time_s:{} * constraint.max_speed_m_s:{}", max_distance, used_time_s, constraint.max_speed_m_s)}); + info.append({"if", std::format("distance_m:{} <= max_distance:{}", distance_m, max_distance)}); + } } - constraint.cb(SSR::CPR_Type::Global_pos_parse, SSR::CPR_Ret_Type::Parse_OK, current_state(msg), msg); - return Pos_Ret(new_pos.value(), msg, other_msg); + constraint.cb(type, SSR::CPR_Ret_Type::Parse_OK, current_state(msg), msg); + return Pos_Ret(new_pos.value(), info); } template RET CPR_MSG::parse_message(P_S msg, double time_s, Constraint constraint) { auto f = get_cpr_format(); Info* that = f == CPR::Frame::Even ? &even_msg : &odd_msg; Info* other = f == CPR::Frame::Even ? &odd_msg : &even_msg; - if (!pos.has_value()) { + if (!last_parse_pos.has_value()) { constraint.cb(SSR::CPR_Type::Local_pos_parse, SSR::CPR_Ret_Type::Lack_Old_Pos, "缺旧位置" + current_state(msg), msg); + // 不返回下面接着走 } else { - RET ret = local_parse_message(pos.value(), msg, time_s, constraint); + RET ret = local_parse_message(last_parse_pos.value(), msg, time_s, constraint); if (ret.has_value()) { that->msg = msg; that->time = time_s; - pos = ret.value().pos; + last_parse_pos = ret.value().pos; last_parse_pos_time = time_s; return ret; } @@ -117,19 +150,21 @@ RET CPR_MSG::parse_message(P_S msg, double time_s, Constraint con that->msg = msg; that->time = time_s; constraint.cb(SSR::CPR_Type::Global_pos_parse, SSR::CPR_Ret_Type::Lack_Other_Msg, "缺另一条消息" + current_state(msg), msg); + return std::nullopt; } - else { - RET ret = global_parse_message(msg, time_s, other->msg, other->time, constraint); - if (ret.has_value()) { - that->msg = msg; - that->time = time_s; - pos = ret.value().pos; - last_parse_pos_time = time_s; - return ret; - } + RET ret = global_parse_message(msg, time_s, other->msg, other->time, constraint); + if (ret.has_value()) { + that->msg = msg; + that->time = time_s; + last_parse_pos = ret.value().pos; + last_parse_pos_time = time_s; + return ret; } - last_parse_pos_time = 0; - pos = std::nullopt; + // 此时新来的点本地解码失败 全局解码也失败 很可能是 中间丢了很多消息 所以旧的pos + // last_parse_pos_time = 0; + // pos = std::nullopt; + that->msg = msg; + that->time = time_s; other->reset(); return std::nullopt; }