From bca673f18339b028833582c6b477f4aa0b80e532 Mon Sep 17 00:00:00 2001
From: "ezekiel.dohmen" <ezekiel.dohmen@ligo.org>
Date: Mon, 22 Apr 2024 16:37:41 -0700
Subject: [PATCH] Cleanup and adding function docs
---
src/epics/util/lib/Statespace.pm | 4 -
src/fe/statespace/stateSpaceCtrl_cdev.c | 8 +-
src/fe/statespace/stateSpacePart.c | 101 ----------
.../statespace/stateSpaceCtrl_cdev.h | 21 ++
.../statespace/stateSpaceCtrl_ioctl.h | 46 +++++
.../part_headers/statespace/stateSpacePart.h | 186 ++++++++++++++++--
6 files changed, 243 insertions(+), 123 deletions(-)
diff --git a/src/epics/util/lib/Statespace.pm b/src/epics/util/lib/Statespace.pm
index fd8c1107c..d51fa3e11 100644
--- a/src/epics/util/lib/Statespace.pm
+++ b/src/epics/util/lib/Statespace.pm
@@ -68,10 +68,6 @@ sub printFrontEndVars {
# Check inputs are connected
sub checkInputConnect {
my ($i) = @_;
-
- #my $ins = $::partInCnt[$::partInNum[$i][0]]; # The number of inputs to the part
- #my $outs = $::partOutputs[$::partOutNum[$i][0]];
-
return "";
}
diff --git a/src/fe/statespace/stateSpaceCtrl_cdev.c b/src/fe/statespace/stateSpaceCtrl_cdev.c
index 75570f7d7..9ba652996 100644
--- a/src/fe/statespace/stateSpaceCtrl_cdev.c
+++ b/src/fe/statespace/stateSpaceCtrl_cdev.c
@@ -102,6 +102,7 @@ static long stateSpaceCtrl_ioctl(struct file *file, unsigned int cmd, unsign
ret_val = -EFAULT;
goto exit_ioctl;
}
+ ret_val = 0;
break;
case STATE_SPACE_GET_PART_NAME:
@@ -134,8 +135,8 @@ static long stateSpaceCtrl_ioctl(struct file *file, unsigned int cmd, unsign
ret_val = -EINVAL;
goto exit_ioctl;
}
+ ret_val = 0;
- return 0;
break;
case STATE_SPACE_CONFIGURE_PART:
@@ -223,8 +224,7 @@ static long stateSpaceCtrl_ioctl(struct file *file, unsigned int cmd, unsign
ret_val = -ETIMEDOUT;
goto exit_ioctl;
}
-
- return 0;
+ ret_val = 0;
break;
@@ -260,6 +260,7 @@ static long stateSpaceCtrl_ioctl(struct file *file, unsigned int cmd, unsign
goto exit_ioctl;
}
+ ret_val = 0;
break;
case STATE_SPACE_GET_PART_DATA:
@@ -320,6 +321,7 @@ static long stateSpaceCtrl_ioctl(struct file *file, unsigned int cmd, unsign
ret_val = -EINVAL;
goto exit_ioctl;
}
+ ret_val = 0;
break;
}
diff --git a/src/fe/statespace/stateSpacePart.c b/src/fe/statespace/stateSpacePart.c
index 7d64a6bb6..3cfd7289a 100644
--- a/src/fe/statespace/stateSpacePart.c
+++ b/src/fe/statespace/stateSpacePart.c
@@ -371,104 +371,3 @@ int getConfigAndDataOfPartStateSpace(unsigned part_index, unsigned data_sz, stru
return 0;
}
-
-//
-// Start Test Code
-//
-
-//TODO: Don't build this into real models
-
-//TODO: Randomize fill order
-int testBufferOverrun(unsigned part_index)
-{
- unsigned i, j, count = 1, errors=0;
-
- StateSpaceCtx_t * ctx = &g_model_config.ss_parts_ptr[part_index].context;
-
-
- //Fill vectors
- for(i=0; i < ctx->state_len; ++i)
- ctx->state_vec[i] = count++;
- for(i=0; i < ctx->state_len; ++i)
- ctx->state_vec_new[i] = count++;
-
- //Fill matrices
- for(i=0; i < ctx->state_len; ++i)
- for(j=0; j < ctx->state_len; ++j)
- ctx->state_matrix[i * ctx->state_len + j] = count++;
-
- for(i=0; i < ctx->state_len; ++i)
- for(j=0; j < ctx->in_len; ++j)
- ctx->input_matrix[i * ctx->in_len + j] = count++;
-
- for(i=0; i < ctx->out_len; ++i)
- for(j=0; j < ctx->state_len; ++j)
- ctx->output_matrix[i * ctx->state_len + j] = count++;
-
- for(i=0; i < ctx->out_len; ++i)
- for(j=0; j < ctx->in_len; ++j)
- ctx->feedthrough_matrix[i * ctx->in_len + j] = count++;
-
- //Reset count for checks
- count = 1;
-
- //Check vectors
- for(i=0; i < ctx->state_len; ++i) {
- if (ctx->state_vec[i] != count++) {
- RTSLOG_ERROR("state_vec[%u] is %lu expected %lu\n", (uint64_t)ctx->state_vec[i], (uint64_t)count);
- errors++;
- }
- }
- for(i=0; i < ctx->state_len; ++i) {
- if (ctx->state_vec_new[i] != count++) {
- RTSLOG_ERROR("state_vec_new[%u] is %lu expected %lu\n", (uint64_t)ctx->state_vec_new[i], (uint64_t)count);
- errors++;
- }
- }
-
- //Check matrices
- for(i=0; i < ctx->state_len; ++i) {
- for(j=0; j < ctx->state_len; ++j) {
- if ( ctx->state_matrix[i * ctx->state_len + j] != count++) {
- RTSLOG_ERROR("state_matrix[%u] is %lu expected %lu\n",
- (uint64_t)ctx->state_matrix[i * ctx->state_len + j], (uint64_t)count);
- errors++;
- }
- }
- }
-
- for(i=0; i < ctx->state_len; ++i) {
- for(j=0; j < ctx->in_len; ++j) {
- if ( ctx->input_matrix[i * ctx->in_len + j] != count++) {
- RTSLOG_ERROR("input_matrix[%u] is %lu expected %lu\n",
- (uint64_t)ctx->input_matrix[i * ctx->in_len + j], (uint64_t)count);
- errors++;
- }
- }
- }
-
- for(i=0; i < ctx->out_len; ++i) {
- for(j=0; j < ctx->state_len; ++j) {
- if ( ctx->output_matrix[i * ctx->state_len + j] != count++) {
- RTSLOG_ERROR("output_matrix[%u] is %lu expected %lu\n",
- (uint64_t)ctx->output_matrix[i * ctx->state_len + j], (uint64_t)count);
- errors++;
- }
- }
- }
-
- for(i=0; i < ctx->out_len; ++i) {
- for(j=0; j < ctx->in_len; ++j) {
- if( ctx->feedthrough_matrix[i * ctx->in_len + j] != count++)
- {
- RTSLOG_ERROR("feedthrough_matrix[%u] is %lu expected %lu\n",
- (uint64_t)ctx->feedthrough_matrix[i * ctx->in_len + j], (uint64_t)count);
- errors++;
- }
- }
- }
-
-
- return errors;
-
-}
\ No newline at end of file
diff --git a/src/include/part_headers/statespace/stateSpaceCtrl_cdev.h b/src/include/part_headers/statespace/stateSpaceCtrl_cdev.h
index a6be9f9e2..1ed082a60 100644
--- a/src/include/part_headers/statespace/stateSpaceCtrl_cdev.h
+++ b/src/include/part_headers/statespace/stateSpaceCtrl_cdev.h
@@ -5,7 +5,28 @@
extern "C" {
#endif
+
+/**
+* @brief Initializes the char device for control over the
+* statespace parts.
+*
+* @param model_name The name of the model, that will be used to
+* to create the /dev/<model_name>
+*
+* @return 0 if the char device was created successfully, -1 if
+* there was an error.
+*/
int initStateSpaceCtrl_cdev( const char * model_name);
+
+
+/**
+* @brief Cleans up the char device, should be
+* called before the model is removed.
+*
+* @param none
+*
+* @return none
+*/
void cleanupStateSpaceCtrl_cdev( void );
#ifdef __cplusplus
diff --git a/src/include/part_headers/statespace/stateSpaceCtrl_ioctl.h b/src/include/part_headers/statespace/stateSpaceCtrl_ioctl.h
index 9078b08e4..ffa791332 100644
--- a/src/include/part_headers/statespace/stateSpaceCtrl_ioctl.h
+++ b/src/include/part_headers/statespace/stateSpaceCtrl_ioctl.h
@@ -48,6 +48,16 @@ struct stateSpace_part_config_t {
//feedthrough_matrix [output_vec_len][input_vec_len]
};
+/**
+* @brief Calculates the total size a stateSpace_part_config_t.data_block
+* would be, with the given in/out/state lengths.
+*
+* @param input_vec_len The number of inputs the part has.
+* @param output_vec_len The number of outputs the part has.
+* @param state_vec_len The number of states the part has.
+*
+* @return The total size (in bytes) of the data_block
+*/
static inline unsigned calcDataBlockSzStateSpace( uint32_t input_vec_len, uint32_t output_vec_len, uint32_t state_vec_len)
{
unsigned num_ele = state_vec_len +
@@ -64,10 +74,46 @@ static inline unsigned calcDataBlockSzStateSpace( uint32_t input_vec_len, uint32
//
#define STATE_SPACE_IOCTL_MAGIC 0xC0
+/**
+* @brief Querys the number of statespace parts the model has.
+*
+* @param A pointer to a stateSpace_info_t structure that will be filled.
+*/
#define STATE_SPACE_GET_NUM_PARTS _IOR(STATE_SPACE_IOCTL_MAGIC, 1, struct stateSpace_info_t)
+
+/**
+* @brief Querys the name of the part identified with the given index.
+*
+* @param A pointer to a stateSpace_get_name_req_t structure that will be filled.
+*/
#define STATE_SPACE_GET_PART_NAME _IOWR(STATE_SPACE_IOCTL_MAGIC, 2, struct stateSpace_get_name_req_t)
+
+/**
+* @brief Requests that the given configuration be applied to the part with the given index.
+*
+* @param A pointer to a stateSpace_part_config_t that contains the requested configuration.
+*/
#define STATE_SPACE_CONFIGURE_PART _IOW(STATE_SPACE_IOCTL_MAGIC, 3, struct stateSpace_part_config_t)
+
+/**
+* @brief Reads out the part with the given index's configuration (ioctl_version through
+* state_vec_len) in the stateSpace_part_config_t struct.
+*
+* stateSpace_part_config_t.part_index must be set to the requested part.
+*
+* @param A pointer to a stateSpace_part_config_t that will be filled.
+*/
#define STATE_SPACE_GET_PART_CONFIG _IOR(STATE_SPACE_IOCTL_MAGIC, 4, struct stateSpace_part_config_t)
+
+/**
+* @brief Reads out the part with the given index's configuration and data
+* (ioctl_version through data_block) in the stateSpace_part_config_t struct.
+*
+* stateSpace_part_config_t.part_index must be set to the requested part.
+*
+* @param A pointer to a stateSpace_part_config_t that will be filled. The size of the struct
+* should be calculated with the calcDataBlockSzStateSpace(...) function.
+*/
#define STATE_SPACE_GET_PART_DATA _IOR(STATE_SPACE_IOCTL_MAGIC, 5, struct stateSpace_part_config_t)
diff --git a/src/include/part_headers/statespace/stateSpacePart.h b/src/include/part_headers/statespace/stateSpacePart.h
index 7c85dfef2..ab8d6198b 100644
--- a/src/include/part_headers/statespace/stateSpacePart.h
+++ b/src/include/part_headers/statespace/stateSpacePart.h
@@ -13,47 +13,203 @@ typedef struct StateSpaceCtx_t
unsigned out_len;
unsigned state_len;
- unsigned buffer_sz;
- //Start of memory block pointers
+ //Stores the size of the data block allocation below, in bytes.
+ unsigned buffer_sz;
+
+ //Start of memory block pointers, all pointers point to different
+ //offsets in the same data block.
double *mem_block_ptr;
double *state_vec; //[state_len]
double *state_vec_new; //[state_len]
- //TODO: Make sure these dim are ordered for the iteration we do over them
+ //TODO: Make sure these dim are ordered
+ //for the iteration we do over them
double *state_matrix; // [state_len][state_len]
double *input_matrix; // [state_len][in_len]
double *output_matrix;// [out_len][state_len]
double *feedthrough_matrix; //Zero or [out_len][in_vec]
} StateSpaceCtx_t;
+//
+// Functions to be called during model setup, before isolation
+//
-//Functions to be called during model setup
+/**
+* @brief Initializes the global data structures for statespace parts
+* in the model.
+*
+* @param num_parts The number of statespace parts the model has.
+*
+* @return 0 if the data structures were initialized successfully,
+* -1 if an error occurred.
+*/
int globStateSpaceInit( unsigned num_parts );
-int globStateSpaceConfigPart( unsigned part_index, const char name [], unsigned num_in, unsigned num_out );
+
+/**
+* @brief Frees all statespace part allocations.
+*
+* This should be called after the RT logic has been stopped,
+* and before model removal, it will free all parts and temp
+* allocations.
+*/
void globStateSpaceFree( void );
+//
+// Functions called in RT Code
+//
+
+/**
+* @brief Initializes the specific part (by index),
+* and gives it the passed name and num ins/outs.
+*
+* AUTO-GENERATED call: This function is generated once for each
+* statespace part in the model, and is inserted into the feCode() init call.
+*
+* @param part_index The index of the statespace part to configure.
+* @param name The name the part will have. (Used to configure by name)
+* @param num_in The numer of inputs the part has.
+* @param num_out The number of outputs the part has.
+*
+* @return 0 if the configuration was set successfully,
+* -1 if an error occurred.
+*/
+int globStateSpaceConfigPart( unsigned part_index, const char name [], unsigned num_in, unsigned num_out );
+
-//Functions called in feCode()
+/**
+* @brief Calculates the next timestep of data with the given inputs,
+* internal state, and producing outputs.
+*
+* AUTO-GENERATED call:
+*
+* @param part_index The index of the part to process.
+*
+* @param reset If >0 the state vector is cleared (0ed out)
+* before the cycle's calculation.
+*
+* @param input_vec A pointer to a double buffer where inputs will be read from.
+* The buffer length must match the num_in parameter passed
+* to globStateSpaceConfigPart()
+*
+* @param out_vec A pointer to a double buffer where outputs will be written to.
+* The buffer length must match the num_out parameter passed
+* to globStateSpaceConfigPart()
+*
+* @return none
+*/
+void calcStateSpace(unsigned part_index, int reset, double * input_vec, double * out_vec);
+
+/**
+* @brief Sets the identified part to the given state vector.
+*
+* @param part_index The index of the statespace part to reset.
+* @param state_vec A pointer to double data to load on reset.
+*
+* @return none
+*/
+void setStateVecStateSpace(unsigned part_index, double * state_vec);
+
+//
+// IOCTL Servicing Functions
+//
+
+/**
+* @brief Gets the name of the part with the given index.
+*
+* @param none
+*
+* @return The number of statespace parts in the model.
+*/
+unsigned getNumPartsStateSpace( void );
+
+/**
+* @brief Gets the current configuration of the part with the given index.
+*
+* @param part_index The index of the part that should be (re)configured.
+* @param input_vec_len The number of inputs (fixed).
+* @param output_vec_len The number of outputs (fixed).
+* @param state_vec_len The number of states
+* @param initial_state_vec A ptr to a buffer of doubles (state_vec_len long)
+* that will be used as the initial state for the part.
+* @param state_matrix A ptr to a buffer of doubles for the flattened (row-major)
+* state matrix. Length must be state_vec_len * state_vec_len.
+* @param input_matrix A ptr to a buffer of doubles for the flattened (row-major)
+* input matrix. Length must be input_vec_len * state_vec_len.
+* @param output_matrix A ptr to a buffer of doubles for the flattened (row-major)
+* output matrix. Length must be output_vec_len * state_vec_len.
+* @param feedthrough_matrix A ptr to a buffer of doubles for the flattened (row-major)
+* feedthrough matrix. Length is input_vec_len * output_vec_len
+* or NULL if the feedthrough is 0 for this configuration.
+*
+* @return 0 if the new configuration was loaded, -1 if the lengths were wrong etc.
+*/
int matrixConfigureStateSpace(unsigned part_index,
unsigned input_vec_len, unsigned output_vec_len, unsigned state_vec_len,
const double * initial_state_vec,
const double * state_matrix, const double * input_matrix,
const double * output_matrix, const double * feedthrough_matrix);
-void calcStateSpace(unsigned part_index, int reset, double * input_vec, double * out_vec);
-void setStateVecStateSpace(unsigned part_index, double * state_vec);
-//IOCTL Servicing Functions
-unsigned getNumPartsStateSpace( void );
+
+/**
+* @brief Gets the name of the part with the given index.
+*
+* @param part_index The index of the part we want the name of.
+* @param name_ptr A pointer to the name of the part. This is for read only.
+*
+* @return none
+*/
void getPartNamePtrStateSpace( unsigned part_index, char ** name_ptr );
-void getConfigOfPartStateSpace(unsigned part_index, struct stateSpace_part_config_t * );
+
+/**
+* @brief Gets the current configuration of the part with the given index.
+*
+* @param part_index The index of the part that should be loaded.
+* @param config A pointer to a buffer to copy the configuration.
+*
+* @return none
+*/
+void getConfigOfPartStateSpace(unsigned part_index, struct stateSpace_part_config_t * config );
+
+/**
+* @brief Copies the part config and data block into the buffer pointed to
+* by fill_me.
+*
+* @param part_index The index of the statespace part to read.
+* @param data_sz The size of the buffer pointed to by fill_me.
+* @param fill_me A pointer to a buffer where the part config should be copied.
+*
+* @return -1 if data for part is not available
+*/
+int getConfigAndDataOfPartStateSpace(unsigned part_index, unsigned data_sz, struct stateSpace_part_config_t * fill_me);
+
+/**
+* @brief Loads the new configuration (now).
+*
+* This is used by librts to load swap the pointers and load
+* the part config immediately.
+*
+* @param part_index The index of the part that should be loaded.
+*
+* @return none
+*/
void loadNewMatrixNowStateSpace( unsigned part_index );
+
+/**
+* @brief Checks if a new configuration is queued and waiting for the RT
+* code to load.
+*
+* Once a new configuration is loaded by matrixConfigureStateSpace(),
+* the new data is stored and the RT code is responsible to sawp out
+* the pointers before its next cycle. This is used to signal, when
+* that swap is complete.
+*
+* @param none
+*
+* @return -1 if nothing is queued, the part index we are waiting on otherwise.
+*/
int check_pending_config( void );
-// @return -1 if data for part is not available
-int getConfigAndDataOfPartStateSpace(unsigned part_index, unsigned data_sz, struct stateSpace_part_config_t * fill_me);
-//Test Methods
-int testBufferOverrun(unsigned part_index);
#ifdef __cplusplus
}
--
GitLab