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 mdlStart(SimStruct *S)
{
#ifndef MATLAB_MEX_FILE
MBX *mbx;
char name[7];
int i;
int_T *dim = ssGetInputPortDimensions(S,0);
#ifdef KRTAI
i = mbx_rt_get_adr(TargetLogMbxID,MAX_RTAI_LOGS);
sprintf(name, "%s%d", TargetLogMbxID, i);
rtaiLog[i] = S;
mbx = mbx_rt_mbx_init(name, (MBX_RTAI_LOG_SIZE/(dim[0]*dim[1]*sizeof(float))*(dim[0]*dim[1]*sizeof(float))));
#else
for (i = 0; i < MAX_RTAI_LOGS; i++) {
sprintf(name, "%s%d", TargetLogMbxID, i);
if (!rt_get_adr(nam2num(name))) break;
}
rtaiLog[i] = S;
if (!(mbx = (MBX *)rt_mbx_init(nam2num(name), (MBX_RTAI_LOG_SIZE/(dim[0]*dim[1]*sizeof(float))*(dim[0]*dim[1]*sizeof(float)))))) {
printf("Cannot init mailbox\n");
exit(1);
}
#endif
ssGetPWork(S)[0] = (void *)mbx;
#endif
}
/* Function: mdlOutputs =======================================================
* Abstract:
* In this function, you compute the outputs of your S-function
* block.
*/
static void mdlOutputs(SimStruct *S, int_T tid) {
// get Objects
void** vecPWork = ssGetPWork(S);
// get Pointers
// accessing inputs
const uint32_T *idx = (const uint32_T*) ssGetInputPortSignal(S, 0);
const real_T *pts = (const real_T*) ssGetInputPortSignal(S, 1);
if (ssGetInputPortNumDimensions(S, 0) != 2 || ssGetInputPortNumDimensions(S, 1) != 2) {
ssSetErrorStatus(S,
"Wrong number of dimensions. This should never happen!.");
return;
}
#if defined(MATLAB_MEX_FILE)
const int_T nTriangles = ssGetCurrentInputPortDimensions(S, 0, 1);
const int_T nPts = ssGetCurrentInputPortDimensions(S, 1, 1);
#else
const int_T nTriangles = ssGetInputPortDimensions(S, 0)[1];
const int_T nPts = ssGetInputPortDimensions(S, 1)[1];
#endif
GenericPublisher<shape_msgs::Mesh>* pub =
(GenericPublisher<shape_msgs::Mesh>*) vecPWork[0];
const std::string* topic = (const std::string*) vecPWork[1];
shape_msgs::Mesh msg;
msg.triangles.resize(nTriangles);
msg.vertices.resize(nPts);
for (int_T i = 0; i < 3; ++i) {
for (int_T j = 0; j < nTriangles; ++j) {
msg.triangles[j].vertex_indices[i] = idx[3 * j + i];
}
}
for (int_T j = 0; j < nPts; ++j) {
msg.vertices[j].x = pts[3 * j + 0];
msg.vertices[j].y = pts[3 * j + 1];
msg.vertices[j].z = pts[3 * j + 2];
}
pub->publish(msg);
}
/* ======================================================================== */
static void startHackrfTx(SimStruct *S, bool print_info)
/* ======================================================================== */
{
double sample_rate = (1.0 / ssGetSampleTime(S, 0)) * ssGetInputPortDimensions(S, 0)[0];
hackrf_device *device = startHackrf(S, sample_rate, GetParam(BANDWIDTH), print_info);
ssSetPWorkValue(S, DEVICE, device);
if (ssGetErrorStatus(S)) return;
int i = 0; for (; i < NUM_PARAMS; i++) ssSetRWorkValue(S, i, NAN);
mdlProcessParameters(S);
sample_buffer_reset((SampleBuffer*) ssGetPWorkValue(S, SBUF));
int ret = hackrf_start_tx(device, hackrf_tx_callback, S);
Hackrf_assert(S, ret, "Failed to start RX streaming");
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
int_T *dim = ssGetInputPortDimensions(S,0);
struct {
float u[dim[0]*dim[1]];
} data;
int i;
#ifndef MATLAB_MEX_FILE
MBX *mbx = (MBX *)ssGetPWork(S)[0];
for (i = 0; i < (dim[0]*dim[1]); i++) {
data.u[i] = (float)*uPtrs[i];
}
#ifdef KRTAI
mbx_rt_mbx_send_if(mbx, &data, sizeof(data));
#else
rt_mbx_send_if(mbx, &data, sizeof(data));
#endif
#endif
}
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 */
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);
}
}
}
