ae/SSR
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

轨迹点信息优化

Browse Source
This commit is contained in:
种太阳的热带美短
2026-07-16 17:51:11 +08:00
parent f8277ac511
commit 6d1ad2780c
4 changed files with 154 additions and 122 deletions
Download Patch File Download Diff File Expand all files Collapse all files
SSR/ADS_B/ADS_B.hpp
+5 -5
View File
@@ -72,7 +72,7 @@ struct CPR_Data {
std::function<std::optional<CPR::Position>(std::string_view, std::string_view, time_t, time_t)> global_parse = nullptr; std::function<std::optional<CPR::Position>(std::string_view, std::string_view, time_t, time_t)> global_parse = nullptr;
std::function<std::optional<CPR::Position>(std::string_view, double lat, double lon, time_t, time_t)> locale_parse = nullptr; std::function<std::optional<CPR::Position>(std::string_view, double lat, double lon, time_t, time_t)> locale_parse = nullptr;
std::function<std::string(double, double, CPR::Frame)> encode = nullptr; std::function<std::string(double, double, CPR::Frame)> encode = nullptr;
std::optional<CPR::Position> pos; std::optional<CPR::Position> last_parse_pos;
CPR::Frame cpr_format = CPR::Frame::Even; CPR::Frame cpr_format = CPR::Frame::Even;
}; };
struct Constraint { struct Constraint {
@@ -91,7 +91,7 @@ struct CPR_MSG : CPR_Data {
std::string current_state(P_S msg); std::string current_state(P_S msg);
Psc::JSON current_state_json() { Psc::JSON current_state_json() {
Psc::JSON ret = Psc::JSON::object(); 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({"odd_msg", odd_msg.to_json()});
ret.append({"even_msg", even_msg.to_json()}); ret.append({"even_msg", even_msg.to_json()});
return ret; return ret;
@@ -104,15 +104,15 @@ struct CPR_MSG : CPR_Data {
} }
virtual Psc::JSON toJson() { virtual Psc::JSON toJson() {
Psc::JSON ret = Psc::JSON::object(); Psc::JSON ret = Psc::JSON::object();
Ret_J(pos) Ret_J(last_parse_pos)
Ret_J(cpr_format) Ret_J(cpr_format)
return ret; return ret;
} }
virtual void set(std::string& msg_bin) { virtual void set(std::string& msg_bin) {
set_bin<53, 1>(msg_bin, static_cast<int>(this->cpr_format)); set_bin<53, 1>(msg_bin, static_cast<int>(this->cpr_format));
if (pos.has_value()) { if (last_parse_pos.has_value()) {
set_bin<54, 34>(msg_bin, 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: private:
SSR/CPR/CPR.h
+48 -38
View File
@@ -14,20 +14,20 @@ using Time = long double;
using D = long double; using D = long double;
using I = std::int64_t; using I = std::int64_t;
struct Position { struct Position {
Position(D lat, D lon) : lat(lat), lon(lon) {} Position(D lat, D lon) : lat(lat), lon(lon) {}
D lat; D lat;
D lon; D lon;
[[nodiscard]] std::string to_string() const; [[nodiscard]] std::string to_string() const;
}; };
enum Frame { enum Frame {
Even = 0, // 偶数帧 Even = 0, // 偶数帧
ODD = 1 // 奇数帧 ODD = 1 // 奇数帧
}; };
using POS = std::optional<Position>; using POS = std::optional<Position>;
const I NZ = 15; const I NZ = 15;
template <typename T = D> template <typename T = D>
T floor(D x) { T floor(D x) {
return static_cast<T>(std::floor(x)); return static_cast<T>(std::floor(x));
} }
D MOD(D x, D y); D MOD(D x, D y);
I NL(D lat); 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 Nb1 = 19;
static I Nb2 = 17; static I Nb2 = 17;
inline std::tuple<I, I> surface_encode(D lat, D lon, I i) { inline std::tuple<I, I> surface_encode(D lat, D lon, I i) {
auto [YZ, XZ] = encode(lat, lon, Nb1, i); auto [YZ, XZ] = encode(lat, lon, Nb1, i);
I base = 2 << (Nb2 - 1); I base = 2 << (Nb2 - 1);
YZ %= base; YZ %= base;
XZ %= base; XZ %= base;
return {YZ, XZ}; return {YZ, XZ};
} }
inline I zone_index(D lon) { 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) { inline D bb(D lon, D lon_s) {
auto j = zone_index(lon); auto j = zone_index(lon);
auto i = zone_index(lon_s); auto i = zone_index(lon_s);
D change = (i - j) * 90.0; D change = (i - j) * 90.0;
D lon2 = lon + change; D lon2 = lon + change;
return lon2; return lon2;
} }
inline std::optional<std::tuple<Position, Position>> surface_global_decode(I YZ0, I YZ1, I XZ0, I XZ1) { inline std::optional<std::tuple<Position, Position>> surface_global_decode(I YZ0, I YZ1, I XZ0, I XZ1) {
auto poses = global_decode(YZ0, YZ1, XZ0, XZ1, Nb2, 90.0); auto poses = global_decode(YZ0, YZ1, XZ0, XZ1, Nb2, 90.0);
// auto pos = global_decode(YZ0, YZ1, XZ0, XZ1, t0, t1,Nb, 360.0); // auto pos = global_decode(YZ0, YZ1, XZ0, XZ1, t0, t1,Nb, 360.0);
if (!poses.has_value()) { if (!poses.has_value()) {
return std::nullopt; return std::nullopt;
} }
auto [pos0, pos1] = poses.value(); auto [pos0, pos1] = poses.value();
return std::tuple(pos0, pos1); return std::tuple(pos0, pos1);
} }
inline std::optional<std::tuple<Position, Position>> surface_global_decode_ref(I YZ0, I YZ1, I XZ0, I XZ1, D lat_s, D lon_s) { inline std::optional<std::tuple<Position, Position>> 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); auto poses = surface_global_decode(YZ0, YZ1, XZ0, XZ1);
if (!poses.has_value()) return std::nullopt; if (!poses.has_value()) return std::nullopt;
auto [pos0, pos1] = poses.value(); auto [pos0, pos1] = poses.value();
auto p0 = Position{bb(pos0.lat, lat_s), bb(pos0.lon, lon_s)}; 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)}; auto p1 = Position{bb(pos1.lat, lat_s), bb(pos1.lon, lon_s)};
return std::tuple(p0, p1); return std::tuple(p0, p1);
} }
inline POS surface_locale_decode(D lat_s, D lon_s, I YZi, I XZi, I i) { 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, Nb2, 90.0);
// auto pos = locale_decode(lat_s, lon_s, YZi, XZi, i, Nb, 360.0); // auto pos = locale_decode(lat_s, lon_s, YZi, XZi, i, Nb, 360.0);
if (!pos.has_value()) { if (!pos.has_value()) {
return std::nullopt; return std::nullopt;
} }
auto [lat, lon] = pos.value(); auto [lat, lon] = pos.value();
return Position{bb(lat, lat_s), bb(lon, lon_s)}; return Position{bb(lat, lat_s), bb(lon, lon_s)};
} }
// time // time
std::string lat_lon_airborne_34(double latitude, double longitude, 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); double& latitude, double& altitude);
} // namespace CPR } // namespace CPR
} // namespace SSR } // namespace SSR
#include <format>
template <>
struct std::formatter<SSR::CPR::Position> {
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);
}
};
SSR/Msg.h
+18 -31
View File
@@ -5,6 +5,7 @@
#include "convert/DataOutputFormats.h" #include "convert/DataOutputFormats.h"
#include "enum.h" #include "enum.h"
#include <functional> #include <functional>
#include <utility>
#include <variant> #include <variant>
#define STRINGIFY(x) #x #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) {} Position_3D(CPR::D lat, CPR::D lon, CPR::D alt) : CPR::Position(lat, lon), alt(alt) {}
}; };
// 这个返回值的意义在于 解码失败后 返回值能反应出是 位置解码失败 // 这个返回值的意义在于 解码失败后 返回值能反应出是 位置解码失败
// 还是奇偶报文解码位置失败 // 还是奇偶报文解码位置失败 轨迹点上会显示的信息
class Pos_Ret { class Pos_Ret {
public: public:
CPR::Position pos; CPR::Position pos;
std::shared_ptr<Mode_S_Msg> that; Psc::JSON info;
using Extra = std::variant<std::shared_ptr<Mode_S_Msg>, CPR::Position>; Pos_Ret(CPR::Position p, Psc::JSON info) : pos(p), info(std::move(info)) {}
Extra extra;
Pos_Ret(CPR::Position p, std::shared_ptr<Mode_S_Msg> t,
std::shared_ptr<Mode_S_Msg> o) : pos(p), that(std::move(t)), extra(std::move(o)) {}
Pos_Ret(CPR::Position p, std::shared_ptr<Mode_S_Msg> t, CPR::Position old) : pos(p), that(std::move(t)),
extra(old) {}
}; };
// 展示出最详细的信息 // 展示出最详细的信息
struct Position_Info : Position_3D { struct Position_Info : Position_3D {
@@ -222,10 +218,8 @@ struct Position_Info : Position_3D {
std::uint64_t utc{}; std::uint64_t utc{};
std::uint64_t odd_even_diff_ns{}; std::uint64_t odd_even_diff_ns{};
Position_Info() : Position_3D(0.0, 0.0, 0.0) {} Position_Info() : Position_3D(0.0, 0.0, 0.0) {}
Pos_Ret::Extra extra; Psc::JSON info;
std::shared_ptr<Mode_S_Msg> that; Position_Info(const Pos_Ret& r, CPR::D alt) : Position_3D(r.pos.lat, r.pos.lon, alt), info(r.info) {
Position_Info(const Pos_Ret& r, CPR::D alt) : Position_3D(r.pos.lat, r.pos.lon, alt), that(r.that),
extra(r.extra) {
using namespace std::chrono; using namespace std::chrono;
utc = static_cast<std::uint64_t>( utc = static_cast<std::uint64_t>(
duration_cast<seconds>(system_clock::now().time_since_epoch()).count()); duration_cast<seconds>(system_clock::now().time_since_epoch()).count());
@@ -236,30 +230,14 @@ struct Position_Info : Position_3D {
Ret_J(lon) Ret_J(lon)
Ret_J(alt) Ret_J(alt)
Ret_J(utc) Ret_J(utc)
Ret_J(that->mlat_timestamp) ret.append_list(info.children);
Ret_J(that->msg_hex)
Ret_J(that->signal_level)
if (auto p = std::get_if<
std::shared_ptr<Mode_S_Msg>>(
&extra)) {
std::shared_ptr<Mode_S_Msg> 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<CPR::Position>(&extra)) {
CPR::Position old = *p;
Ret_J(old)
return ret;
}
return ret; return ret;
} }
}; };
struct POS_List { struct POS_List {
NoneCopyLock mtx; NoneCopyLock mtx;
size_t max_size; size_t max_size{};
POS_List() {} POS_List() = default;
void reset() { void reset() {
std::lock_guard g(mtx.mtx); std::lock_guard g(mtx.mtx);
seq = 0; seq = 0;
@@ -300,3 +278,12 @@ protected:
std::vector<Position_Info> buf; std::vector<Position_Info> buf;
}; };
} // namespace SSR } // namespace SSR
template <>
struct std::formatter<SSR::Play_Back_Time_Point> {
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());
}
};
SSR/export.hpp
+83 -48
View File
@@ -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); if (mode_s_logger) mode_s_logger->debug("CPR/" + Psc::to_string(t) + "/" + Psc::to_string(err_type), {}, log);
}; };
namespace ADS_B_T { namespace ADS_B_T {
// cpr回调
template <typename That>
RET<Pos_Ret> CPR_MSG<That>::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 <typename That> template <typename That>
std::string CPR_MSG<That>::current_state(P_S msg) { std::string CPR_MSG<That>::current_state(P_S msg) {
auto src = msg->source; auto src = msg->source;
@@ -63,52 +38,110 @@ std::string CPR_MSG<That>::current_state(P_S msg) {
"}}\n", "}}\n",
src->get_key(), src->get_key(),
msg->msg_hex, Psc::to_string(get_cpr_format()), 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(), odd_msg.to_string(),
even_msg.to_string(), even_msg.to_string(),
Psc::to_string(last_parse_pos_time) Psc::to_string(last_parse_pos_time)
); );
} }
// cpr回调
template <typename That>
RET<Pos_Ret> CPR_MSG<That>::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 <typename That> template <typename That>
RET<Pos_Ret> CPR_MSG<That>::global_parse_message(P_S msg, double time_s, P_S other_msg, double other_time_s, Constraint constraint) { RET<Pos_Ret> CPR_MSG<That>::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); auto diff_time = std::abs(time_s - other_time_s);
if (constraint.time_space_filter && diff_time > constraint.max_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; 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<CPR::Position> new_pos = global_parse(msg->msg_bin, other_msg->msg_bin, time_s, other_time_s); RET<CPR::Position> new_pos = global_parse(msg->msg_bin, other_msg->msg_bin, time_s, other_time_s);
if (!new_pos.has_value()) { 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; return std::nullopt;
} }
if (constraint.speed_filter && pos.has_value()) { else {
auto distance_m = CPR::haversine(pos.value(), new_pos.value()); 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 used_time_s = time_s - other_time_s;
auto max_distance = used_time_s * constraint.max_speed_m_s; auto max_distance = used_time_s * constraint.max_speed_m_s;
if (distance_m > max_distance) { 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); 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; 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); constraint.cb(type, SSR::CPR_Ret_Type::Parse_OK, current_state(msg), msg);
return Pos_Ret(new_pos.value(), msg, other_msg); return Pos_Ret(new_pos.value(), info);
} }
template <typename That> template <typename That>
RET<Pos_Ret> CPR_MSG<That>::parse_message(P_S msg, double time_s, Constraint constraint) { RET<Pos_Ret> CPR_MSG<That>::parse_message(P_S msg, double time_s, Constraint constraint) {
auto f = get_cpr_format(); auto f = get_cpr_format();
Info* that = f == CPR::Frame::Even ? &even_msg : &odd_msg; Info* that = f == CPR::Frame::Even ? &even_msg : &odd_msg;
Info* other = f == CPR::Frame::Even ? &odd_msg : &even_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); constraint.cb(SSR::CPR_Type::Local_pos_parse, SSR::CPR_Ret_Type::Lack_Old_Pos, "缺旧位置" + current_state(msg), msg);
// 不返回下面接着走
} }
else { else {
RET<Pos_Ret> ret = local_parse_message(pos.value(), msg, time_s, constraint); RET<Pos_Ret> ret = local_parse_message(last_parse_pos.value(), msg, time_s, constraint);
if (ret.has_value()) { if (ret.has_value()) {
that->msg = msg; that->msg = msg;
that->time = time_s; that->time = time_s;
pos = ret.value().pos; last_parse_pos = ret.value().pos;
last_parse_pos_time = time_s; last_parse_pos_time = time_s;
return ret; return ret;
} }
@@ -117,19 +150,21 @@ RET<Pos_Ret> CPR_MSG<That>::parse_message(P_S msg, double time_s, Constraint con
that->msg = msg; that->msg = msg;
that->time = time_s; that->time = time_s;
constraint.cb(SSR::CPR_Type::Global_pos_parse, SSR::CPR_Ret_Type::Lack_Other_Msg, "缺另一条消息" + current_state(msg), msg); constraint.cb(SSR::CPR_Type::Global_pos_parse, SSR::CPR_Ret_Type::Lack_Other_Msg, "缺另一条消息" + current_state(msg), msg);
return std::nullopt;
} }
else { RET<Pos_Ret> ret = global_parse_message(msg, time_s, other->msg, other->time, constraint);
RET<Pos_Ret> ret = global_parse_message(msg, time_s, other->msg, other->time, constraint); if (ret.has_value()) {
if (ret.has_value()) { that->msg = msg;
that->msg = msg; that->time = time_s;
that->time = time_s; last_parse_pos = ret.value().pos;
pos = ret.value().pos; last_parse_pos_time = time_s;
last_parse_pos_time = time_s; return ret;
return ret;
}
} }
last_parse_pos_time = 0; // 此时新来的点本地解码失败 全局解码也失败 很可能是 中间丢了很多消息 所以旧的pos
pos = std::nullopt; // last_parse_pos_time = 0;
// pos = std::nullopt;
that->msg = msg;
that->time = time_s;
other->reset(); other->reset();
return std::nullopt; return std::nullopt;
} }