/* ======================================================================== */ static int tx_callback( hackrf_transfer * transfer ) /* ======================================================================== */ { unsigned char * buf = transfer->buffer; int len = transfer->valid_length; /* get access to Simulink stuff */ SimStruct *S = (SimStruct *) transfer->tx_ctx; SampleBuffer *sbuf = (SampleBuffer *) ssGetPWorkValue( S, SBUF ); /* write received samples to sample buffer */ std::mutex * mutex = (std::mutex *) ssGetPWorkValue( S, MUTEX ); std::unique_lock<std::mutex> lock( *mutex ); /* get pos write pos in buffers */ if ( sbuf->count == 0 ) { memset( buf, 0, len ); sbuf->underrun = true; sbuf->underrun_before = true; }else { memcpy( buf, sbuf->buf[sbuf->tail], len ); sbuf->tail = (sbuf->tail + 1) % sbuf->num; sbuf->count--; } std::condition_variable * cond_var = (std::condition_variable *) ssGetPWorkValue( S, COND_VAR ); cond_var->notify_one(); /* notify output function */ return(0); }
/* ======================================================================== */ static int tx_callback(hackrf_transfer * transfer) /* ======================================================================== */ { unsigned char * buf = transfer->buffer; int len = transfer->valid_length; // get access to Simulink stuff SimStruct *S = (SimStruct *)transfer->tx_ctx; SampleBuffer *sbuf = (SampleBuffer *)ssGetPWorkValue(S, SBUF); // write received samples to sample buffer pthread_mutex_t* mutex = (pthread_mutex_t*)ssGetPWorkValue(S, MUTEX); pthread_mutex_lock(mutex); // get pos write pos in buffers if (sbuf->count == 0){ memset(buf, 0, len); sbuf->underrun = true; sbuf->underrun_before = true; } else{ memcpy(buf, sbuf->buf[sbuf->tail], len); sbuf->tail = (sbuf->tail + 1) % sbuf->num; sbuf->count--; } pthread_mutex_unlock(mutex); // notify output function pthread_cond_signal((pthread_cond_t*)ssGetPWorkValue(S, COND_VAR)); return 0; }
/* Function: mdlOutputs ======================================================= * Abstract: * y = Cx + Du */ static void mdlOutputs(SimStruct *S, int_T tid) { // output vectors real_T *accel_out = ssGetOutputPortRealSignal(S,0); real_T *compass_out = ssGetOutputPortRealSignal(S,1); real_T *gyro_out = ssGetOutputPortRealSignal(S,2); uint16_T *ADC_out = (uint16_T *)ssGetOutputPortSignal(S,3); // counter int i = 0; #if defined(__linux) // vector of return values unsigned short ret_values[3]; I2CDevice *accel_p, *magneto_p, *gyro_p; ADCDevice adc_p; // get the accelerometer pointer from PWork accel_p = (I2CDevice*)ssGetPWorkValue(S, 1); // read the data register, 3 axis x 2 bytes/axis = 6 bytes accel_p->read6Reg((void *)0x28, ret_values); // output the accel data, properly scaled in +/- 2g maximum accel_out[0] = ((double)((short)ret_values[0]))/16384.0; accel_out[1] = ((double)((short)ret_values[1]))/16384.0; accel_out[2] = ((double)((short)ret_values[2]))*(2.0/32767.0); // get the magnetometer pointer from PWork magneto_p = (I2CDevice*)ssGetPWorkValue(S, 2); // read the data register, 3 axis x 2 bytes/axis = 6 bytes magneto_p->read6Reg((void *)0x03, ret_values); // output the magnetometer data, properly scaled in +/- Gauss (1.3 maximum) compass_out[0] = ((double)((short)ret_values[0]))/1055.0*1.3; compass_out[1] = ((double)((short)ret_values[1]))/1055.0*1.3; compass_out[2] = ((double)((short)ret_values[2]))/950.0*1.3; // get the gyro pointer from PWork gyro_p = (I2CDevice*)ssGetPWorkValue(S, 3); // read the data register, 3 axis x 2 bytes/axis = 6 bytes gyro_p->read6Reg((void*)0x28, ret_values); // output the gyro data, properly scaled in degrees/s (maximum 250) gyro_out[0] = ((double)((short)ret_values[0]))*(250.0/32767.0); gyro_out[1] = ((double)((short)ret_values[1]))*(250.0/32767.0); gyro_out[2] = ((double)((short)ret_values[2]))*(250.0/32767.0); // get the ADC pointer from PWork adc_p = (ADCDevice*)ssGetPWorkValue(S, 6); // output the ADC values for (i=0;i<8;i++) { // don't do channel 4 if (i != 4) { ADC_out[i] = adc_p->readADC(i); } } #endif }
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 int hackrf_tx_callback(hackrf_transfer *transfer) /* ======================================================================== */ { SimStruct *S = transfer->tx_ctx; SampleBuffer *sbuf = ssGetPWorkValue(S, SBUF); if (transfer->valid_length != BUFFER_SIZE) { sbuf->error = SB_SIZE_MISSMATCH; return -1; } if (sbuf->startup_skip) { memset(transfer->buffer, 0, (size_t) transfer->valid_length); sbuf->startup_skip--; } else if (sbuf->ready) { memcpy(transfer->buffer, sbuf->buffers[sbuf->head], BUFFER_SIZE); if (++sbuf->head >= NUMBER_OF_BUFFERS) sbuf->head = 0; pthread_mutex_lock(&sbuf->mutex); sbuf->ready--; pthread_cond_signal(&sbuf->cond_var); pthread_mutex_unlock(&sbuf->mutex); } else { /* underrun, no buffers ready */ memset(transfer->buffer, 0, (size_t) transfer->valid_length); sbuf->error = SB_UNDERRUN; sbuf->had_error = true; } return 0; }
/* Function: mdlOutputs ======================================================= * */ static void mdlOutputs(SimStruct *S, int_T tid) { uint8_T *y0 = (uint8_T *)ssGetOutputPortRealSignal(S,0); const int_T y_width = ssGetOutputPortWidth(S,0); int i; char* buffer[1]; int connected=ssGetIWorkValue(S, 0); int curCon = ssGetIWorkValue(S, 2); char* last = (void*) ssGetPWorkValue(S, 1); if(connected==1) { fcntl(curCon, F_SETFL, O_NONBLOCK); if(recv(curCon,buffer, 1, MSG_PEEK)>0) { read(curCon,last,y_width); /*printf("%.*s\n",y_width,last);*/ } } else { connected=tryConnect(S); if (connected) printf("Connected To Client\n"); } /*fflush(stdout);*/ for(i=0;i<y_width;i++) y0[i]=last[i]; ssSetIWorkValue(S, 0, connected); /*ssSetPWorkValue(S, 0, (void*) newsockfd);*/ }
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."); } }
/* Function: mdlTerminate ===================================================== * Abstract: * No termination needed, but we are required to have this routine. */ static void mdlTerminate(SimStruct *S) { #if defined(__linux) Robovero *robo1; I2CDevice *accel_p, *magneto_p, *gyro_p; PWMDevice *servo1_p, *servo4_p; ADCDevice *adc_p; // get all the pointers from PWork robo1 = (Robovero*)ssGetPWorkValue(S, 0); accel_p = (I2CDevice*)ssGetPWorkValue(S, 1); magneto_p = (I2CDevice*)ssGetPWorkValue(S, 2); gyro_p = (I2CDevice*)ssGetPWorkValue(S, 3); servo1_p = (PWMDevice*)ssGetPWorkValue(S, 4); servo1_p = (PWMDevice*)ssGetPWorkValue(S, 5); adc_p = (ADCDevice*)ssGetPWorkValue(S, 6); // remove from memory and shutdown delete adc_p; delete servo4_p; delete servo1_p; delete gyro_p; delete magneto_p; delete accel_p; delete robo1; #endif UNUSED_ARG(S); /* unused input argument */ }
static void mdlTerminate(SimStruct * S) { void *buffer = ssGetPWorkValue(S, 0); int id = ssGetIWorkValue(S, 0); udpClose(id); if (buffer != NULL) { free(buffer); } }
static void mdlTerminate(SimStruct *S) { #ifdef CO_DEBUG printf(" co_udp_receiver :: mdlTerminate\n"); #endif struct my_data_t *my_data = (struct my_data_t *) ssGetPWorkValue(S,0); udp_marker_rec_close(my_data); delete my_data; }
void mdlProcessParameters(SimStruct *S) /* ========================================================================*/ { if(!ssGetPWorkValue(S, DEVICE)) return; Hackrf_set_param(S, hackrf_set_freq, uint64_t, FREQUENCY, "Failed to set center frequency"); Hackrf_set_param(S, hackrf_set_txvga_gain, uint32_t, TXVGA_GAIN, "Failed to set TXVGA gain (range 0-47 step 1db)"); }
/* Function: mdlUpdate ======================================================== * Abstract: * This function is called once for every major integration time step. * Discrete states are typically updated here, but this function is useful * for performing any tasks that should only take place once per integration * step. */ static void mdlUpdate(SimStruct *S, int_T tid) { // get PWM motor input commands InputRealPtrsType servo_cmd = ssGetInputPortRealSignalPtrs(S,0); #if defined(__linux) // get the PWM device pointers from PWork vector PWMDevice *servo1_p, *servo4_p; servo1_p = (PWMDevice*)ssGetPWorkValue(S, 4); servo4_p = (PWMDevice*)ssGetPWorkValue(S, 5); // apply the command to the motors, casting it as uint // have to test out servo/motor range to find possible input range servo1_p->move((unsigned int)*servo_cmd[0]); servo4_p->move((unsigned int)*servo_cmd[3]); #endif }
void mdlSimStatusChange(SimStruct *S, ssSimStatusChangeType simStatus) /* ========================================================================*/ { if (simStatus == SIM_PAUSE) { SampleBuffer *sbuf = ssGetPWorkValue(S, SBUF); if (sbuf->had_error) ssPrintf("\n"); stopHackRf(S, DEVICE); } else if (simStatus == SIM_CONTINUE) startHackrfTx(S, false); }
static void mdlTerminate(SimStruct *S) { #ifndef MATLAB_MEX_FILE MBX *mbx = (MBX *)ssGetPWorkValue(S,0); #ifdef KRTAI mbx_rt_mbx_delete(mbx); #else rt_mbx_delete(mbx); #endif #endif }
static void mdlInitializeConditions(SimStruct *S) { if (ssGetNumPWork(S) > 0) { wbt::Block *block = static_cast<wbt::Block*>(ssGetPWorkValue(S, 0)); wbt::Error error; if (!block || !block->initializeInitialConditions(S, &error)) { static char errorBuffer[1024]; sprintf(errorBuffer, "[mdlInitializeConditions]%s", error.message.substr(0, 1023 - strlen("[mdlInitializeConditions]")).c_str()); ssSetErrorStatus(S, errorBuffer); } } }
static void mdlOutputs(SimStruct * S, int_T tid) { int_T id = ssGetIWorkValue(S, 0); int_T noBytes = ssGetIWorkValue(S, 1); int_T count; uint8_T *buffer = (uint8_T *)ssGetPWorkValue(S, 0); count = udpReceive(id, buffer, noBytes); *(ssGetOutputPortRealSignal(S, 1)) = !count; if (count == 0) memcpy((void *) ssGetOutputPortSignal(S, 0), (void *) (uint8_T *) buffer, noBytes); }
static void mdlOutputs(SimStruct *S,int_T tid) { real_T *y; #ifndef MATLAB_MEX_FILE int i; rosShmData_t *shm = (rosShmData_t *)ssGetPWorkValue(S,0); SEM *sem = (SEM *)ssGetPWorkValue(S,1); if (rt_sem_wait_if(sem) != 0) { y = ssGetOutputPortRealSignal(S,0); for (i = 0; i < shm->length; ++i) { y[i] = shm->data[i]; } if (ssGetNumOutputPorts(S) > 1) { y = ssGetOutputPortRealSignal(S,1); y[0] = shm->header.time; y[1] = shm->header.seq; } rt_sem_signal(sem); } #endif UNUSED_ARG(tid); /* not used in single tasking mode */ }
/* ======================================================================== */ 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); } }
static void mdlUpdate(SimStruct *S, int_T tid) { UNUSED_ARG(tid); if (ssGetNumPWork(S) > 0) { wbt::Block *block = static_cast<wbt::Block*>(ssGetPWorkValue(S, 0)); wbt::Error error; if (!block || !block->updateDiscreteState(S, &error)) { static char errorBuffer[1024]; sprintf(errorBuffer, "[mdlOutputs]%s", error.message.substr(0, 1023 - strlen("[mdlOutputs]")).c_str()); ssSetErrorStatus(S, errorBuffer); } } }
/* ======================================================================== */ static void mdlTerminate(SimStruct *S) /* ======================================================================== */ { /* check if HackRF object has been created */ if (ssGetPWorkValue(S, DEVICE)) { struct hackrf_device *device = (struct hackrf_device *)ssGetPWorkValue(S, DEVICE); hackrf_stop_tx(device); hackrf_close(device); hackrf_exit(); } /* release thread stuff */ if (ssGetPWorkValue(S, MUTEX)) { pthread_mutex_t *mutex = (pthread_mutex_t *)ssGetPWorkValue(S, MUTEX); pthread_mutex_destroy(mutex); free(mutex); mutex = NULL; } if (ssGetPWorkValue(S, COND_VAR)) { pthread_cond_t* cond_var = (pthread_cond_t *)ssGetPWorkValue(S, COND_VAR); pthread_cond_destroy(cond_var); free(cond_var); cond_var = NULL; } /* destroy sample buffer struct */ if (ssGetPWorkValue(S, SBUF)) { SampleBuffer *sbuf = (SampleBuffer *)ssGetPWorkValue(S, SBUF); if (sbuf->underrun_before) { ssPrintf("\n"); } if (sbuf->buf) { for (unsigned int i = 0; i < sbuf->num; ++i) { if (sbuf->buf[i]) free(sbuf->buf[i]); } free(sbuf->buf); } free(sbuf); } }
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 mdlEnable(SimStruct *S) { #ifndef MATLAB_MEX_FILE const real_T *initVal = PARAM(2); int_T initValLen = PARAM_SIZE(2); bool reset = PARAM(4)[0]; int i; rosShmData_t *shm = (rosShmData_t *)ssGetPWorkValue(S,0); SEM *sem = (SEM *)ssGetPWorkValue(S,1); if (!reset) return; if (rt_sem_wait_if(sem) != 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); } #endif }
static void mdlTerminate(SimStruct * S) { void *buffer = ssGetPWorkValue(S, 0); int id = ssGetIWorkValue(S, 0); /* stop the thread (FIRST) */ run_pthread = 0; #ifdef __linux__ usleep(thread_usleep); if (dc1394_stop_iso_transmission(raw1394Handle,dc1394Camera.node) != DC1394_SUCCESS) return; cleanup(); #endif if (buffer != NULL) free(buffer); }
static void mdlOutputs (SimStruct *S, int_T tid) { bus_t *c = (bus_t*) ssGetPWorkValue(S,0); /* Входные порты */ c->rn0 = *(boolean_T*) ssGetInputPortSignal (S,0); c->rn1 = *(boolean_T*) ssGetInputPortSignal (S,1); c->rn2 = *(boolean_T*) ssGetInputPortSignal (S,2); c->rn3 = *(boolean_T*) ssGetInputPortSignal (S,3); bus_step (c); /* Выходные порты */ *(boolean_T*) ssGetOutputPortSignal (S,0) = c->tx; }
/* 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 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 int tryConnect(SimStruct *S) { struct sockaddr_in* cli_addr=(void*)ssGetPWorkValue(S, 0); int curCon,accCon=ssGetIWorkValue(S, 1),clilen = sizeof(cli_addr); listen(accCon,5); curCon = accept(accCon,(struct sockaddr *) &cli_addr,&clilen); ssSetIWorkValue(S, 2, curCon); if (curCon < 0) return 0; else { return 1; } }
static void mdlOutputs(SimStruct *S, int_T tid) { InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0); float data; #ifndef MATLAB_MEX_FILE MBX *mbx = (MBX *)ssGetPWorkValue(S,0); data = (float)*uPtrs[0]; if (data < -1.0) data = -1.0; if (data > 1.0) data = 1.0; #ifdef KRTAI mbx_rt_mbx_send_if(mbx, &data, sizeof(data)); #else rt_mbx_send_if(mbx, &data, sizeof(data)); #endif #endif }
/* 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); } } } }