void mdlStart(SimStruct *S) /* ======================================================================== */ { int i = 0; for (; i < P_WORK_LENGTH; ++i) ssSetPWorkValue(S, i, NULL); ssSetPWorkValue(S, SBUF, sample_buffer_new()); Hackrf_assert(S, hackrf_init(), "Failed to initialize HackRF API"); startHackrfTx(S, true); }
static void mdlStart(SimStruct * S) { uchar_T ipAddress[16]; int_T ipPort = (int_T) mxGetPr(IP_PORT_ARG)[0]; int_T noBytes = (int_T) mxGetPr(NO_BYTES_ARG)[0]; int_T id = 0; uint8_T *buffer = (uint8_T *) calloc(noBytes, sizeof(uint8_T)); if (buffer == NULL) { sprintf(msg, "Error UDP Receive: Memory allocation failed"); ssSetErrorStatus(S, msg); return; } ssSetPWorkValue(S, 0, (void *) buffer); /* initialize buffer with zeros */ memset((void *) buffer, 0, noBytes * sizeof(uint8_T)); mxGetString(IP_ADDRESS_ARG, ipAddress, 16); id = udpOpenReceive(ipPort, ipAddress, noBytes); if(id < 0){ sprintf(msg, "Error: No UDP channel free or UDP error"); ssSetErrorStatus(S, msg); } ssSetIWorkValue(S, 0, id); ssSetIWorkValue(S, 1, noBytes); }
static void mdlUpdate(SimStruct *S, int_T tid) { UNUSED(tid); TimePoint *wall_clock_start_time = static_cast<TimePoint *>(ssGetPWorkValue(S, 0)); if (!wall_clock_start_time) { // t0 not set yet, set it now. (note: used to set this in mdlStart, but // there can be a big pause between mdlStart and the full simulation start) wall_clock_start_time = new TimePoint(); *wall_clock_start_time = TimeClock::now(); ssSetPWorkValue(S, 0, wall_clock_start_time); } double sim_time = ssGetT(S); double realtime_factor = mxGetScalar(ssGetSFcnParam(S, 1)); TimePoint wall_time = TimeClock::now(); TimePoint desired_time = *wall_clock_start_time + TimeDuration(sim_time / realtime_factor); if (desired_time > wall_time) { // could probably just call sleep_until, but just in case std::this_thread::sleep_until(desired_time); } else if (wall_time > desired_time + std::chrono::duration<double>( 1.0 / realtime_factor)) { mexPrintf("at time %f, I'm behind by more than 1 (scaled) second\n", sim_time); ssSetErrorStatus(S, "Simulink is not keeping up with real time. Consider " "reducing demands on your ODE solver, or optimizing your " "code."); } }
static void mdlStart(SimStruct *S) { real_T *y = ssGetOutputPortRealSignal(S,0); const real_T *initVal = PARAM(2); int_T initValLen = PARAM_SIZE(2); int i; #ifndef MATLAB_MEX_FILE int num; rosShmData_t *shm; SEM *sem; rosBlockInitResult_t res; unsigned int strlen = sizeof(char_T)*(PARAM_SIZE(1)+1); char_T *str = (char_T *)malloc(strlen); mxGetString(ssGetSFcnParam(S,1), str, strlen); res = registerRosBlock(S, str, SUBSCRIBER, PARAM(0)[0]); shm = res.shm; sem = res.sem; num = res.num; rt_sem_wait(sem); shm->length = ssGetOutputPortWidth(S,0); for (i = 0; i < shm->length; ++i) { if (initValLen > 1) { shm->data[i] = initVal[i]; } else { shm->data[i] = initVal[0]; } } rt_sem_signal(sem); ssSetIWorkValue(S,0,num); ssSetPWorkValue(S,0,(void *)shm); ssSetPWorkValue(S,1,(void *)sem); free(str); #endif for (i = 0; i < ssGetOutputPortWidth(S,0); ++i) { if (initValLen > 1) { y[i] = initVal[i]; } else { y[i] = initVal[0]; } } }
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: 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) { #if defined(__linux) Robovero *robo1; I2CDevice *accel_p, *magneto_p, *gyro_p; PWMDevice *servo1_p, *servo4_p; ACDDevice *adc_p; // create Robovero object //robo1 = new Robovero("/dev/ttyACM0","SF_logging.log"); robo1 = new Robovero("/dev/ttyACM0"); // save the Robovero pointer to the PWork vector ssSetPWorkValue(S, 0, robo1); // create the accelerometer I2C device accel_p = new I2CDevice(robo1,0x18); // save it to the PWork vector ssSetPWorkValue(S, 1, accel_p); // initialize the accelerometer accel_p->writeReg((void *)0x20,0x27); accel_p->writeReg((void *)0x23,0x00); // create the magnetometer I2C device magneto_p = new I2CDevice(robo1,0x1E); // save it to the PWork vector ssSetPWorkValue(S, 2, magneto_p); // initialize the magnetometer magneto_p->writeReg((void *)0x00,0x18); magneto_p->writeReg((void *)0x01,0x20); magneto_p->writeReg((void *)0x02,0x00); // create the gyro I2C device gyro_p = new I2CDevice(robo1, 0x68); // save it to the PWork vector ssSetPWorkValue(S, 3, gyro_p); // initalize the gyro gyro_p->writeReg((void *)0x22,0x08); gyro_p->writeReg((void *)0x23,0x80); gyro_p->writeReg((void *)0x20,0x0F); // initalize the PWM devices and save their pointers to PWork servo1_p = new PWMDevice(robo1,1); ssSetPWorkValue(S, 4, servo1_p); servo4_p = new PWMDevice(robo1,4); ssSetPWorkValue(S, 5, servo4_p); // initalize the ADC and save pointer to PWork adc_p = new ADCDevice(robo1); ssSetPWorkValue(S, 6, adc_p); #endif }
/* ======================================================================== */ void mdlTerminate(SimStruct *S) /* ======================================================================== */ { stopHackRf(S, DEVICE); Hackrf_assert(S, hackrf_exit(), "Failed to exit HackRF API"); SampleBuffer *sbuf = ssGetPWorkValue(S, SBUF); if (sbuf) { if (sbuf->had_error) ssPrintf("\n"); sample_buffer_free(sbuf); ssSetPWorkValue(S, SBUF, NULL); } }
/* * 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) { bus_t *c; char *name; c = bus_alloc (); ssSetPWorkValue (S, 0, c); name = strrchr (ssGetPath (S), '/'); if (name) c->name = name + 1; else c->name = ssGetPath (S); }
static void mdlTerminate(SimStruct *S) { // IF YOU FORGET TO DESTROY OBJECTS OR DEALLOCATE MEMORY, MATLAB WILL CRASH. // Retrieve and destroy C++ object BufferedPort<Vector> *toPort = static_cast<BufferedPort<Vector>*>(ssGetPWork(S)[0]); toPort->close(); if(ssGetPWork(S) != NULL){ ssSetPWorkValue(S,0,NULL); } Network::fini(); fprintf(stderr,"Everything was closed correctly\n"); }
/* ======================================================================== */ 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"); }
/* Function: mdlTerminate ================================================= * Abstract: * In this function, you should perform any actions that are necessary * at the termination of a simulation. */ static void mdlTerminate(SimStruct *S) { /* * Get access to Parameter/Input/Output/DWork/size information */ void *work1 = ssGetPWorkValue(S, 0); /* * Call the legacy code function */ closeLogFile( &work1); /* Update the PWorks */ ssSetPWorkValue(S, 0, work1); }
static void mdlStart(SimStruct *S) { const real_T *pp1 = mxGetData(PARAM_DEF0(S)); const real_T *pp2 = mxGetData(PARAM_DEF1(S)); int P2=*pp2; if(P2!=0) ssSetOutputPortWidth(S, 0,P2); int connected=0,accCon,portno; struct sockaddr_in serv_addr; char* P1=(char*)mxGetPr(ssGetSFcnParam(S,0)); char* last; int num=mxGetNumberOfElements(ssGetSFcnParam(S,0)); int count; char *port; char porttemp[20]; /*Determine Port Number*/ portno =*pp1; const int_T y_width = ssGetOutputPortWidth(S,0); int yes=1;int i; accCon = socket(AF_INET, SOCK_STREAM, 0); if (accCon < 0) error("ERROR opening socket"); bzero((char *) &serv_addr, sizeof(serv_addr)); setsockopt(accCon,SOL_SOCKET,SO_REUSEADDR,&yes,sizeof(yes)); serv_addr.sin_family = AF_INET; serv_addr.sin_addr.s_addr = INADDR_ANY; serv_addr.sin_port = htons(portno); if (bind(accCon, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0) error("ERROR on binding"); fcntl(accCon, F_SETFL, O_NONBLOCK); last = malloc( y_width); for(i=0;i<y_width;i++) last[i]=128; ssSetIWorkValue(S, 0, connected); ssSetIWorkValue(S, 1, accCon); /*ssSetPWorkValue(S, 2, (void*) &cli_addr);*/ ssSetPWorkValue(S, 1, (void*) last); }
static void mdlStart(SimStruct *S) { char *classNameStr = mxArrayToString(ssGetSFcnParam(S, 0)); std::string className(classNameStr); mxFree(classNameStr); wbt::Block *block = wbt::Block::instantiateBlockWithClassName(className); ssSetPWorkValue(S, 0, block); wbt::Error error; if (!block || !block->initialize(S, &error)) { yError() << "[mdlStart]" << error.message; static char errorBuffer[1024]; sprintf(errorBuffer, "[mdlStart]%s", error.message.substr(0, 1023 - strlen("[mdlStart]")).c_str()); ssSetErrorStatus(S, errorBuffer); } }
/* 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) { /* * Get access to Parameter/Input/Output/DWork/size information */ real_T *u1 = (real_T *) ssGetInputPortSignal(S, 0); void *work1 = ssGetPWorkValue(S, 0); uint32_T *work2 = (uint32_T *) ssGetDWork(S, 0); /* * Call the legacy code function */ incAndLogFaultCounter( work1, work2, *u1); /* Update the PWorks */ ssSetPWorkValue(S, 0, work1); }
static void mdlTerminate(SimStruct *S) { if (ssGetNumPWork(S) > 0 && ssGetPWork(S)) { wbt::Block *block = static_cast<wbt::Block*>(ssGetPWorkValue(S, 0)); wbt::Error error; if (block) { if (block->terminate(S, &error)) { delete block; ssSetPWorkValue(S, 0, NULL); } else { static char errorBuffer[1024]; sprintf(errorBuffer, "[mdlTerminate]%s", error.message.substr(0, 1023 - strlen("[mdlTerminate]")).c_str()); ssSetErrorStatus(S, errorBuffer); } } } }
static void mdlStart(SimStruct *S) { #ifdef CO_DEBUG printf("co_udp_receiver :: in mdlStart\n"); #endif //allocate my_data_t block which stores all variable used in this block struct my_data_t *my_data = new my_data_t; memset(my_data,0,sizeof(my_data_t)); ssSetPWorkValue(S,0,(void*)my_data); int co_port = (int)*mxGetPr(ssGetSFcnParam(S,0)); int nonblockingmode = (int)*mxGetPr(ssGetSFcnParam(S,1)); #ifdef CO_DEBUG printf(" co_udp_receiver :: CO Port number received is %d\n", co_port); #endif if(co_port < 1 || co_port > 64000) { /* print error message and exit */ fprintf(stderr," co_udp_receiver :: Bad port number %d\n",co_port); } co_udp_server_init(co_port,nonblockingmode,my_data); }
static void mdlStart(SimStruct * S) { int_T noBytes; uint8_T *buffer; noBytes = (int_T) mxGetPr(NO_BYTES_ARG)[0]; /*alloc memory and store pointer*/ buffer = (uint8_T *) calloc(noBytes, sizeof(uint8_T)); if (buffer == NULL) { sprintf(msg, "Error in memory allocation"); ssSetErrorStatus(S, msg); return; } ssSetPWorkValue(S, 0, (void *) buffer); /* initialize buffer with zeros */ memset((void *) buffer, 0, noBytes * sizeof(uint8_T)); #ifdef __linux if (camera_setup()) { sprintf(msg, "Error in camera_setup"); ssSetErrorStatus(S, msg); return; } #ifndef DO_THREAD thread_id = 1; if (pthread_create(&thread_str, NULL, image_thread, &thread_id)) { cleanup(); if (buffer == NULL) { sprintf(msg, "Error in pthread_create"); ssSetErrorStatus(S, msg); return; } } run_pthread = 1; frame_flag = 0; #endif #endif ssSetIWorkValue(S, 0, noBytes); }
static void mdlStart(SimStruct *S) /* ======================================================================== */ { int ret = HACKRF_SUCCESS; hackrf_device_list_t *list; struct hackrf_device *device; const uint32_t device_index = (uint32_t)mxGetScalar(ssGetSFcnParam(S, DEVICE_INDEX)); const uint64_t frequency = (uint64_t)mxGetScalar(ssGetSFcnParam(S, FREQUENCY)); const double sample_rate = mxGetScalar(ssGetSFcnParam(S, SAMPLE_RATE)); const uint32_t txvga = (uint32_t)mxGetScalar(ssGetSFcnParam(S, TXVGA)); const uint32_t amp = (uint32_t)mxGetScalar(ssGetSFcnParam(S, AMP)); const uint32_t bandwidth = (uint32_t)mxGetScalar(ssGetSFcnParam(S, BANDWIDTH)); /* Set options of this Block */ ssSetOptions(S, ssGetOptions(S) | SS_OPTION_CALL_TERMINATE_ON_EXIT); /* give handle to PWork vector */ ssSetPWorkValue(S, DEVICE, NULL); /* init HackRF device */ ret = hackrf_init(); if (ret < 0) { ssSetErrorStatusf(S, "Failed to init HackRF device #%d", device_index); return; } list = hackrf_device_list(); if (list->devicecount < 1) { ssSetErrorStatusf(S, "No HackRF boards found.\n"); return; } /* open HackRF device */ ret = hackrf_device_list_open(list, device_index, &device); if (ret < 0) { ssSetErrorStatusf(S, "Failed to open HackRF device #%d", device_index); return; } /* give handle to PWork vector */ ssSetPWorkValue(S, DEVICE, (struct hackrf_device *)device); ret = hackrf_set_sample_rate(device, sample_rate); if (ret < 0) { ssSetErrorStatusf(S, "Failed to Set HackRF Sample Rate #%d", device_index); } ret = hackrf_set_freq(device, frequency); if (ret < 0) { ssSetErrorStatusf(S, "Failed to Set HackRF frequency #%d", device_index); } ret = hackrf_set_baseband_filter_bandwidth(device, bandwidth); if (ret < 0) { ssSetErrorStatusf(S, "Failed to Set HackRF bandwidth #%d", device_index); } ret = hackrf_set_txvga_gain(device, txvga); ret |= hackrf_set_amp_enable(device, amp); if (ret < 0) { ssSetErrorStatusf(S, "Failed to Set HackRF gain #%d", device_index); } /* create mutex for sample thread */ pthread_mutex_t *mutex = (pthread_mutex_t *)malloc(sizeof(pthread_mutex_t)); //*mutex = PTHREAD_MUTEX_INITIALIZER; pthread_mutex_init(mutex, NULL); ssSetPWorkValue(S, MUTEX, mutex); /* create condition variable for sample thread */ pthread_cond_t *cond_var = (pthread_cond_t *)malloc(sizeof(pthread_cond_t)); pthread_cond_init(cond_var, NULL); ssSetPWorkValue(S, COND_VAR, cond_var); /* allocate memory for sample buffer */ SampleBuffer *sbuf = (SampleBuffer*)malloc(sizeof(SampleBuffer)); sbuf->num = BUF_NUM; sbuf->head = sbuf->offset = sbuf->count = sbuf->tail = 0; sbuf->samp_avail = BUF_SIZE / BYTES_PER_SAMPLE; sbuf->underrun = sbuf->underrun_before = false; sbuf->buf = (unsigned char **)malloc(sbuf->num * sizeof(unsigned char *)); if (sbuf->buf) { for (unsigned int i = 0; i < sbuf->num; ++i) sbuf->buf[i] = (unsigned char *)malloc(BUF_SIZE * sizeof(unsigned char)); } ssSetPWorkValue(S, SBUF, sbuf); ret = hackrf_start_tx(device, tx_callback, (void*)S); if (ret < 0) { ssSetErrorStatusf(S, "Failed to Start HackRF #%d", device_index); } }
static void mdlStart(SimStruct *S) { ssSetPWorkValue(S, 0, NULL); }