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); }
