static void mdlSetWorkWidths(SimStruct *S)
{
int_T nq = 0;
if( mxGetNumberOfElements(paramQuaternionIndex) == 2 )
{
nq = intval(mxGetPr(paramQuaternionIndex)[1]) - intval(mxGetPr(paramQuaternionIndex)[0]) + 1;
nq = nq / 4;
}
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);
ssSetNumRWork(S, 0);
ssSetNumIWork(S, 1);
ssSetNumPWork(S, 1);
ssSetNumDWork(S, (nq > 0 ? 3 : 2));
ssSetDWorkWidth(S, 0, ssGetInputPortWidth(S, 0));
ssSetDWorkWidth(S, 1, ssGetInputPortWidth(S, 0));
ssSetDWorkDataType(S, 0, SS_DOUBLE);
ssSetDWorkDataType(S, 1, SS_DOUBLE);
if( nq )
{
ssSetDWorkWidth(S, 2, ssGetInputPortWidth(S, 0));
ssSetDWorkDataType(S, 2, SS_DOUBLE);
}
ssSetNumModes(S, 0);
}
static void mdlSetInputPortDimensionInfo(SimStruct *S,
int_T port,
const DimsInfo_T *dimsInfo)
{
boolean_T isNotVector = ((dimsInfo->numDims == 2 ) &&
(dimsInfo->dims[0] > 1 && dimsInfo->dims[1] > 1)) ;
if(isNotVector){
ssSetErrorStatus(S, "The block only accepts vector signals. "
"It does not accept a [mxn] matrix signal "
"where m > 1 and n > 1.");
}else{
int otherPort = (port == 0) ? 1 : 0;
if(!ssSetInputPortDimensionInfo(S, port, dimsInfo)) return;
/*
* If other port width is unknown, set the other port width.
* Note1: we cannot update other port dimension info.
* Note2: For simplicity, this block cannot accept partial dimension,
* however, it may partially set other port dimension info.
*/
if(ssGetInputPortWidth(S, otherPort) == DYNAMICALLY_SIZED &&
ssGetInputPortWidth(S, port) != DYNAMICALLY_SIZED){
DECL_AND_INIT_DIMSINFO(dimsInfo);
dimsInfo.width = ssGetInputPortWidth (S, port);
dimsInfo.numDims = ssGetInputPortNumDimensions(S, otherPort);
dimsInfo.dims = ssGetInputPortDimensions (S, otherPort);
if(!ssSetInputPortDimensionInfo(S, otherPort, &dimsInfo)) return;
}
}
}
static void mdlSetOutputPortWidth(SimStruct *S, int_T port, int_T outputPortWidth)
{
if( ((ssGetInputPortWidth(S, 0) != DYNAMICALLY_SIZED) && (ssGetInputPortWidth(S, 0) != outputPortWidth)) || \
((ssGetOutputPortWidth(S, 1) != DYNAMICALLY_SIZED) && (ssGetOutputPortWidth(S, 1) != outputPortWidth)) )
{
ssSetErrorStatus(S,"xdot and x must have the same size");
return;
}
if( intval(mxGetScalar(paramInitialConditionSource)) > 1 )
{
if( (ssGetInputPortWidth(S, 1) != DYNAMICALLY_SIZED) && (ssGetInputPortWidth(S, 1) != outputPortWidth) )
{
ssSetErrorStatus(S,"xdot, x0 and x must have the same size");
return;
}
ssSetInputPortWidth(S, 1, outputPortWidth);
}
else
{
if( (mxGetNumberOfElements(paramInitialCondition) != 1) && (mxGetNumberOfElements(paramInitialCondition) != outputPortWidth) )
{
ssSetErrorStatus(S,"xdot, x0 and x must have the same size");
return;
}
}
ssSetInputPortWidth(S, 0, outputPortWidth);
ssSetOutputPortWidth(S, 1, outputPortWidth);
}
/* Function: mdlSetDefaultPortDimensionInfo ===============================
* Abstract:
* This method is called when there is not enough information in your
* model to uniquely determine the port dimensionality of signals
* entering or leaving your block. When this occurs, Simulink's
* dimension propagation engine calls this method to ask you to set
* your S-functions default dimensions for any input and output ports
* that are dynamically sized.
*
* If you do not provide this method and you have dynamically sized ports
* where Simulink does not have enough information to propagate the
* dimensionality to your S-function, then Simulink will set these unknown
* ports to the 'block width' which is determined by examining any known
* ports. If there are no known ports, the width will be set to 1.
*
*/
static void mdlSetDefaultPortDimensionInfo(SimStruct *S)
{
/* Set input port 4 default dimension */
if (ssGetInputPortWidth(S, 3) == DYNAMICALLY_SIZED) {
ssSetInputPortWidth(S, 3, 1);
}
/* Set input port 5 default dimension */
if (ssGetInputPortWidth(S, 4) == DYNAMICALLY_SIZED) {
ssSetInputPortWidth(S, 4, 1);
}
}
static void mdlOutputs(SimStruct *S,int_T tid) {
InputRealPtrsType uPtrs0 = ssGetInputPortRealSignalPtrs(S,0);
InputRealPtrsType uPtrs1 = ssGetInputPortRealSignalPtrs(S,1);
real_T prev = ssGetRWorkValue(S,0);
bool dataPort = PARAM(2)[0];
int_T i;
#ifndef MATLAB_MEX_FILE
rosShmData_t *shm = (rosShmData_t *)ssGetPWorkValue(S,0);
SEM *sem = (SEM *)ssGetPWorkValue(S,1);
#endif
char_T *msg;
unsigned int strlen = sizeof(char_T)*(PARAM_SIZE(1)+1);
UNUSED_ARG(tid); /* not used in single tasking mode */
if (U0(0) > 0.5 && U0(0) > prev) {
msg = (char_T *)malloc(strlen);
mxGetString(ssGetSFcnParam(S,1), msg, strlen);
#ifndef MATLAB_MEX_FILE
if (dataPort) {
for (i = 0; i < ssGetInputPortWidth(S,1); ++i) {
asprintf(&msg, "%s %f", msg, U1(i));
}
}
if (rt_sem_wait_if(sem) != 0) {
memcpy(shm->msg.text, msg, MAX_LOG_MSG_SIZE);
shm->msg.state = NEW_VALUE;
rt_sem_signal(sem);
}
#else
switch ((int)PARAM(0)[0]) {
case 1: printf("DEBUG"); break;
case 2: printf("INFO"); break;
case 3: printf("WARN"); break;
case 4: printf("ERROR"); break;
case 5: printf("FATAL"); break;
default: printf("NONE"); break;
}
printf(": %s", msg);
if (dataPort) {
for (i = 0; i < ssGetInputPortWidth(S,1); ++i) {
printf(" %f", U1(i));
}
}
printf("\n");
#endif
free(msg);
}
ssSetRWorkValue(S,0,U0(0));
}
/* Function: mdlOutputs =======================================================
* Abstract:
*
* y0 = u * 0.1
* y1 = u * 0.2
* y2 = u * 0.3
* y3 = u * 0.4
* y4 = u * 0.5
* y5 = u * 0.6
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
boolean_T u0IsComplex = ssGetInputPortComplexSignal(S, 0) == COMPLEX_YES;
int_T iWidth = ssGetInputPortWidth(S, 0);
int_T i, j;
real_T gain = 0.0;
for (j=0; j<6; j++) {
real_T *y = ssGetOutputPortRealSignal(S,j);
/* Output of jth Port is gain [(j+1)*(0.1)] */
if (j != 2) {
gain += 0.1;
} else {
/* special case due to normalize numerical precision */
gain = 0.3;
}
for (i=0; i<iWidth; i++) {
*y++ = uPtrs[i][0] * gain;
if (u0IsComplex) {
*y++ = uPtrs[i][1] * gain;
}
}
}
}
static void mdlSetInputPortDataType(SimStruct *S, int_T port, DTypeId dataType) {
int portWidth;
if ( port == 0 ) {
portWidth = ssGetInputPortWidth(S,0);
if( isAcceptableDataType( S, dataType,portWidth ) ) {
/*
* Accept proposed data type if it is an unsigned integer type
* force all data ports to use this data type.
*/
ssSetInputPortDataType( S, 0, dataType );
} else {
/* Reject proposed data type */
ssSetErrorStatus(S,"Invalid input signal width, data type or input port is not connected. See mask help for valid data types.");
goto EXIT_POINT;
}
} else {
/*
* Should not end up here. Simulink will only call this function
* for existing input ports whose data types are unknown.
*/
ssSetErrorStatus(S, "Error setting input port data type.");
goto EXIT_POINT;
}
EXIT_POINT:
return;
}
/* Function: mdlDerivatives =================================================
* Abstract:
* xdot = Ax + Bu
*/
static void mdlDerivatives(SimStruct *S)
{
real_T *dx = ssGetdX(S);
real_T *x = ssGetContStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
const real_T *apr = mxGetPr(A_PARAM(S));
const real_T *bpr = mxGetPr(B_PARAM(S));
int_T nStates = ssGetNumContStates(S);
int_T nInputs = ssGetInputPortWidth(S,0);
int_T i, j;
real_T accum;
/* Matrix Multiply: dx = Ax + Bu */
for (i = 0; i < nStates; i++) {
accum = 0.0;
/* Ax */
for (j = 0; j < nStates; j++) {
accum += apr[i + nStates*j] * x[j];
}
/* Bu */
for (j = 0; j < nInputs; j++) {
accum += bpr[i + nStates*j] * U(j);
}
dx[i] = accum;
}
}
/* Function: mdlOutputs =======================================================
* Abstract:
* y = Cx + Du
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
real_T *y = ssGetOutputPortRealSignal(S,0);
real_T *x = ssGetContStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
const real_T *cpr = mxGetPr(C_PARAM(S));
const real_T *dpr = mxGetPr(D_PARAM(S));
int_T nStates = ssGetNumContStates(S);
int_T nInputs = ssGetInputPortWidth(S,0);
int_T nOutputs = ssGetOutputPortWidth(S,0);
int_T i, j;
real_T accum;
UNUSED_ARG(tid); /* not used in single tasking mode */
/* Matrix Multiply: y = Cx + Du */
for (i = 0; i < nOutputs; i++) {
accum = 0.0;
/* Cx */
for (j = 0; j < nStates; j++) {
accum += cpr[i + nOutputs*j] * x[j];
}
/* Du */
for (j = 0; j < nInputs; j++) {
accum += dpr[i + nOutputs*j] * U(j);
}
y[i] = accum;
}
}
static void mdlSetWorkWidths(SimStruct *S)
{
/* Set the width of DWork(s) used for marshalling the IOs */
if (isDWorkPresent) {
/* Update dwork 0 */
ssSetDWorkWidth(S, 0, ssGetInputPortWidth(S, 0));
/* Update dwork 1 */
ssSetDWorkWidth(S, 1, ssGetInputPortWidth(S, 1));
/* Update dwork 2 */
ssSetDWorkWidth(S, 2, ssGetOutputPortWidth(S, 0));
}
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
short i;
int_T portWidth;
InputRealPtrsType uPtrs;
//Read the inputs
for(i=0;i<numberOfInputs;i++)
{
portWidth = ssGetInputPortWidth(S,i);
uPtrs = ssGetInputPortRealSignalPtrs(S,i);
if (portWidth>0)
{
if (uPtrs[0]!=NULL)
{
privateSetInputValByIndex(i,*uPtrs[0]);
}
}
}
//Update the outputs
for(i=0;i<numberOfOutputs;i++)
{
double *y = (double *)ssGetOutputPortSignal(S,i);
y[0] = privateGetOutputValByIndex(i);
}
}
static void mdlRTW(SimStruct *S){
int_T type = P_TYPE;
int_T id = P_ID;
DTypeId dataType = ssGetInputPortDataType(S,0);
int_T length =ssGetDataTypeSize(S,dataType) * ssGetInputPortWidth(S,0);
int32_T endian = P_ENDIAN;
/* -- Write Invariant Parameter Settings -- */
ssWriteRTWParamSettings(S,4,
SSWRITE_VALUE_DTYPE_NUM, "TYPE",
&type,
DTINFO(SS_UINT32,0),
SSWRITE_VALUE_DTYPE_NUM, "ID",
&id,
DTINFO(SS_UINT32,0),
SSWRITE_VALUE_DTYPE_NUM, "LENGTH",
&length,
DTINFO(SS_UINT16,0),
SSWRITE_VALUE_DTYPE_NUM, "Endian",
&endian,
DTINFO(SS_UINT32,0)
);
/* -- Write Invariant Signal Settings -- */
if ( !mxIsFinite( ssGetSampleTime( S, 0 ) ) ) {
CAN_FRAME * frame = (CAN_FRAME *) ssGetOutputPortSignal(S,0);
CAN_write_rtw_frame(S,frame);
}
}
/* Function: mdlInitializeSizes ===============================================
* Abstract:
* The sizes information is used by Simulink to determine the S-function
* block's characteristics (number of inputs, outputs, states, etc.).
*/
static void mdlInitializeSizes(SimStruct *S)
{
/* See sfuntmpl.doc for more details on the macros below */
ssSetNumSFcnParams(S, 0); /* Number of expected parameters */
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
/* Return if number of expected != number of actual parameters */
return;
}
ssSetNumContStates(S, 1); /* how many continuous states? */
ssSetNumDiscStates(S, 0);
if (!ssSetNumInputPorts(S, 1)) return;
ssSetInputPortWidth(S, 0, 1);
/*
* Set direct feedthrough flag (1=yes, 0=no).
* A port has direct feedthrough if the input is used in either
* the mdlOutputs or mdlGetTimeOfNextVarHit functions.
* See matlabroot/simulink/src/sfuntmpl_directfeed.txt.
*/
ssSetInputPortDirectFeedThrough(S, 0, 1);
if (!ssSetNumOutputPorts(S, 1)) return;
ssSetOutputPortWidth(S, 0, 1);
ssSetNumSampleTimes(S, 1);
/*
* If your Fortran code uses REAL for the state, input, and/or output
* datatypes, use these DWorks as work areas to downcast continuous
* states from double to REAL before calling your code. You could
* also put the work vectors in hard-coded local (stack) variables.
*
* For fixed step code, keep a copy of the variables to be output
* in a DWork vector so the mdlOutputs() function can provide output
* data when needed. You can use as many DWork vectors as you like
* for both input and output (or hard-code local variables).
*/
if(!ssSetNumDWork( S, 3)) return;
ssSetDWorkWidth( S, 0, ssGetOutputPortWidth(S,0));
ssSetDWorkDataType( S, 0, SS_SINGLE); /* use SS_DOUBLE if needed */
ssSetDWorkWidth( S, 1, ssGetInputPortWidth(S,0));
ssSetDWorkDataType( S, 1, SS_SINGLE);
ssSetDWorkWidth( S, 2, ssGetNumContStates(S));
ssSetDWorkDataType( S, 2, SS_SINGLE);
ssSetNumNonsampledZCs(S, 0);
/* Specify the sim state compliance to be same as a built-in block */
/* see sfun_simstate.c for example of other possible settings */
ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE);
ssSetOptions(S, 0);
}
/* Function: mdlOutputs =======================================================
*
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
const real_T *u0 = (const real_T*) ssGetInputPortSignal(S,0);
uint32_T *y0 = (uint32_T *)ssGetOutputPortRealSignal(S,0);
const int_T y_width = ssGetOutputPortWidth(S,0);
const int_T u_width = ssGetInputPortWidth(S,0);
fptofix_Outputs_wrapper(u0, y0, y_width, u_width, S);
}
/* Function: mdlOutputs =======================================================
*
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
const int8_T *u0 = (const int8_T*) ssGetInputPortSignal(S,0);
int8_T *y0 = (int8_T *)ssGetOutputPortRealSignal(S,0);
const int_T y_width = ssGetOutputPortWidth(S,0);
const int_T u_width = ssGetInputPortWidth(S,0);
jpeg2bmp_Outputs_wrapper(u0, y0, y_width, u_width);
}
static void mdlSetDefaultPortDataTypes(SimStruct *S) {
if ( ssGetInputPortDataType(S,0) == DYNAMICALLY_TYPED ){
if ( isAcceptableDataType(S,SS_UINT8,ssGetInputPortWidth(S,0))){
ssSetInputPortDataType( S, 0, SS_UINT8 );
}else{
ssSetErrorStatus(S,"Invalid input signal width, data type or input port is not connected. See mask help for valid data types.");
}
}
}
/* Function: mdlOutputs =======================================================
*
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
const uint32_T *u0 = (const uint32_T*) ssGetInputPortSignal(S,0);
real_T *y0 = (real_T *)ssGetOutputPortRealSignal(S,0);
const int_T p_width0 = mxGetNumberOfElements(PARAM_DEF0(S));
const real_T *scale = mxGetData(PARAM_DEF0(S));
const int_T y_width = ssGetOutputPortWidth(S,0);
const int_T u_width = ssGetInputPortWidth(S,0);
fixtofpwithscale_Outputs_wrapper(u0, y0, scale, p_width0, y_width, u_width, S);
}
/* Function: mdlOutputs =======================================================
* Abstract:
* In this function, you compute the outputs of your S-function
* block. Generally outputs are placed in the output vector, ssGetY(S).
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
CAN_FRAME * frame;
const void * uFrame = ssGetInputPortSignal(S,0);
int length,idx,signals,portWidth,dataTypeSize,offset ;
InputInt32PtrsType uID;
DTypeId dataType = ssGetInputPortDataType(S,0);
length = ssGetDataTypeSize(S,dataType) * ssGetInputPortWidth(S,0);
frame = (CAN_FRAME *) ssGetOutputPortSignal(S,0);
frame->LENGTH = length;
memset(frame->DATA,0,8);
if (P_ID == -1 ){
uID = (InputInt32PtrsType) ssGetInputPortSignalPtrs(S,1);
frame->ID = *uID[0];
}else{
frame->ID = P_ID;
}
if (P_ENDIAN == CPU_ENDIAN){
/* Target is same endianess as source */
memcpy(frame->DATA,uFrame,length);
}else{
/* Target is alternate endianess as source.
* Reverse the bytes in each signal but
* preserve the ordering of the signals. */
dataTypeSize = ssGetDataTypeSize(S,ssGetInputPortDataType(S,0));
portWidth = ssGetInputPortWidth(S,0);
for (signals = 0 ; signals < portWidth ; signals++){
offset = signals * dataTypeSize;
for ( idx=0;idx < dataTypeSize;idx++ ){
((uint8_T *)(frame->DATA))[offset+idx]=(( uint8_T *)uFrame)[offset+dataTypeSize-idx-1];
}
}
}
}
static void mdlSetDefaultPortDimensionInfo(SimStruct *S)
{
int_T size_sig;
if( (intval(mxGetScalar(paramInitialConditionSource)) == 1) && (mxGetNumberOfElements(paramInitialCondition) > 1) )
{
size_sig = mxGetNumberOfElements(paramInitialCondition);
if( ssGetInputPortWidth(S, 0) == DYNAMICALLY_SIZED )
{
ssSetInputPortWidth(S, 0, size_sig);
}
if( ssGetOutputPortWidth(S, 1) == DYNAMICALLY_SIZED )
{
ssSetOutputPortWidth(S, 1, size_sig);
}
if( ssGetInputPortWidth(S, 0) != ssGetOutputPortWidth(S, 1) )
{
ssSetErrorStatus(S,"xdot, x0 and x must have the same size");
return;
}
}
}
void mdlSetDefaultPortDimensionInfo(SimStruct *S)
{
DECL_AND_INIT_DIMSINFO(dimsInfo);
/* Either 2nd input or 2nd output should be already known */
if (ssGetOutputPortWidth(S, 0) != DYNAMICALLY_SIZED) {
/* It is the output that is known, get the dimensions first */
dimsInfo.width = ssGetOutputPortWidth(S, 0);
dimsInfo.numDims = ssGetOutputPortNumDimensions(S, 0);
dimsInfo.dims = ssGetOutputPortDimensions(S, 0);
/*set second input, first output if unknown */
if (ssGetInputPortWidth(S, 0) == DYNAMICALLY_SIZED) {
if(!ssSetInputPortDimensionInfo(S, 0, &dimsInfo)) return;
}
return;
}
else {
if (ssGetInputPortWidth(S, 0) != DYNAMICALLY_SIZED) {
/* It is the input that is known, get the dimensions */
dimsInfo.width = ssGetInputPortWidth(S, 0);
dimsInfo.numDims = ssGetInputPortNumDimensions(S, 0);
dimsInfo.dims = ssGetInputPortDimensions(S, 0);
/*set first and second output if unknown */
if (ssGetOutputPortWidth(S, 0) == DYNAMICALLY_SIZED) {
if(!ssSetOutputPortDimensionInfo(S, 0, &dimsInfo)) return;
}
return;
}
else
{
/* Default everything to a scalar signal */
ssSetOutputPortVectorDimension(S, 0, 1);
ssSetInputPortVectorDimension(S, 0, 1);
return;
}
}
} /* end mdlSetDefaultPortDimensionInfo */
/* Function: mdlSetOutputPortDimensionInfo ===================================
*/
static void mdlSetOutputPortDimensionInfo(SimStruct *S,
int_T portIndex,
const DimsInfo_T *dimsInfo)
{
int inPortWidth = 0;
if(!ssSetOutputPortDimensionInfo(S, portIndex, dimsInfo)) return;
inPortWidth = ssGetInputPortWidth( S, 0 );
if ( inPortWidth == DYNAMICALLY_SIZED )
{
if(!ssSetInputPortDimensionInfo(S, 0, dimsInfo)) return;
}
else if ( inPortWidth != dimsInfo->width )
{
ssSetErrorStatus(S,"Output port width not compatible with input port width.");
}
} /* mdlSetOutputPortDimensionInfo */
/* == FUNCTION mdlOutputs =====================================================
* Triggers a step in the simulation. Called by Simulink whenever a simulation
* step takes place.
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
debug("( trace )");
real_T* outp = ssGetOutputPortRealSignal(S, 0);
size_t width = (size_t) ssGetInputPortWidth(S, 0);
switch(Protocol_get_data(protocol, &delta_time, outp)) {
case PROTOCOL_SUCCESS:
break;
case PROTOCOL_HALT:
sim_running = false;
Protocol_destroy(protocol);
ssSetErrorStatus(S, "Simulator requested death, halting.");
return;
default:
ssSetErrorStatus(S, "PROTOCOL_FAILURE");
return;
}
}
/* Function: mdlDerivatives =================================================
* Abstract:
* xdot = Ax + Bu
*/
static void mdlDerivatives(SimStruct *S)
{
real_T *dx = ssGetdX(S);
real_T *x = ssGetContStates(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
const real_T *apr = mxGetPr(MAGICNUM_PARAM(S));
int_T nStates = ssGetNumContStates(S);
int_T nInputs = ssGetInputPortWidth(S,0);
int_T i, j;
real_T accum;
/* Matrix Multiply: dx = Ax + Bu */
real_T T_0 = x[0];
real_T A = U(0);
real_T x1 = U(1); //muscle length
real_T x2 = U(2); //muscle change of length (vel)
// real_T dT = dx[0];
real_T Kse = apr[0];
real_T Kpe = apr[1];
real_T b = apr[2];
dx[0] = Kse / b * (Kpe * (x1 - 1.0) + b*x2 - (1 + Kpe/Kse)*T_0 + A);
// for (i = 0; i < nStates; i++) {
// accum = 0.0;
//
// /* Ax */
// for (j = 0; j < nStates; j++) {
// accum += apr[i + nStates*j] * x[j];
// }
//
// /* Bu */
// for (j = 0; j < nInputs; j++) {
// accum += bpr[i + nStates*j] * U(j);
// }
//
// dx[i] = accum;
// }
}
void mdlOutputs(SimStruct *S, int_T tid)
/* ======================================================================== */
{
UNUSED_ARG(tid);
SampleBuffer *sbuf = ssGetPWorkValue(S, SBUF);
if (sbuf->error) {
ssPrintf(sample_buffer_error_names[sbuf->error]);
/* not in callback, due to issues with Simulink */
sbuf->error = SB_NO_ERROR;
}
pthread_mutex_lock(&sbuf->mutex);
if(sbuf->ready == NUMBER_OF_BUFFERS) {
hackrf_device* device = ssGetPWorkValue(S, DEVICE);
if (hackrf_is_streaming(device) == HACKRF_TRUE) {
pthread_cond_wait(&sbuf->cond_var, &sbuf->mutex);
} else {
ssSetErrorStatus(S, "Streaming to device stopped");
pthread_mutex_unlock(&sbuf->mutex);
return;
}
}
pthread_mutex_unlock(&sbuf->mutex);
size_t len_in = 2 * (size_t) ssGetInputPortWidth(S, 0);
memcpy(sbuf->buffers[sbuf->tail] + sbuf->offset,
ssGetInputPortSignalPtrs(S, 0)[0], len_in);
sbuf->offset += len_in;
if (sbuf->offset >= BUFFER_SIZE) {
sbuf->offset = 0;
if (++sbuf->tail >= NUMBER_OF_BUFFERS) sbuf->tail = 0;
pthread_mutex_lock(&sbuf->mutex);
sbuf->ready += 1;
pthread_mutex_unlock(&sbuf->mutex);
}
}
static void mdlSetDefaultPortDimensionInfo(SimStruct *S)
{
DimsInfo_T inDI;
int k;
for (k = 0; k < 2; k++) {
inDI.width = ssGetInputPortWidth( S, k);
inDI.numDims = ssGetInputPortNumDimensions(S, k);
inDI.dims = ssGetInputPortDimensions( S, k);
if (!IsFullInfo(&inDI)) {
DimsInfo_T tmpDims;
int dims[2] = {DYNAMICALLY_SIZED, DYNAMICALLY_SIZED};
tmpDims.width = DYNAMICALLY_SIZED;
tmpDims.numDims = DYNAMICALLY_SIZED;
tmpDims.dims = dims;
FillInFullDimensions(&inDI, &tmpDims);
mdlSetInputPortDimensionInfo(S, k, &tmpDims);
}
}
}
static void mdlSetOutputPortWidth(SimStruct *S, int_T port,
int_T outputPortWidth)
{
ssSetOutputPortWidth(S,port,ssGetInputPortWidth(S,0));
}
/* Function: mdlInitializeSizes ===============================================
* Abstract:
* The sizes information is used by Simulink to determine the S-function
* block's characteristics (number of inputs, outputs, states, etc.).
*/
static void mdlInitializeSizes(SimStruct *S)
{
/* See sfuntmpl_doc.c for more details on the macros below */
ssSetNumSFcnParams(S, 9); /* Number of expected parameter vectors*/
if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
/* Return if number of expected != number of actual parameters */
return;
}
//ssSetNumContStates(S, 12);
ssSetNumDiscStates(S, 12);
/* if (!ssSetNumInputPorts(S, 1)) return; */
ssSetNumInputPorts(S, 1);
ssSetInputPortWidth(S, 0, 8);//[thr ail el rud mxtr run flap gear]
/* ssSetInputPortRequiredContiguous(S, 0, true); /*direct input signal access*/
/*
* Set direct feedthrough flag (1=yes, 0=no).
* A port has direct feedthrough if the input is used in either
* the mdlOutputs or mdlGetTimeOfNextVarHit functions.
* See matlabroot/simulink/src/sfuntmpl_directfeed.txt.
*/
/* ssSetInputPortDirectFeedThrough(S, 0, 1); */
if (!ssSetNumOutputPorts(S, 6)) return;
ssSetOutputPortWidth(S, 0, 12);//The model has 12 states:[u v w p q r h-sl-ft long lat phi theta psi]
/* Flight Controls output [thr-pos-norm left-ail-pos-rad el-pos-rad tvc-pos-rad rud-pos-rad flap-pos-norm right-ail-pos-rad
* speedbrake-pos-rad spoiler-pos-rad lef-pos-rad gear-pos-norm Nose-gear-steering-pos-deg gear-unit-WOW]
*/
ssSetOutputPortWidth(S, 1, 13);
/* Propulsion output piston (per engine) [prop-rpm prop-thrust-lbs mixture fuel-flow-gph advance-ratio power-hp pt-lbs_sqft
* volumetric-efficiency bsfc-lbs_hphr prop-torque blade-angle prop-pitch]
* Propulsion output turbine (per engine) [thrust-lbs n1 n2 fuel-flow-pph fuel-flow-pps pt-lbs_sqft pitch-rad reverser-rad yaw-rad inject-cmd
* set-running fuel-dump]
*/
ssSetOutputPortWidth(S, 2, 48);
ssSetOutputPortWidth(S, 3, 11);//Calculated outputs [pilot-Nz alpha alpha-dot beta beta-dot vc-fps vc-kts
// Vt-fps vg-fps mach climb-rate]
ssSetOutputPortWidth(S, 4, 6);//JSBSim Calculated State derivatives output [u_dot v_dot w_dot p_dot q_dot r_dot]
ssSetOutputPortWidth(S, 5, 6);//JSBSim Calculated Aerodynamic forces and monents output [X Y Z L M C]
//ssSetNumSampleTimes(S, 1);
if(!ssSetNumDWork( S, 7)) return;
ssSetDWorkWidth( S, 0, ssGetInputPortWidth(S,0));//Work vector for input port
ssSetDWorkDataType( S, 0, SS_DOUBLE); /* use SS_DOUBLE if needed */
ssSetDWorkWidth( S, 1, ssGetNumDiscStates(S));//Work vector for states * may need to add actuator states!
ssSetDWorkDataType( S, 1, SS_DOUBLE);
ssSetDWorkWidth( S, 2, ssGetOutputPortWidth(S,4)); //Work vector derivatives
ssSetDWorkDataType( S, 2, SS_DOUBLE);
ssSetDWorkWidth( S, 3, ssGetOutputPortWidth(S,1));//Work vector for flight controls outputs
ssSetDWorkDataType( S, 3, SS_DOUBLE);
ssSetDWorkWidth( S, 4, ssGetOutputPortWidth(S,2));//Work vector for propulsion outputs
ssSetDWorkDataType( S, 4, SS_DOUBLE);
ssSetDWorkWidth( S, 5, ssGetOutputPortWidth(S,3));//Work vector for calculated outputs
ssSetDWorkDataType( S, 5, SS_DOUBLE);
ssSetDWorkWidth( S, 6, ssGetOutputPortWidth(S,5));//Work vector for aerodynamic outputs
ssSetDWorkDataType( S, 6, SS_DOUBLE);
ssSetNumPWork(S, 1); // reserve element in the pointers vector
// to store a C++ object
ssSetNumNonsampledZCs(S, 0);
ssSetOptions(S, 0);
}
void mdlSetDefaultPortDimensionInfo(SimStruct *S)
/* ========================================================================*/
{
if (ssGetInputPortWidth(S, 0) == DYNAMICALLY_SIZED)
ssSetInputPortWidth(S, 0, BUFFER_SIZE / BYTES_PER_SAMPLE);
}
/* Function: mdlOutputs =======================================================
* Abstract:
* In this function, you compute the outputs of your S-function
* block. Generally outputs are placed in the output vector(s),
* ssGetOutputPortSignal.
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
const void *pVoidIn = (const void *) ssGetInputPortSignal(S, 0);
void *pVoidOut = ssGetOutputPortSignal(S,0);
int regionIdx = 0;
boolean_T inIsComplex = (ssGetInputPortComplexSignal(S,0) == COMPLEX_YES);
int dataSize = ssGetDataTypeSize( S, ssGetInputPortDataType( S, 0 ) );
int dataWidth = ssGetInputPortWidth(S,0);
unsigned int idx = 0;
char * pCharInBase = NULL;
char * pCharOutBase = NULL;
char * pCharIn = NULL;
char * pCharOut =NULL;
uint32_T regionValue = 0;
fxpStorageContainerCategory inStorageContainerCategory;
fxpStorageContainerCategory outStorageContainerCategory;
DTypeId dTypeId;
if ( inIsComplex )
{
dataWidth = 2 * dataWidth;
}
dTypeId = ssGetInputPortDataType(S,0);
inStorageContainerCategory = ssGetDataTypeStorageContainCat(S,dTypeId);
dTypeId = ssGetOutputPortDataType(S,0);
outStorageContainerCategory = ssGetDataTypeStorageContainCat(S,dTypeId);
if(inStorageContainerCategory != outStorageContainerCategory)
{
ssSetErrorStatus(S, "Input and output have different container categories");
return;
}
if(inStorageContainerCategory == FXP_STORAGE_DOUBLE ||
inStorageContainerCategory == FXP_STORAGE_SINGLE ||
inStorageContainerCategory == FXP_STORAGE_SCALEDDOUBLE)
{
memcpy( pVoidOut, pVoidIn, dataWidth * dataSize );
}
else /*are fixed point*/
{
pCharInBase = (char *) pVoidIn;
pCharOutBase = (char *) pVoidOut;
for(idx = 0; idx < dataWidth; idx ++)
{
pCharIn = &pCharInBase[ idx * dataSize ];
pCharOut = &pCharOutBase[ idx * dataSize ];
for (regionIdx = 0; regionIdx < FXP_NUM_CHUNKS; regionIdx++)
{
regionValue = ssFxpGetU32BitRegion(S,
pCharIn,
dTypeId,
regionIdx);
ssFxpSetU32BitRegion(S,
pCharOut,
dTypeId,
regionValue,
regionIdx);
}
}
}
return;
} /* end mdlOutputs */
/* Function: mdlOutputs =======================================================
*
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
int param_width1=mxGetNumberOfElements(ssGetSFcnParam(S,0));
const real_T *pu0 = (const real_T*) ssGetInputPortSignal(S,0);
real_T *py0 = (real_T *)ssGetOutputPortRealSignal(S,0);
const int_T py_width = ssGetOutputPortWidth(S,0);
const int_T pu_width = ssGetInputPortWidth(S,0);
int j,i,charPos=0,inPos=0,temp,tVal;
char b[1];char tempBuf[5];
memset(msg, 0,100* sizeof(char));
/*Begin Write Block------------------------------*/
msg[charPos++]='[';
/*printf ("pow\n");
for(i=0;i<%<pu_width>;i++)
printf("ins= %d\n",((int)*%<pu0>+i));*/
for(i=0;i<20;i++)
{
if((prefix[i]=='O' || prefix[i]=='P'|| prefix[i]=='M'|| prefix[i]=='T'|| prefix[i]=='S'))
{
temp=(int)(*(pu0+inPos));
if(lasts[i]!=temp)
{
/*printf("pref=%c\n",prefix[i]);*/
msg[charPos++]='W';
if(prefix[i]=='O')
msg[charPos++]='D';
else
msg[charPos++]=prefix[i];
msg[charPos++]=i+48;
memcpy(msg+charPos,&temp, sizeof(int));/*integer-based value*/
charPos+=sizeof(int);
/*msg[charPos++]=(int)(*(%<pu0>+inPos));*/
lasts[i]=(int)(*(pu0+inPos));
}
inPos++;
}
}
if(py_width>=1)/*If need to read*/
{
msg[charPos++]='R';/*Append a read all command at the end*/
msg[charPos++]='+';
msg[charPos++]='0';
msg[charPos++]='0';
msg[charPos++]='0';
msg[charPos++]='0';
msg[charPos++]='0';
}
msg[charPos++]=']';
if(charPos>2)
{
printf("out=");
for(i=0;i<charPos;i++)
printf("%c",*(msg+i));
printf("\n");
j=write(fd,msg,charPos);
if (j < 0)
fputs("write() of bytes failed!\n", stderr);
}
/*End Write Block------------------------------*/
/*Begin Read Block------------------------------*/
if(py_width>=1)/*If need to read*/
{
memset(msg, 0,100* sizeof(char));
i=0;
do {
j=read(fd, b, 1); /* read a char at a time*/
if( j==-1)
{
fputs("read of bytes failed!\n", stderr);
goto OUT2;
}
if( j==0 ) {
printf("Reconfiguring USB\n");
j=write(fd,"!!!!!!X",7);
j=write(fd,conf,param_width1);
j=write(fd,"[R+00000]",9);
i=0;
goto OUT2;
}
msg[i] = b[0]; i++;
} while( b[0] != ']');
msg[i] = 0; /* null terminate the string*/
printf("msg=%s\n",msg+2);
j=0;inPos=0;
while(msg[j++]!='['){if (j>3)goto OUT2;}
while(j<i-3)
{
temp=0;
charPos=msg[j++]-48;
memcpy(&tVal, msg+j, 4);
j+=4;
if(charPos<=19 && inPos<py_width)
*(py0+inPos)=(double)tVal;
inPos++;
}
}
OUT2:;;
}
