diff --git a/fpsdk_common/include/fpsdk_common/parser/fpa.hpp b/fpsdk_common/include/fpsdk_common/parser/fpa.hpp index e0bb828..6d91b0f 100644 --- a/fpsdk_common/include/fpsdk_common/parser/fpa.hpp +++ b/fpsdk_common/include/fpsdk_common/parser/fpa.hpp @@ -671,6 +671,43 @@ enum class FpaTimeref : int */ const char* FpaTimerefStr(const FpaTimeref ref); +/** + * @brief FP_A-CONFIG action + */ +enum class FpaConfigAction : int +{ + UNSPECIFIED = 0, //!< Unspecified + DEFAULT, //!< Default +}; + +/** + * @brief Stringify action + * + * @param[in] action The action + * + * @returns the stringification of the action ("DEFAULT", etc.) + */ +const char* FpaConfigActionStr(const FpaConfigAction action); + +/** + * @brief FP_A-ACK result + */ +enum class FpaAckRes : int +{ + UNSPECIFIED = 0, //!< Unspecified + OK, //!< Positive acknowledgement + FAIL, //!< Negative acknowledgement +}; + +/** + * @brief Stringify result + * + * @param[in] res The result + * + * @returns the stringification of the result ("OK", "FAIL", ..) + */ +const char* FpaAckResStr(const FpaAckRes res); + /** * @brief FP_A integer value */ @@ -753,8 +790,19 @@ enum class FpaMessageType TF, //!< FpaTfPayload TP, //!< FpaTpPayload VERSION, //!< FpaVersionPayload + CONFIG, //!< FpaConfigPayload + ACK, //!< FpaAckPayload }; +/** + * @brief Stringify FP_A message type + * + * @param[in] msg_type The message type + * + * @returns the stringification of the message type + */ +const char* FpaMessageTypeStr(const FpaMessageType msg_type); + /** * @brief FP_A payload base class */ @@ -1075,6 +1123,33 @@ struct FpaVersionPayload : public FpaPayload static constexpr const char* MSG_NAME = "FP_A-VERSION"; //!< Message name }; +/** + * @brief FP_A-CONFIG (version 1) message payload + */ +struct FpaConfigPayload : public FpaPayload +{ // clang-format off + FpaConfigAction action = FpaConfigAction::UNSPECIFIED; //!< Action + // clang-format on + + bool SetFromMsg(const uint8_t* msg, const std::size_t msg_size) final; + + static constexpr const char* MSG_NAME = "FP_A-CONFIG"; //!< Message name +}; + +/** + * @brief FP_A-ACK (version 1) message payload + */ +struct FpaAckPayload : public FpaPayload +{ // clang-format off + FpaMessageType ack_msg = FpaMessageType::UNSPECIFIED; //!< Message is being acknowledged + FpaAckRes ack_res = FpaAckRes::UNSPECIFIED; //!< Acknowledge result + // clang-format on + + bool SetFromMsg(const uint8_t* msg, const std::size_t msg_size) final; + + static constexpr const char* MSG_NAME = "FP_A-ACK"; //!< Message name +}; + // --------------------------------------------------------------------------------------------------------------------- //! Pointer to FP_A payload 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 90478fd..339918d 100644 --- a/fpsdk_common/include/fpsdk_common/to_json/parser_fpa.hpp +++ b/fpsdk_common/include/fpsdk_common/to_json/parser_fpa.hpp @@ -419,6 +419,25 @@ inline void to_json(nlohmann::json& j, const FpaVersionPayload& m) // --------------------------------------------------------------------------------------------------------------------- +inline void to_json(nlohmann::json& j, const FpaConfigPayload& m) +{ + j = nlohmann::json::object({ + { "action", FpaConfigActionStr(m.action) }, + }); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const FpaAckPayload& m) +{ + j = nlohmann::json::object({ + { "ack_msg", FpaMessageTypeStr(m.ack_msg) }, + { "ack_res", FpaAckResStr(m.ack_res) }, + }); +} + +// --------------------------------------------------------------------------------------------------------------------- + inline void to_json(nlohmann::json& j, const FpaPayloadPtr& fpa) { j = nlohmann::json::object(); @@ -444,6 +463,8 @@ inline void to_json(nlohmann::json& j, const FpaPayloadPtr& fpa) case FpaMessageType::TF: j.update(dynamic_cast(*fpa)); break; case FpaMessageType::TP: j.update(dynamic_cast(*fpa)); break; case FpaMessageType::VERSION: j.update(dynamic_cast(*fpa)); break; + case FpaMessageType::CONFIG: j.update(dynamic_cast(*fpa)); break; + case FpaMessageType::ACK: j.update(dynamic_cast(*fpa)); break; } // clang-format on } } diff --git a/fpsdk_common/src/parser/fpa.cpp b/fpsdk_common/src/parser/fpa.cpp index 0879810..08177c7 100644 --- a/fpsdk_common/src/parser/fpa.cpp +++ b/fpsdk_common/src/parser/fpa.cpp @@ -477,6 +477,56 @@ const char* FpaTimerefStr(const FpaTimeref ref) return "?"; } +// --------------------------------------------------------------------------------------------------------------------- + +const char* FpaConfigActionStr(const FpaConfigAction action) +{ + switch (action) { // clang-format off + case FpaConfigAction::UNSPECIFIED: return "UNSPECIFIED"; + case FpaConfigAction::DEFAULT: return "DEFAULT"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* FpaAckResStr(const FpaAckRes res) +{ + switch (res) { // clang-format off + case FpaAckRes::UNSPECIFIED: return "UNSPECIFIED"; + case FpaAckRes::OK: return "OK"; + case FpaAckRes::FAIL: return "FAIL"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* FpaMessageTypeStr(const FpaMessageType msg_type) +{ + switch (msg_type) { // clang-format off + case FpaMessageType::UNSPECIFIED: return "UNSPECIFIED"; + case FpaMessageType::EOE: return "EOE"; + case FpaMessageType::GNSSANT: return "GNSSANT"; + case FpaMessageType::GNSSCORR: return "GNSSCORR"; + case FpaMessageType::RAWIMU: return "RAWIMU"; + case FpaMessageType::CORRIMU: return "CORRIMU"; + case FpaMessageType::IMUBIAS: return "IMUBIAS"; + case FpaMessageType::LLH: return "LLH"; + case FpaMessageType::ODOMETRY: return "ODOMETRY"; + case FpaMessageType::ODOMENU: return "ODOMENU"; + case FpaMessageType::ODOMSH: return "ODOMSH"; + case FpaMessageType::ODOMSTATUS: return "ODOMSTATUS"; + case FpaMessageType::TEXT: return "TEXT"; + case FpaMessageType::TF: return "TF"; + case FpaMessageType::TP: return "TP"; + case FpaMessageType::VERSION: return "VERSION"; + case FpaMessageType::CONFIG: return "CONFIG"; + case FpaMessageType::ACK: return "ACK"; + } // clang-format on + return "?"; +} + // --------------------------------------------------------------------------------------------------------------------- // Various helpers for the SetFromMsg() FP_A decoding functions @@ -1005,6 +1055,34 @@ static bool GetTimeref(FpaTimeref& ref, const std::string& field) return ok; } +static bool GetAction(FpaConfigAction& act, const std::string& field) +{ + bool ok = true; // clang-format off + if (field == "DEFAULT") { act = FpaConfigAction::DEFAULT; } + else { act = FpaConfigAction::UNSPECIFIED; } // clang-format on + FPA_TRACE("GetAction(\"%s\")=%s act=%d", field.c_str(), string::ToStr(ok), fp::common::EnumToUnderlyingType(act)); + return ok; +} + +static bool GetAckRes(FpaAckRes& res, const std::string& field) +{ + bool ok = true; // clang-format off + if (field == "OK") { res = FpaAckRes::OK; } + else if (field == "FAIL") { res = FpaAckRes::FAIL; } + else { res = FpaAckRes::UNSPECIFIED; } // clang-format on + FPA_TRACE("GetAckRes(\"%s\")=%s res=%d", field.c_str(), string::ToStr(ok), fp::common::EnumToUnderlyingType(res)); + return ok; +} + +static bool GetAckMsg(FpaMessageType& msg, const std::string& field) +{ + bool ok = true; // clang-format off + if (field == "CONFIG") { msg = FpaMessageType::CONFIG; } + else { msg = FpaMessageType::UNSPECIFIED; } // clang-format on + FPA_TRACE("GetAckMsg(\"%s\")=%s msg=%d", field.c_str(), string::ToStr(ok), fp::common::EnumToUnderlyingType(msg)); + return ok; +} + static constexpr int INAN = std::numeric_limits::max(); // Get integer value @@ -1537,6 +1615,46 @@ bool FpaVersionPayload::SetFromMsg(const uint8_t* msg, const std::size_t msg_siz // --------------------------------------------------------------------------------------------------------------------- +bool FpaConfigPayload::SetFromMsg(const uint8_t* msg, const std::size_t msg_size) +{ + // clang-format off + // $FP,CONFIG,1,DEFAULT,,,,,,*4A + // 0 123456 + // clang-format on + bool ok = false; + FpaParts m; + if (GetParts(m, "CONFIG", msg, msg_size) && (m.meta_.msg_version_ == 1) && (m.fields_.size() == 7)) { + ok = GetAction(action, m.fields_[0]) && m.fields_[1].empty() && m.fields_[2].empty() && m.fields_[3].empty() && + m.fields_[4].empty() && m.fields_[5].empty() && m.fields_[6].empty(); + } + FPA_TRACE("FpaConfigPayload %s", string::ToStr(ok)); + valid_ = ok; + msg_type_ = FpaMessageType::CONFIG; + return ok; +} + +// --------------------------------------------------------------------------------------------------------------------- + +bool FpaAckPayload::SetFromMsg(const uint8_t* msg, const std::size_t msg_size) +{ + // clang-format off + // $FP,ACK,1,CONFIG,OK,,,,*60\r\n + // 0 1 2345 + // clang-format on + bool ok = false; + FpaParts m; + if (GetParts(m, "ACK", msg, msg_size) && (m.meta_.msg_version_ == 1) && (m.fields_.size() == 6)) { + ok = GetAckMsg(ack_msg, m.fields_[0]) && GetAckRes(ack_res, m.fields_[1]) && m.fields_[2].empty() && + m.fields_[3].empty() && m.fields_[4].empty() && m.fields_[5].empty(); + } + FPA_TRACE("FpaAckPayload %s", string::ToStr(ok)); + valid_ = ok; + msg_type_ = FpaMessageType::ACK; + return ok; +} + +// --------------------------------------------------------------------------------------------------------------------- + FpaPayloadPtr FpaDecodeMessage(const uint8_t* msg, const std::size_t msg_size) { FpaMessageMeta meta; @@ -1570,6 +1688,8 @@ FpaPayloadPtr FpaDecodeMessage(const uint8_t* msg, const std::size_t msg_size) _GEN("TF", FpaTfPayload) _GEN("TP", FpaTpPayload) _GEN("VERSION", FpaVersionPayload) + _GEN("CONFIG", FpaConfigPayload) + _GEN("ACK", FpaAckPayload) // clang-format on #undef _GEN @@ -1577,7 +1697,8 @@ FpaPayloadPtr FpaDecodeMessage(const uint8_t* msg, const std::size_t msg_size) return nullptr; } -/* ****************************************************************************************************************** */ +/* ****************************************************************************************************************** + */ } // namespace fpa } // namespace parser } // namespace common diff --git a/fpsdk_common/test/parser_fpa_test.cpp b/fpsdk_common/test/parser_fpa_test.cpp index c41acf3..a7106ad 100644 --- a/fpsdk_common/test/parser_fpa_test.cpp +++ b/fpsdk_common/test/parser_fpa_test.cpp @@ -978,7 +978,42 @@ TEST(ParserFpaTest, FpaVersionPayload) // --------------------------------------------------------------------------------------------------------------------- -TEST(ParserNmeaTest, FpaDecodeMessage) +TEST(FpaTest, FpaConfigPayload) +{ + { + FpaConfigPayload payload; + const char* msg = "$FP,CONFIG,1,DEFAULT,,,,,,*4A\r\n"; + EXPECT_TRUE(payload.SetFromMsg((const uint8_t*)msg, std::strlen(msg))); + EXPECT_TRUE(payload.valid_); + EXPECT_EQ(payload.action, FpaConfigAction::DEFAULT); + } +} + +// --------------------------------------------------------------------------------------------------------------------- + +TEST(FpaTest, FpaAckPayload) +{ + { + FpaAckPayload payload; + const char* msg = "$FP,ACK,1,CONFIG,OK,,,,*60\r\n"; + EXPECT_TRUE(payload.SetFromMsg((const uint8_t*)msg, std::strlen(msg))); + EXPECT_TRUE(payload.valid_); + EXPECT_EQ(payload.ack_msg, FpaMessageType::CONFIG); + EXPECT_EQ(payload.ack_res, FpaAckRes::OK); + } + { + FpaAckPayload payload; + const char* msg = "$FP,ACK,1,CONFIG,FAIL,,,,*66\r\n"; + EXPECT_TRUE(payload.SetFromMsg((const uint8_t*)msg, std::strlen(msg))); + EXPECT_TRUE(payload.valid_); + EXPECT_EQ(payload.ack_msg, FpaMessageType::CONFIG); + EXPECT_EQ(payload.ack_res, FpaAckRes::FAIL); + } +} + +// --------------------------------------------------------------------------------------------------------------------- + +TEST(ParserFpaTest, FpaDecodeMessage) { { const char* msg = "$FP,VERSION,1,fp_vrtk2-release-vr2_2.123.0-456,NAV_VR2,v1.3b,fp-5d6f64,VRTK2_STK,*3F\r\n";