From 5ccd0e031c14056055e9f531ce5c912059a064f7 Mon Sep 17 00:00:00 2001 From: flipflip Date: Thu, 4 Jun 2026 09:11:54 +0200 Subject: [PATCH] update FP_A-ODOMSTATUS --- .../include/fpsdk_common/parser/fpa.hpp | 12 +++--- .../fpsdk_common/to_json/parser_fpa.hpp | 2 + fpsdk_common/src/parser/fpa.cpp | 39 ++++++++++--------- fpsdk_common/test/parser_fpa_test.cpp | 38 ++++++++++++++++++ 4 files changed, 67 insertions(+), 24 deletions(-) diff --git a/fpsdk_common/include/fpsdk_common/parser/fpa.hpp b/fpsdk_common/include/fpsdk_common/parser/fpa.hpp index 2e5032b..e0bb828 100644 --- a/fpsdk_common/include/fpsdk_common/parser/fpa.hpp +++ b/fpsdk_common/include/fpsdk_common/parser/fpa.hpp @@ -389,8 +389,8 @@ enum class FpaCamStatus : int UNSPECIFIED = '!', //!< Unspecified CAM_UNAVL = '0', //!< Camera not available BAD_FEAT = '1', //!< Camera available, but not usable (e.g. too dark) - RESERVED2 = '2', //!< Reserved - RESERVED3 = '3', //!< Reserved + NOT_CONVERGED = '2', //!< Camera not converged + WARMSTARTED = '3', //!< Camera warmstarted (previously converged) RESERVED4 = '4', //!< Reserved GOOD = '5', //!< Camera working and available }; // clang-format on @@ -984,7 +984,8 @@ struct FpaOdomstatusPayload : public FpaPayload FpaMeasStatus fusion_gnss1; //!< Fusion measurement status: GNSS 1 FpaMeasStatus fusion_gnss2; //!< Fusion measurement status: GNSS 2 FpaMeasStatus fusion_corr; //!< Fusion measurement status: GNSS corrections - FpaMeasStatus fusion_cam1; //!< Fusion measurement status: camera + FpaMeasStatus fusion_cam1; //!< Fusion measurement status: camera 1 + FpaMeasStatus fusion_cam2; //!< Fusion measurement status: camera 2 FpaMeasStatus fusion_ws; //!< Fusion measurement status: wheelspeed FpaMeasStatus fusion_markers; //!< Fusion measurement status: markers FpaImuStatus imu_status; //!< IMU bias status @@ -995,10 +996,11 @@ struct FpaOdomstatusPayload : public FpaPayload FpaBaselineStatus baseline_status; //!< Baseline status FpaCorrStatus corr_status; //!< GNSS correction status FpaCamStatus cam1_status; //!< Camera 1 status + FpaCamStatus cam2_status; //!< Camera 2 status FpaWsStatus ws_status; //!< Wheelspeed status FpaWsConv ws_conv; //!< Wheelspeed convergence status - FpaMarkersStatus markers_status; //!< Marker status - FpaMarkersConv markers_conv; //!< Marker convergence status + FpaMarkersStatus markers_status; //!< Markers status + FpaMarkersConv markers_conv; //!< Markers convergence status // clang-format on bool SetFromMsg(const uint8_t* msg, const std::size_t msg_size) final; diff --git a/fpsdk_common/include/fpsdk_common/to_json/parser_fpa.hpp b/fpsdk_common/include/fpsdk_common/to_json/parser_fpa.hpp index 083347f..90478fd 100644 --- a/fpsdk_common/include/fpsdk_common/to_json/parser_fpa.hpp +++ b/fpsdk_common/include/fpsdk_common/to_json/parser_fpa.hpp @@ -346,6 +346,7 @@ inline void to_json(nlohmann::json& j, const FpaOdomstatusPayload& m) { "fusion_gnss2", m.fusion_gnss2 }, { "fusion_corr", m.fusion_corr }, { "fusion_cam1", m.fusion_cam1 }, + { "fusion_cam2", m.fusion_cam2 }, { "fusion_ws", m.fusion_ws }, { "fusion_markers", m.fusion_markers }, { "imu_status", m.imu_status }, @@ -356,6 +357,7 @@ inline void to_json(nlohmann::json& j, const FpaOdomstatusPayload& m) { "baseline_status", m.baseline_status }, { "corr_status", m.corr_status }, { "cam1_status", m.cam1_status }, + { "cam2_status", m.cam2_status }, { "ws_status", m.ws_status }, { "ws_conv", m.ws_conv }, { "markers_status", m.markers_status }, diff --git a/fpsdk_common/src/parser/fpa.cpp b/fpsdk_common/src/parser/fpa.cpp index b91b444..0879810 100644 --- a/fpsdk_common/src/parser/fpa.cpp +++ b/fpsdk_common/src/parser/fpa.cpp @@ -283,13 +283,13 @@ const char* FpaBaselineStatusStr(const FpaBaselineStatus status) const char* FpaCamStatusStr(const FpaCamStatus status) { switch (status) { // clang-format off - case FpaCamStatus::UNSPECIFIED: return "UNSPECIFIED"; - case FpaCamStatus::CAM_UNAVL: return "CAM_UNAVL"; - case FpaCamStatus::BAD_FEAT: return "BAD_FEAT"; - case FpaCamStatus::RESERVED2: return "RESERVED2"; - case FpaCamStatus::RESERVED3: return "RESERVED3"; - case FpaCamStatus::RESERVED4: return "RESERVED4"; - case FpaCamStatus::GOOD: return "GOOD"; + case FpaCamStatus::UNSPECIFIED: return "UNSPECIFIED"; + case FpaCamStatus::CAM_UNAVL: return "CAM_UNAVL"; + case FpaCamStatus::BAD_FEAT: return "BAD_FEAT"; + case FpaCamStatus::NOT_CONVERGED: return "NOT_CONVERGED"; + case FpaCamStatus::WARMSTARTED: return "WARMSTARTED"; + case FpaCamStatus::RESERVED4: return "RESERVED4"; + case FpaCamStatus::GOOD: return "GOOD"; } // clang-format on return "?"; } @@ -784,12 +784,12 @@ static bool GetCamStatus(FpaCamStatus& status, const std::string& field) status = FpaCamStatus::UNSPECIFIED; if (!field.empty()) { switch ((FpaCamStatus)field[0]) { // clang-format off - case FpaCamStatus::CAM_UNAVL: status = FpaCamStatus::CAM_UNAVL; ok = true; break; - case FpaCamStatus::BAD_FEAT: status = FpaCamStatus::BAD_FEAT; ok = true; break; - case FpaCamStatus::RESERVED2: status = FpaCamStatus::RESERVED2; ok = true; break; - case FpaCamStatus::RESERVED3: status = FpaCamStatus::RESERVED3; ok = true; break; - case FpaCamStatus::RESERVED4: status = FpaCamStatus::RESERVED4; ok = true; break; - case FpaCamStatus::GOOD: status = FpaCamStatus::GOOD; ok = true; break; + case FpaCamStatus::CAM_UNAVL: status = FpaCamStatus::CAM_UNAVL; ok = true; break; + case FpaCamStatus::BAD_FEAT: status = FpaCamStatus::BAD_FEAT; ok = true; break; + case FpaCamStatus::NOT_CONVERGED: status = FpaCamStatus::NOT_CONVERGED; ok = true; break; + case FpaCamStatus::WARMSTARTED: status = FpaCamStatus::WARMSTARTED; ok = true; break; + case FpaCamStatus::RESERVED4: status = FpaCamStatus::RESERVED4; ok = true; break; + case FpaCamStatus::GOOD: status = FpaCamStatus::GOOD; ok = true; break; case FpaCamStatus::UNSPECIFIED: break; } // clang-format on } else { @@ -1433,12 +1433,13 @@ bool FpaOdomstatusPayload::SetFromMsg(const uint8_t* msg, const std::size_t msg_ ok = (GetGpsTime(gps_time, m.fields_, 0, false) && GetInitStatus(init_status, m.fields_[2]) && GetMeasStatus(fusion_imu, m.fields_[3]) && GetMeasStatus(fusion_gnss1, m.fields_[4]) && GetMeasStatus(fusion_gnss2, m.fields_[5]) && GetMeasStatus(fusion_corr, m.fields_[6]) && - GetMeasStatus(fusion_cam1, m.fields_[7]) && GetMeasStatus(fusion_ws, m.fields_[9]) && - GetMeasStatus(fusion_markers, m.fields_[10]) && GetImuStatus(imu_status, m.fields_[15]) && - GetImuNoise(imu_noise, m.fields_[16]) && GetImuConv(imu_conv, m.fields_[17]) && - GetGnssStatus(gnss1_status, m.fields_[18]) && GetGnssStatus(gnss2_status, m.fields_[19]) && - GetBaselineStatus(baseline_status, m.fields_[20]) && GetCorrStatus(corr_status, m.fields_[21]) && - GetCamStatus(cam1_status, m.fields_[22]) && GetWsStatus(ws_status, m.fields_[24]) && + GetMeasStatus(fusion_cam1, m.fields_[7]) && GetMeasStatus(fusion_cam2, m.fields_[8]) && + GetMeasStatus(fusion_ws, m.fields_[9]) && GetMeasStatus(fusion_markers, m.fields_[10]) && + GetImuStatus(imu_status, m.fields_[15]) && GetImuNoise(imu_noise, m.fields_[16]) && + GetImuConv(imu_conv, m.fields_[17]) && GetGnssStatus(gnss1_status, m.fields_[18]) && + GetGnssStatus(gnss2_status, m.fields_[19]) && GetBaselineStatus(baseline_status, m.fields_[20]) && + GetCorrStatus(corr_status, m.fields_[21]) && GetCamStatus(cam1_status, m.fields_[22]) && + GetCamStatus(cam2_status, m.fields_[23]) && GetWsStatus(ws_status, m.fields_[24]) && GetWsConv(ws_conv, m.fields_[25]) && GetMarkersStatus(markers_status, m.fields_[26]) && GetMarkersConv(markers_conv, m.fields_[27])); } diff --git a/fpsdk_common/test/parser_fpa_test.cpp b/fpsdk_common/test/parser_fpa_test.cpp index 707f83c..c41acf3 100644 --- a/fpsdk_common/test/parser_fpa_test.cpp +++ b/fpsdk_common/test/parser_fpa_test.cpp @@ -747,6 +747,7 @@ TEST(ParserFpaTest, FpaOdomstatusPayload) EXPECT_EQ(payload.fusion_gnss2, FpaMeasStatus::USED); EXPECT_EQ(payload.fusion_corr, FpaMeasStatus::USED); EXPECT_EQ(payload.fusion_cam1, FpaMeasStatus::USED); + EXPECT_EQ(payload.fusion_cam2, FpaMeasStatus::UNSPECIFIED); EXPECT_EQ(payload.fusion_ws, FpaMeasStatus::NOT_USED); EXPECT_EQ(payload.fusion_markers, FpaMeasStatus::UNSPECIFIED); EXPECT_EQ(payload.imu_status, FpaImuStatus::WARMSTARTED); @@ -757,6 +758,7 @@ TEST(ParserFpaTest, FpaOdomstatusPayload) EXPECT_EQ(payload.baseline_status, FpaBaselineStatus::PASSING); EXPECT_EQ(payload.corr_status, FpaCorrStatus::GOOD_CORR); EXPECT_EQ(payload.cam1_status, FpaCamStatus::GOOD); + EXPECT_EQ(payload.cam2_status, FpaCamStatus::UNSPECIFIED); EXPECT_EQ(payload.ws_status, FpaWsStatus::NOT_ENABLED); EXPECT_EQ(payload.ws_conv, FpaWsConv::IDLE); EXPECT_EQ(payload.markers_status, FpaMarkersStatus::UNSPECIFIED); @@ -778,6 +780,7 @@ TEST(ParserFpaTest, FpaOdomstatusPayload) EXPECT_EQ(payload.fusion_gnss2, FpaMeasStatus::UNSPECIFIED); EXPECT_EQ(payload.fusion_corr, FpaMeasStatus::UNSPECIFIED); EXPECT_EQ(payload.fusion_cam1, FpaMeasStatus::UNSPECIFIED); + EXPECT_EQ(payload.fusion_cam2, FpaMeasStatus::UNSPECIFIED); EXPECT_EQ(payload.fusion_ws, FpaMeasStatus::UNSPECIFIED); EXPECT_EQ(payload.fusion_markers, FpaMeasStatus::UNSPECIFIED); EXPECT_EQ(payload.imu_status, FpaImuStatus::UNSPECIFIED); @@ -788,11 +791,46 @@ TEST(ParserFpaTest, FpaOdomstatusPayload) EXPECT_EQ(payload.baseline_status, FpaBaselineStatus::UNSPECIFIED); EXPECT_EQ(payload.corr_status, FpaCorrStatus::UNSPECIFIED); EXPECT_EQ(payload.cam1_status, FpaCamStatus::UNSPECIFIED); + EXPECT_EQ(payload.cam2_status, FpaCamStatus::UNSPECIFIED); EXPECT_EQ(payload.ws_status, FpaWsStatus::UNSPECIFIED); EXPECT_EQ(payload.ws_conv, FpaWsConv::UNSPECIFIED); EXPECT_EQ(payload.markers_status, FpaMarkersStatus::UNSPECIFIED); EXPECT_EQ(payload.markers_conv, FpaMarkersConv::UNSPECIFIED); } + { + FpaOdomstatusPayload payload; + const char* msg = // clang-format off + "$FP,ODOMSTATUS,1,2421,199196.400000,2,2,0,0,0,1,0,0,,,,,,1,1,3,0,0,1,1,5,0,0,6,,,,,,,,,,,,*2B\r\n"; // clang-format on + EXPECT_TRUE(payload.SetFromMsg((const uint8_t*)msg, std::strlen(msg))); + EXPECT_EQ(payload.msg_type_, FpaMessageType::ODOMSTATUS); + EXPECT_TRUE(payload.valid_); + EXPECT_TRUE(payload.gps_time.week.valid); + EXPECT_EQ(payload.gps_time.week.value, 2421); + EXPECT_TRUE(payload.gps_time.tow.valid); + EXPECT_NEAR(payload.gps_time.tow.value, 199196.400000, 1e-9); + EXPECT_EQ(payload.init_status, FpaInitStatus::GLOBAL_INIT); + EXPECT_EQ(payload.fusion_imu, FpaMeasStatus::DEGRADED); + EXPECT_EQ(payload.fusion_gnss1, FpaMeasStatus::NOT_USED); + EXPECT_EQ(payload.fusion_gnss2, FpaMeasStatus::NOT_USED); + EXPECT_EQ(payload.fusion_corr, FpaMeasStatus::NOT_USED); + EXPECT_EQ(payload.fusion_cam1, FpaMeasStatus::USED); + EXPECT_EQ(payload.fusion_cam2, FpaMeasStatus::NOT_USED); + EXPECT_EQ(payload.fusion_ws, FpaMeasStatus::NOT_USED); + EXPECT_EQ(payload.fusion_markers, FpaMeasStatus::UNSPECIFIED); + EXPECT_EQ(payload.imu_status, FpaImuStatus::WARMSTARTED); + EXPECT_EQ(payload.imu_noise, FpaImuNoise::LOW_NOISE); + EXPECT_EQ(payload.imu_conv, FpaImuConv::WAIT_MOTION); + EXPECT_EQ(payload.gnss1_status, FpaGnssStatus::NO_FIX); + EXPECT_EQ(payload.gnss2_status, FpaGnssStatus::NO_FIX); + EXPECT_EQ(payload.baseline_status, FpaBaselineStatus::NO_FIX); + EXPECT_EQ(payload.corr_status, FpaCorrStatus::NO_GNSS); + EXPECT_EQ(payload.cam1_status, FpaCamStatus::GOOD); + EXPECT_EQ(payload.cam2_status, FpaCamStatus::CAM_UNAVL); + EXPECT_EQ(payload.ws_status, FpaWsStatus::NOT_ENABLED); + EXPECT_EQ(payload.ws_conv, FpaWsConv::IDLE); + EXPECT_EQ(payload.markers_status, FpaMarkersStatus::UNSPECIFIED); + EXPECT_EQ(payload.markers_conv, FpaMarkersConv::UNSPECIFIED); + } } // ---------------------------------------------------------------------------------------------------------------------