/* 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];
}
}
}
}
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);
}
}
/* 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)
{
/*
*typedef struct {
uint8_T LENGTH;
uint8_T RTR;
CanFrameType type;
uint32_T ID;
uint8_T DATA[8];
} CAN_FRAME;
*/
/* pointer to input frame */
CAN_FRAME * frame = ((CAN_FRAME *) (ssGetInputPortSignalPtrs(S,0)[0]));
/* pointer to output double */
real_T * outputData = ssGetOutputPortRealSignal(S, 0);
real_T * outputData1 = ssGetOutputPortRealSignal(S, 1);
real_T * outputData2 = ssGetOutputPortRealSignal(S, 2);
real_T * outputData3 = ssGetOutputPortRealSignal(S, 3);
/* copy data bytes */
*outputData=frame->ID;
*outputData1 =frame->type; //STD or Extended
*outputData2=frame->LENGTH;
memcpy(outputData3,frame->DATA,8);
}
static void mdlOutputs(SimStruct *S, int_T tid)
/* ======================================================================== */
{
const int_T frame_length = (int_T)(double)mxGetScalar(ssGetSFcnParam(S, FRAME_LENGTH));
struct hackrf_device *_device = (hackrf_device*)ssGetPWorkValue(S, DEVICE);
// get sample buffer work vars
SampleBuffer *sbuf = (SampleBuffer *)ssGetPWorkValue(S, SBUF);
/* input buffer */
double* out = (double*)ssGetInputPortSignalPtrs(S, 0);
pthread_mutex_t* mutex = (pthread_mutex_t*)ssGetPWorkValue(S, MUTEX);
pthread_mutex_lock(mutex);
while (sbuf->count == sbuf->num){
pthread_cond_wait((pthread_cond_t*)ssGetPWorkValue(S, COND_VAR), mutex);
}
// output underrun status
if (sbuf->underrun) {
ssPrintf("U");
sbuf->underrun = false;
}
pthread_mutex_unlock(mutex);
// get next samples to be read (select buffer + offset)
char *buf = (char *)sbuf->buf[sbuf->head] + BYTES_PER_SAMPLE*(sbuf->offset);
if (frame_length < sbuf->samp_avail)
{
for (int k = 0; k < BYTES_PER_SAMPLE * frame_length; ++k)
{
buf[k] = (char)(out[k] * 127.0);
}
sbuf->offset += frame_length;
sbuf->samp_avail -= frame_length;
}
else{
for (int k = 0; k < BYTES_PER_SAMPLE * sbuf->samp_avail; ++k)
{
buf[k] = (char)(out[k] * 127.0);
}
pthread_mutex_lock(mutex);
sbuf->head = (sbuf->head + 1) % sbuf->num;
sbuf->count++;
pthread_mutex_unlock(mutex);
// set to start of the next sample buffer
buf = (char *)sbuf->buf[sbuf->head];
int remaining = frame_length - sbuf->samp_avail;
for (int k = 0; k < BYTES_PER_SAMPLE*(remaining); ++k){
buf[k] = (char)(out[k + BYTES_PER_SAMPLE*sbuf->samp_avail] * 127.0);
}
sbuf->offset = remaining;
sbuf->samp_avail = (BUF_SIZE / BYTES_PER_SAMPLE) - remaining;
}
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
PCONFIG_DATA config;
const real_T *in_xdot;
const real_T *in_x0;
InputRealPtrsType pin_params;
const boolean_T *in_reset;
real_T *out_x;
real_T *out_t;
real_T *xd;
real_T *xd_temp;
real_T *xd_temp2;
time_T initial_time;
time_T final_time;
quaternion phi;
quaternion q;
vector omegadot_temp;
const real_T *pomegadot;
int_T i;
/* Retrieve C object from the pointers vector */
config = ssGetPWork(S)[0];
xd = (real_T*) ssGetDWork(S,0);
xd_temp = (real_T*) ssGetDWork(S,1);
if( config->nq ) xd_temp2 = (real_T*) ssGetDWork(S,2);
in_xdot = ssGetInputPortRealSignal(S, config->idxin_xdot);
if( config->idxin_x0 ) in_x0 = ssGetInputPortRealSignal(S, config->idxin_x0);
if( config->idxin_params ) pin_params = ssGetInputPortRealSignalPtrs(S, config->idxin_params);
if( config->idxin_reset ) in_reset = ((InputBooleanPtrsType) ssGetInputPortSignalPtrs(S, config->idxin_reset))[0];
out_x = ssGetOutputPortRealSignal(S, 1);
if( config->idxout_time ) out_t = ssGetOutputPortRealSignal(S, config->idxout_time);
switch( intval(mxGetScalar(paramSpecificationsSource)) )
{
case 1:
initial_time = config->initial_time;
final_time = ssGetTaskTime(S,0);
break;
case 2:
initial_time = mxGetScalar(paramInitialTime);
final_time = mxGetScalar(paramFinalTime);
break;
case 3:
initial_time = *(pin_params[0]);
final_time = *(pin_params[1]);
break;
default:
ssSetErrorStatus(S,"Wrong integration algorithm selected");
return;
}
/* ssPrintf("ti=%f, tf=%f\r\n", initial_time, final_time); */
/* Reset the states */
if( ssGetIWorkValue(S, 0) || (config->idxin_reset && *in_reset) || (intval(mxGetScalar(paramSpecificationsSource)) > 1) )
{
ssSetIWorkValue(S, 0, 0);
if( intval(mxGetScalar(paramInitialConditionSource)) == 1 )
{
/* Internal initial conditions */
for( i=0; i<config->nstates; i++ )
{
xd[i] = mxGetPr(paramInitialCondition)[(mxGetNumberOfElements(paramInitialCondition) == 1 ? 0 : i)];
}
}
else
{
/* External initial conditions */
memcpy(xd, in_x0, config->nstates*sizeof(real_T));
}
memcpy(out_x, xd, config->nstates*sizeof(real_T));
if( config->idxout_time ) out_t[0] = initial_time;
}
if( final_time > initial_time )
{
if( intval(mxGetScalar(paramIntegrationAlgorithm)) == 1 )
{
/* Euler algorithm */
if( !ssCallSystemWithTid(S,0,tid) ) return;
i = 0;
while( i<config->nstates )
{
if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
{
pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
QuaternionIntegralEulerChange( final_time-initial_time, &in_xdot[i], pomegadot, &xd[i], phi );
QuaternionProd(&xd[i], phi, &xd[i]);
QuaternionNormalize(&xd[i]);
i += 4;
}
else
{
xd[i] += in_xdot[i] * (final_time-initial_time);
i++;
}
}
}
if( intval(mxGetScalar(paramIntegrationAlgorithm)) == 2 )
{
/* Runge-Kutta algorithm */
/* f1 */
if( !ssCallSystemWithTid(S,0,tid) ) return;
i = 0;
while( i<config->nstates )
{
if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
{
pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], &xd_temp[i] );
QuaternionProd(&xd_temp[i], &xd_temp[i], q);
QuaternionProd(&xd_temp[i], q, phi);
QuaternionProd(&xd[i], phi, &out_x[i]);
i += 4;
}
else
{
xd_temp[i] = in_xdot[i];
out_x[i] = xd[i] + 0.5*(final_time-initial_time)*in_xdot[i];
i++;
}
}
if( config->idxout_time ) out_t[0] = initial_time + 0.5*(final_time-initial_time);
/* f2 */
if( !ssCallSystemWithTid(S,0,tid) ) return;
i = 0;
while( i<config->nstates )
{
if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
{
pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], q );
QuaternionProd(q, q, phi);
QuaternionProd(&xd_temp[i], phi, &xd_temp[i]);
QuaternionProd(phi, q, phi);
QuaternionProd(&xd[i], phi, &out_x[i]);
i += 4;
}
else
{
xd_temp[i] += 2*in_xdot[i];
out_x[i] = xd[i] + 0.5*(final_time-initial_time)*in_xdot[i];
i++;
}
}
if( config->idxout_time ) out_t[0] = initial_time + 0.5*(final_time-initial_time);
/* f3 */
if( !ssCallSystemWithTid(S,0,tid) ) return;
i = 0;
while( i<config->nstates )
{
if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
{
pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], q );
QuaternionProd(q, q, phi);
QuaternionProd(&xd_temp[i], phi, &xd_temp[i]);
QuaternionProd(phi, q, phi);
QuaternionProd(phi, phi, phi);
QuaternionProd(&xd[i], phi, &out_x[i]);
i += 4;
}
else
{
xd_temp[i] += 2*in_xdot[i];
out_x[i] = xd[i] + (final_time-initial_time)*in_xdot[i];
i++;
}
}
if( config->idxout_time ) out_t[0] = final_time;
/* f4 */
if( !ssCallSystemWithTid(S,0,tid) ) return;
i = 0;
while( i<config->nstates )
{
if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
{
pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], q );
QuaternionProd(&xd_temp[i], q, &xd_temp[i]);
QuaternionProd(&xd[i], &xd_temp[i], &xd[i]);
QuaternionNormalize(&xd[i]);
i += 4;
}
else
{
xd_temp[i] += in_xdot[i];
xd[i] += 1.0/6*(final_time-initial_time)*xd_temp[i];
i++;
}
}
}
}
else
{
if( !ssCallSystemWithTid(S,0,tid) ) return;
}
config->initial_time = final_time;
/* Update the outputs */
memcpy(out_x, xd, config->nstates*sizeof(real_T));
if( config->idxout_time ) out_t[0] = final_time;
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
/*
* detect which inputs are complex or not
*/
boolean_T u0IsComplex = ssGetInputPortComplexSignal(S, 0) == COMPLEX_YES;
boolean_T u1IsComplex = ssGetInputPortComplexSignal(S, 1) == COMPLEX_YES;
boolean_T yIsComplex = ssGetOutputPortComplexSignal(S, 0)== COMPLEX_YES;
DTypeId dType = ssGetOutputPortDataType(S,0);
InputPtrsType u0VPtr = ssGetInputPortSignalPtrs(S,0);
InputPtrsType u1VPtr = ssGetInputPortSignalPtrs(S,1);
int_T width = ssGetInputPortWidth(S,0);
int_T i;
switch (dType) {
case (SS_DOUBLE): {
real_T *y = (real_T *)ssGetOutputPortSignal(S,0);
real_T yr = 0.0; /* real part of the result */
real_T yi = 0.0; /* imag part of the result */
InputRealPtrsType u0Ptr = (InputRealPtrsType)u0VPtr;
InputRealPtrsType u1Ptr = (InputRealPtrsType)u1VPtr;
if(!yIsComplex){
/* both inputs are real */
for (i = 0; i < width; i++) {
yr += (**u0Ptr++) * (**u1Ptr++);
}
} else {
/* At least one if the inputs is complex. */
for (i = 0; i < width; i++) {
real_T u0r = u0Ptr[i][0];
real_T u0i = (u0IsComplex)? - u0Ptr[i][1] : 0.0;
real_T u1r = u1Ptr[i][0];
real_T u1i = (u1IsComplex)? u1Ptr[i][1] : 0.0;
yr += (u0r * u1r - u0i * u1i);
yi += (u0r * u1i + u0i * u1r);
}
}
/* Update output */
y[0] = yr;
if (yIsComplex) {
y[1] = yi;
}
}
break;
case (SS_SINGLE): {
real32_T *y = (real32_T *)ssGetOutputPortSignal(S,0);
real32_T yr = 0.0F; /* real part of the result */
real32_T yi = 0.0F; /* imag part of the result */
if(!yIsComplex){
/* both inputs are real */
for (i = 0; i < width; i++) {
const real32_T *u0Ptr = u0VPtr[i];
const real32_T *u1Ptr = u1VPtr[i];
yr += (*u0Ptr) * (*u1Ptr);
}
} else {
/* At least one if the inputs is complex. */
for (i = 0; i < width; i++) {
const real32_T *u0Ptr = u0VPtr[i];
const real32_T *u1Ptr = u1VPtr[i];
real32_T u0r = u0Ptr[0];
real32_T u0i = (u0IsComplex)? - u0Ptr[1] : 0.0F;
real32_T u1r = u1Ptr[0];
real32_T u1i = (u1IsComplex)? u1Ptr[1] : 0.0F;
yr += (u0r * u1r - u0i * u1i);
yi += (u0r * u1i + u0i * u1r);
}
}
/* Update output */
y[0] = yr;
if (yIsComplex) {
y[1] = yi;
}
}
break;
}
}
/* mdlOutputs() */
static void mdlOutputs(SimStruct *S, int_T tid) {
CAN_FRAME ** frames = (CAN_FRAME **)ssGetInputPortSignalPtrs(S,0);
uint8_T * data_output;
uint32_T * id_output;
uint32_T * length_output;
int_T payload_len;
// The number of output ports required
int nOP = 0;
// The current output port
int cOP = 0;
// Check that the payload is a valid size
payload_len = frames[0]->LENGTH;
if (payload_len < 0 || payload_len > MAX_PAYLOAD_LEN) {
ssSetErrorStatus(S,"Invalid input frame payload length.");
return;
}
if ( P_SHOW_DATA){
DTypeId dtypeID = ssGetOutputPortDataType(S,cOP);
int_T outWidth = ssGetDataTypeSize(S, dtypeID) * ssGetOutputPortWidth(S,cOP);
int_T idx_end = payload_len < outWidth ? payload_len : outWidth;
int idx;
data_output = (unsigned char * ) ssGetOutputPortSignal(S,cOP);
if (P_ENDIAN == CPU_ENDIAN ){
memcpy(data_output,&(frames[0]->DATA[0]),idx_end);
}else{
int idx;
int dataTypeSize,portWidth,signals,offset;
dataTypeSize = ssGetDataTypeSize(S,ssGetOutputPortDataType(S,cOP));
portWidth = ssGetOutputPortWidth(S,cOP);
for ( signals = 0; signals < portWidth; signals ++ ){
offset = signals * dataTypeSize;
for ( idx=0;idx< dataTypeSize;idx++ ){
((uint8_T *)(data_output))[idx+offset]=
frames[0]->DATA[offset+dataTypeSize-idx-1];
}
}
}
/* Zero the remaining elements in the output if there are any */
for ( idx=idx_end; idx < outWidth; idx++ ){
((uint8_T *)(data_output))[idx]=0;
}
cOP++;
}
if(P_SHOW_ID){
id_output = (uint32_T *)ssGetOutputPortSignal(S,cOP);
*id_output = frames[0]->ID;
cOP++;
}
if(P_SHOW_LENGTH){
length_output = (uint32_T *)ssGetOutputPortSignal(S,cOP);
*length_output = frames[0]->LENGTH;
cOP++;
}
}
