static void mdlStart(SimStruct *S)
{
ssSetRWorkValue(S,0,ssGetTStart(S));
real_T t_pre= getTime();
real_T t_pre0=t_pre;
ssSetRWorkValue(S,1,t_pre);
ssSetRWorkValue(S,2,t_pre0);
ssSetRWorkValue(S,3,0.02);
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
real_T t_previousSimTime = ssGetRWorkValue(S,0);
const real_T *scaleFactor = mxGetPr(TIME_SCALE_FACTOR(S));
time_T t_SimTime = ssGetT(S);
real_T t_diff = 0.0;
real_T dt;
real_T t_current;
real_T t_0;
real_T t_previous;
real_T t_elapsed;
/* Desired Delta time */
dt = (t_SimTime - t_previousSimTime) * (scaleFactor[0]);
/* Get clock time at the beginning of this step*/
t_previous = (real_T)clock()/CLOCKS_PER_SEC;
t_0 = t_previous;
/* Wait to reach the desired time */
while (t_diff<dt){
t_current = (real_T) clock()/CLOCKS_PER_SEC;
/* Look for wrapup */
if (t_current<t_previous){
t_elapsed = t_previous - t_0;
t_0 = (real_T) clock()/CLOCKS_PER_SEC - t_elapsed;
}
t_diff = t_current - t_0;
t_previous = t_current;
}
/* Store current time to be used in next time step*/
ssSetRWorkValue(S, 0, t_SimTime);
}
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));
}
/* ======================================================================== */
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 mdlStart(SimStruct *S) {
#ifndef MATLAB_MEX_FILE
int num;
rosShmData_t *shm;
SEM *sem;
rosBlockInitResult_t res;
unsigned int strlen = sizeof(char_T)*(PARAM_SIZE(1)+1);
res = registerRosBlock(S, "rosout", LOGGER, 0);
shm = res.shm;
sem = res.sem;
num = res.num;
shm->msg.level = PARAM(0)[0]-1;
ssSetIWorkValue(S,0,num);
ssSetPWorkValue(S,0,(void *)shm);
ssSetPWorkValue(S,1,(void *)sem);
#endif
ssSetRWorkValue(S,0,0.0);
}
/* Function: mdlOutputs =======================================================
*
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
const uint32_T *set_BaudRate = (const uint32_T*) ssGetInputPortSignal(S,0);
const int16_T *data_send = (const int16_T*) ssGetInputPortSignal(S,1);
int16_T *data_out = (int16_T *)ssGetOutputPortRealSignal(S,0);
int16_T *data_new_flag = (int16_T *)ssGetOutputPortRealSignal(S,1);
real_T *HL_Status = (real_T *)ssGetOutputPortRealSignal(S,2);
int16_T *realTime = (int16_T *)ssGetOutputPortRealSignal(S,3);
real_T t_previousSimTime = ssGetRWorkValue(S,0);
time_T t_SimTime = ssGetT(S);
real_T t_diff = 0.0;
real_T dt;
real_T t_current=getTime();
real_T t_0=t_current;
real_T t_previous=t_current;
real_T t_elapsed;
real_T *y0 = (real_T *)ssGetOutputPortRealSignal(S,0);
real_T t_pre=ssGetRWorkValue(S,1);
real_T t_pre0=ssGetRWorkValue(S,2);
real_T t_term=ssGetRWorkValue(S,3);
real_T t_realTime=t_current-t_pre0;
/* Desired Delta time */
dt = (t_SimTime - t_previousSimTime) ;
/* Get clock time at the beginning of this step*/
if(t_0-t_pre>(dt+0.001))
{
printf("Warning:It is slower than the real time::%f\r\n",t_0-t_pre);
}
t_pre= t_0;
/* Store current time to be used in next time step*/
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */
//static int FTDIopen=0;
if (openFtdi==0)
{
openFtdi = open_ftdi(*set_BaudRate, "Quad_USB2", 5, 15);
}
if (openFtdi == 1)
{
*data_new_flag = read_ftdi (data_out, HL_Status);
while (t_realTime<t_term-dt/2){/*(t_diff<dt){*/
t_current = getTime();
t_realTime=t_current-t_pre0;
/* Look for wrapup */
if (t_current<t_previous){
t_elapsed = t_previous - t_0;
t_0 = getTime() - t_elapsed;
}
t_diff = t_current - t_0;
t_previous = t_current;
}
send_ftdi (data_send);
}
else
{
*data_new_flag = -1;
}
/* Wait to reach the desired time */
while (t_realTime<t_term){/*(t_diff<dt){*/
t_current = getTime();
t_realTime=t_current-t_pre0;
/* Look for wrapup */
if (t_current<t_previous){
t_elapsed = t_previous - t_0;
t_0 = getTime() - t_elapsed;
}
t_diff = t_current - t_0;
t_previous = t_current;
}
ssSetRWorkValue(S, 0, t_SimTime);
ssSetRWorkValue(S,1,t_pre);
ssSetRWorkValue(S,3,t_realTime+dt);
*realTime=t_0-t_pre0;
// Quadlinux_Com_Outputs_wrapper(set_BaudRate, data_send, data_out, data_new_flag, HL_Status, bytes_sent);
}
static void mdlStart(SimStruct *S)
{
ssSetRWorkValue(S,0,ssGetTStart(S));
}
/* Function: mdlStart =======================================================
* Abstract:
* This function is called once at start of model execution. If you
* have states that should be initialized once, this is the place
* to do it.
*/
static void mdlStart(SimStruct *S)
{
#ifndef MATLAB_MEX_FILE
int_T nChannels,range,i,channel;
uint_T base;
volatile uint32_T *ioaddress;
//
//These part is omitted.
//
ssSetIWorkValue(S,BASE_ADDR_I_IND,base);
nChannels=(int_T)mxGetN(CHANNEL_ARG);
ssSetIWorkValue(S,CHANNELS_I_IND,nChannels);
range=(int_T)mxGetPr(RANGE_ARG)[0];
switch(range){
case 1://+/-5V
ssSetRWorkValue(S,GAIN_R_IND,10.0/4096.0);
ssSetRWorkValue(S,OFFSET_R_IND,-5.0);
break;
case 2://+/-10V
ssSetRWorkValue(S,GAIN_R_IND,20.0/4096.0);
ssSetRWorkValue(S,OFFSET_R_IND,-10.0);
break;
case 3://0-10V
ssSetRWorkValue(S,GAIN_R_IND,10.0/4096.0);
ssSetRWorkValue(S,OFFSET_R_IND,0.0);
break;
default:
sprintf(msg,"Range must between 1 to 3");
ssSetErrorStatus(S,msg);
return;
}
//write the channel number into corresponding register (ADCNTL0)
for(i=0;i<nChannels;i++)
{
channel=(int_T)mxGetPr(CHANNEL_ARG)[i]-1;
ioaddress=(void *)(base+0x200+(i*0x4));
ioaddress[0]=channel;
delay(16e-6);
}
//set the number of channels (ADCNTL1)
ioaddress=(void *)(base+0x290);
ioaddress[0]=nChannels-1;
delay(16e-16);
//set the frequency of A/D sampling (ADCNTL1)
ioaddress=(void *)(base+0x280);
ioaddress[0]=0xFF;
delay(16e-6);
//clear FIFO (ADCNTL7)
ioaddress=(void *)(base+0x2E0);
ioaddress[0]=0;
delay(16e-6);
//disallow A\D interrupt
ioaddress=(void *)(base+0x2D0);
ioaddress[0]=0;
delay(16e-16);
#endif
}
