Skip to content

Add FP_A-CONFIG and FP_A-ACK #214

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
Aug 6, 2026
Merged

Add FP_A-CONFIG and FP_A-ACK #214

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
75 changes: 75 additions & 0 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 @@ -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
*/
Expand Down Expand Up @@ -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
*/
Expand Down Expand Up @@ -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
Expand Down
21 changes: 21 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 @@ -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();
Expand All @@ -444,6 +463,8 @@ inline void to_json(nlohmann::json& j, const FpaPayloadPtr& fpa)
case FpaMessageType::TF: j.update(dynamic_cast<const FpaTfPayload&>(*fpa)); break;
case FpaMessageType::TP: j.update(dynamic_cast<const FpaTpPayload&>(*fpa)); break;
case FpaMessageType::VERSION: j.update(dynamic_cast<const FpaVersionPayload&>(*fpa)); break;
case FpaMessageType::CONFIG: j.update(dynamic_cast<const FpaConfigPayload&>(*fpa)); break;
case FpaMessageType::ACK: j.update(dynamic_cast<const FpaAckPayload&>(*fpa)); break;
} // clang-format on
}
}
Expand Down
123 changes: 122 additions & 1 deletion fpsdk_common/src/parser/fpa.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -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<int>::max();

// Get integer value
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -1570,14 +1688,17 @@ 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

return nullptr;
}

/* ****************************************************************************************************************** */
/* ******************************************************************************************************************
*/
} // namespace fpa
} // namespace parser
} // namespace common
Expand Down
37 changes: 36 additions & 1 deletion fpsdk_common/test/parser_fpa_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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";
Expand Down
Loading