Skip to content

update FP_A-ODOMSTATUS #196

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
flipflip8952 merged 1 commit into from
Jun 4, 2026
Merged

update FP_A-ODOMSTATUS #196

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 7 additions & 5 deletions fpsdk_common/include/fpsdk_common/parser/fpa.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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;
Expand Down
2 changes: 2 additions & 0 deletions fpsdk_common/include/fpsdk_common/to_json/parser_fpa.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 },
Expand All @@ -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 },
Expand Down
39 changes: 20 additions & 19 deletions fpsdk_common/src/parser/fpa.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 "?";
}
Expand Down Expand Up @@ -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 {
Expand Down Expand Up @@ -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]));
}
Expand Down
38 changes: 38 additions & 0 deletions fpsdk_common/test/parser_fpa_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand All @@ -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);
}
}

// ---------------------------------------------------------------------------------------------------------------------
Expand Down
Loading