Skip to content

Commit

Permalink
Implement the FPManager state machine
Browse files Browse the repository at this point in the history
  • Loading branch information
watney committed Apr 8, 2024
1 parent 4982e36 commit e3d61eb
Show file tree
Hide file tree
Showing 24 changed files with 357 additions and 112 deletions.
3 changes: 3 additions & 0 deletions .gitmodules
View file
Original file line number Diff line number Diff line change
@@ -1,3 +1,6 @@
[submodule "googletest"]
path = googletest
url = https://github.com/google/googletest.git
[submodule "STARS"]
path = STARS
url = https://github.com/JPLOpenSource/STARS.git
1 change: 1 addition & 0 deletions STARS
Submodule STARS added at 21bafd
4 changes: 4 additions & 0 deletions Svc/FPManager/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,8 +4,11 @@
# Note: using PROJECT_NAME as EXECUTABLE_NAME
set(SOURCE_FILES
"${CMAKE_CURRENT_LIST_DIR}/FPManager.fpp"
"${CMAKE_CURRENT_LIST_DIR}/SMEvents.fpp"
"${CMAKE_CURRENT_LIST_DIR}/TlmDict.fppi"
"${CMAKE_CURRENT_LIST_DIR}/FPManagerImpl.cpp"
"${CMAKE_CURRENT_LIST_DIR}/FPManagerSm.cpp"
"${CMAKE_CURRENT_LIST_DIR}/FPManagerSm.cpp"
"${CMAKE_CURRENT_LIST_DIR}/Response.cpp"
"${CMAKE_CURRENT_LIST_DIR}/CommandSerializer.cpp"
"${CMAKE_CURRENT_LIST_DIR}/FPResponseTable.cpp"
Expand All @@ -20,6 +23,7 @@ register_fprime_module()
#
set(UT_SOURCE_FILES
"${CMAKE_CURRENT_LIST_DIR}/FPManager.fpp"
"${CMAKE_CURRENT_LIST_DIR}/SMEvents.fpp"
"${CMAKE_CURRENT_LIST_DIR}/test/ut/Tester.cpp"
"${CMAKE_CURRENT_LIST_DIR}/test/ut/TestMain.cpp"
"${CMAKE_CURRENT_LIST_DIR}/test/ut/ResponseExecutionRules.cpp"
Expand Down
4 changes: 4 additions & 0 deletions Svc/FPManager/CMakeLists.txt.tmpl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,8 +8,11 @@
####
set(SOURCE_FILES
"\${CMAKE_CURRENT_LIST_DIR}/FPManager.fpp"
"\${CMAKE_CURRENT_LIST_DIR}/SMEvents.fpp"
"\${CMAKE_CURRENT_LIST_DIR}/TlmDict.fppi"
"\${CMAKE_CURRENT_LIST_DIR}/FPManagerImpl.cpp"
"\${CMAKE_CURRENT_LIST_DIR}/FPManagerSm.cpp"
"\${CMAKE_CURRENT_LIST_DIR}/FPManagerSm.cpp"
"\${CMAKE_CURRENT_LIST_DIR}/Response.cpp"
"\${CMAKE_CURRENT_LIST_DIR}/CommandSerializer.cpp"
"\${CMAKE_CURRENT_LIST_DIR}/FPResponseTable.cpp"
Expand All @@ -24,6 +27,7 @@ register_fprime_module()
# ### UTs ###
set(UT_SOURCE_FILES
"\${CMAKE_CURRENT_LIST_DIR}/FPManager.fpp"
"\${CMAKE_CURRENT_LIST_DIR}/SMEvents.fpp"
"\${CMAKE_CURRENT_LIST_DIR}/test/ut/Tester.cpp"
"\${CMAKE_CURRENT_LIST_DIR}/test/ut/TestMain.cpp"
"\${CMAKE_CURRENT_LIST_DIR}/test/ut/ResponseExecutionRules.cpp"
Expand Down
2 changes: 1 addition & 1 deletion Svc/FPManager/CommandOpcodes.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,9 +27,9 @@ namespace Svc {
namespace OPCODES {

enum Opcodes {
CMD_TEST_CMD_1 = 0x502,
CMD_CLEAR_TRACKING = 0x503,
CMD_NO_OP = 0x500,
CMD_TEST_CMD_1 = 0x502,
};

}
Expand Down
36 changes: 18 additions & 18 deletions Svc/FPManager/CommandSerializer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,10 @@ namespace Svc {


Fw::SerializeStatus
CMD_CLEAR_TRACKING(
CMD_TEST_CMD_1(
I32 arg1,
F32 arg2,
U8 arg3,
Fw::ComBuffer& comBuffer
)
{
Expand All @@ -41,16 +44,25 @@ namespace Svc {
status = comBuffer.serialize(descriptor);
// Serialize the opcode
if (status == Fw::FW_SERIALIZE_OK) {
FwOpcodeType opcode = OPCODES::CMD_CLEAR_TRACKING;
FwOpcodeType opcode = OPCODES::CMD_TEST_CMD_1;
status = comBuffer.serialize(opcode);
}
// Serialize arguments
if (status == Fw::FW_SERIALIZE_OK) {
status = comBuffer.serialize(arg1);
}
if (status == Fw::FW_SERIALIZE_OK) {
status = comBuffer.serialize(arg2);
}
if (status == Fw::FW_SERIALIZE_OK) {
status = comBuffer.serialize(arg3);
}
return status;
}


Fw::SerializeStatus
CMD_NO_OP(
CMD_CLEAR_TRACKING(
Fw::ComBuffer& comBuffer
)
{
Expand All @@ -62,7 +74,7 @@ namespace Svc {
status = comBuffer.serialize(descriptor);
// Serialize the opcode
if (status == Fw::FW_SERIALIZE_OK) {
FwOpcodeType opcode = OPCODES::CMD_NO_OP;
FwOpcodeType opcode = OPCODES::CMD_CLEAR_TRACKING;
status = comBuffer.serialize(opcode);
}
// Serialize arguments
Expand All @@ -71,10 +83,7 @@ namespace Svc {


Fw::SerializeStatus
CMD_TEST_CMD_1(
I32 arg1,
F32 arg2,
U8 arg3,
CMD_NO_OP(
Fw::ComBuffer& comBuffer
)
{
Expand All @@ -86,19 +95,10 @@ namespace Svc {
status = comBuffer.serialize(descriptor);
// Serialize the opcode
if (status == Fw::FW_SERIALIZE_OK) {
FwOpcodeType opcode = OPCODES::CMD_TEST_CMD_1;
FwOpcodeType opcode = OPCODES::CMD_NO_OP;
status = comBuffer.serialize(opcode);
}
// Serialize arguments
if (status == Fw::FW_SERIALIZE_OK) {
status = comBuffer.serialize(arg1);
}
if (status == Fw::FW_SERIALIZE_OK) {
status = comBuffer.serialize(arg2);
}
if (status == Fw::FW_SERIALIZE_OK) {
status = comBuffer.serialize(arg3);
}
return status;
}

Expand Down
18 changes: 9 additions & 9 deletions Svc/FPManager/CommandSerializer.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,26 +33,26 @@ namespace Svc {
namespace CommandSerializer {


//! Serialize a CMD_CLEAR_TRACKING command
//! Serialize a CMD_TEST_CMD_1 command
//! return The serialization status
Fw::SerializeStatus CMD_CLEAR_TRACKING (
Fw::SerializeStatus CMD_TEST_CMD_1 (
I32 arg1,
F32 arg2,
U8 arg3,
Fw::ComBuffer& comBuffer //!< The result
);


//! Serialize a CMD_NO_OP command
//! Serialize a CMD_CLEAR_TRACKING command
//! return The serialization status
Fw::SerializeStatus CMD_NO_OP (
Fw::SerializeStatus CMD_CLEAR_TRACKING (
Fw::ComBuffer& comBuffer //!< The result
);


//! Serialize a CMD_TEST_CMD_1 command
//! Serialize a CMD_NO_OP command
//! return The serialization status
Fw::SerializeStatus CMD_TEST_CMD_1 (
I32 arg1,
F32 arg2,
U8 arg3,
Fw::SerializeStatus CMD_NO_OP (
Fw::ComBuffer& comBuffer //!< The result
);

Expand Down
3 changes: 3 additions & 0 deletions Svc/FPManager/FPManager.fpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,9 @@ module Svc {

telemetry port Tlm

@ internal port for handling state-machine Events
internal port sendEvents(ev: Svc.SMEvents)


# ----------------------------------------------------------------------
# Commands
Expand Down
132 changes: 72 additions & 60 deletions Svc/FPManager/FPManagerImpl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@ Os::BufferQueue FPManagerImpl::respPendingQue;

FPManagerImpl::FPManagerImpl(const char* compName) :
FPManagerComponentBase(compName),
fpState(IDLE),
fpManagerSm(this),
saveDroppedMessages(0),
responsePackedState(0),
autoThrottleEvrClrCtr(0),
Expand Down Expand Up @@ -72,6 +72,9 @@ void FPManagerImpl::init(NATIVE_INT_TYPE queueDepth, NATIVE_INT_TYPE instance) {
// Send out the response enable/disable state telemetry
packRespStateTelemetry();

// Initialize the state machine
fpManagerSm.init();


}

Expand Down Expand Up @@ -151,66 +154,8 @@ void FPManagerImpl::Run_handler(
log_WARNING_HI_FP_RESPONSES_DROPPED(droppedMessages);
}

if ( this->fpState == IDLE) {
U8 response;
// Pop from the pending response queue
NATIVE_UINT_TYPE size = sizeof(response);
NATIVE_INT_TYPE priority;


// If the response pending queue is not empty then pop the queue and start a response
if (!this->respPendingQue.isEmpty()) {
bool qRes = respPendingQue.pop(reinterpret_cast<U8*>(&response), size, priority);
FW_ASSERT(qRes);
} else {
return;
}

FW_ASSERT( (response <= FPResponses::NUMBER_RESPONSES) && (response > 0), response);

// Set the current response
this->currentResponse = this->responseTable[response-1].response;

// Start the response
this->currentResponse->start();
log_WARNING_HI_FP_RESPONSE_STARTED(response);


this->fpState = RUNNING;

} else {
// fpState is RUNNING
// Pull the next command from the currently executing response command list
// Dispatch the command to the output comCmdOut port
// If there are no more commands to be dispatched for this response then update the last response
// value, update the response counter and generate an EVR indicating this response has completed
// execution, then return back command response OK via the CmdStatus output port. Set the fpState
// to Idle.
sendEvent(FPManagerSm::RTI_SIG);

Fw::ComBuffer comBuffer;

FW_ASSERT(this->currentResponse != 0);

if ( this->currentResponse->run(comBuffer) ) {
comCmdOut_out(0, comBuffer, 0);
} else {
// Response has completed
U32 id = this->currentResponse->getId();
log_WARNING_HI_FP_RESPONSE_COMPLETED(id);

// Push out telemetry completion
FW_ASSERT( (id <= FPResponses::NUMBER_RESPONSES) && (id > 0), id);
FW_ASSERT(tlmResponseComplete[id-1] != NULL);
(this->*(tlmResponseComplete[id-1]))(this->currentResponse->getNumberCompletes(), Fw::Time());

tlmWrite_FP_LastResponseComplete(id);


this->fpState = IDLE;
this->currentResponse = 0;

}
}
}

void FPManagerImpl::cmdResponseIn_handler(
Expand Down Expand Up @@ -380,4 +325,71 @@ void FPManagerImpl::packRespStateTelemetry(void) {

}

void FPManagerImpl::sendEvent(U32 eventSignal) {
Svc::SMEvents event;
event.seteventSignal(eventSignal);
sendEvents_internalInterfaceInvoke(event);
}

void FPManagerImpl::sendEvents_internalInterfaceHandler(const Svc::SMEvents& ev)
{
this->fpManagerSm.update(&ev);
}



// State machine functions
//
bool FPManagerImpl::FPManagerSm_emptyQueue() {
return (this->respPendingQue.isEmpty());
}

bool FPManagerImpl::FPManagerSm_responseRunning() {
Fw::ComBuffer comBuffer;
if (this->currentResponse->run(comBuffer))
{
comCmdOut_out(0, comBuffer, 0);
return true;
} else {
return false;
}
}

void FPManagerImpl::FPManagerSm_startNextResponse() {
U8 response;
NATIVE_UINT_TYPE size = sizeof(response);
NATIVE_INT_TYPE priority;

bool qRes = respPendingQue.pop(reinterpret_cast<U8*>(&response), size, priority);
FW_ASSERT(qRes);

FW_ASSERT( (response <= FPResponses::NUMBER_RESPONSES) && (response > 0), response);

// Set the current response
this->currentResponse = this->responseTable[response-1].response;

// Start the response
this->currentResponse->start();
log_WARNING_HI_FP_RESPONSE_STARTED(response);

}


void FPManagerImpl::FPManagerSm_reportResponseComplete() {
U32 id = this->currentResponse->getId();
log_WARNING_HI_FP_RESPONSE_COMPLETED(id);

// Push out telemetry completion
FW_ASSERT( (id <= FPResponses::NUMBER_RESPONSES) && (id > 0), id);
FW_ASSERT(tlmResponseComplete[id-1] != NULL);
(this->*(tlmResponseComplete[id-1]))(this->currentResponse->getNumberCompletes(), Fw::Time());

tlmWrite_FP_LastResponseComplete(id);

this->currentResponse = 0;


}


}
Loading

0 comments on commit e3d61eb

Please sign in to comment.