static void mdlStart(SimStruct *S)
{
char string_aux[255]; // auxiliar string
char serial_port_path[255]; // auxiliar string
char my_port[255];
int baud_rate;
comm_settings comm_settings_t;
ssPrintf("qbmove simulink library version: %s\n", QBMOVE_SIMULINK_VERSION);
//====================================================== opening serial port
for (int i = 0; param_com_port(i); ++i)
serial_port_path[i] = (char) param_com_port(i);
switch(param_com_baudrate){
case 1:
baud_rate = BAUD_RATE_2000000;
break;
case 2:
baud_rate = BAUD_RATE_460800;
break;
case 3:
baud_rate = BAUD_RATE_115200;
break;
case 4:
baud_rate = BAUD_RATE_57600;
break;
}
#if defined(_WIN32) || defined(_WIN64)
sprintf(my_port, "\\\\.\\%s", serial_port_path);
#else
strcpy(my_port, serial_port_path);
#endif
openRS485(&comm_settings_t, my_port, baud_rate);
pwork_handle = comm_settings_t.file_handle;
#if defined(_WIN32) || defined(_WIN64)
if(pwork_handle == INVALID_HANDLE_VALUE)
#else
if(pwork_handle == -1)
#endif
{
ssPrintf("Check your COM port. \nCould not connect to %s\n", serial_port_path);
out_handle = &pwork_handle;
// Stop simulation
mexEvalString("set_param(bdroot, 'SimulationCommand', 'stop')");
return;
}
out_handle = &pwork_handle;
}
/* == FUNCTION mdlTerminate ===================================================
* Called by Simulink whenever the simulation finishes.
*
* Closes handles, unlinks FIFOs, resets model and checks errno.
*/
static void mdlTerminate(SimStruct *S) {
//-- Signal death, if not dead --------------------------------------------//
if (sim_running) {
write(hs_input_fd, P_DIE, 1);
Protocol_destroy(protocol);
}
//-- Reset everything -----------------------------------------------------//
delta_time = MINIMUM_DELTA;
sim_running = false;
free(intermediate);
/* Close file descriptors, not terribly interested if this succeeds
* or not as we're done anyway.
*/
close(hs_input_fd);
close(hs_output_fd);
unlink(hs_input_fifo);
unlink(hs_output_fifo);
#ifndef NDEBUG
ssPrintf("= Unlinked FIFOs and reset model.\n");
#endif
}
/* Function: mdlInitializeSampleTimes =========================================
* Abstract:
* Port based sample times have already been configured, therefore this
* method doesn't need to perform any action (you can check the
* current port sample times).
*/
static void mdlInitializeSampleTimes(SimStruct *S)
{
#if 0 /* set to 1 to see port sample times */
const char_T *bpath = ssGetPath(S);
int_T i;
for (i = 0; i < NINPUTS; i++) {
ssPrintf("%s input port %d sample time = [%g, %g]\n", bpath, i,
ssGetInputPortSampleTime(S,i),
ssGetInputPortOffsetTime(S,i));
}
for (i = 0; i < NOUTPUTS; i++) {
ssPrintf("%s output port %d sample time = [%g, %g]\n", bpath, i,
ssGetOutputPortSampleTime(S,i),
ssGetOutputPortOffsetTime(S,i));
}
#endif
ssSetModelReferenceSampleTimeDefaultInheritance(S);
} /* end mdlInitializeSampleTimes */
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);
}
/* ======================================================================== */
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);
}
}
void mdlSetInputPortWidth(SimStruct *S, int portIndex, int width)
{
if (width != protocol->p_input_width) {
ssPrintf("AUTOSAR model expects an input port width of %u\n"
"Tried to set %u\n", protocol->p_input_width, width);
ssSetErrorStatus(S, "Tried to set port width not matching that specified"
"by AUTOSAR model");
return;
}
ssSetInputPortWidth(S, portIndex, width);
return;
}
/* ======================================================================== */
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 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;
}
}
int
main(int argc, char * argv[])
{
RT_MODEL * S;
const char * status;
int_T count;
int exit_code = exit_success;
boolean_T parseError = FALSE;
real_T final_time = -2; /* Let model select final time */
int scheduling_priority;
struct qsched_param scheduling;
t_period timeout;
t_timer_notify notify;
t_error result;
/*
* Make controller threads higher priority than external mode threads:
* ext_priority = priority of lowest priority external mode thread
* min_priority = minimum allowable priority of lowest priority model task
* max_priority = maximum allowable priority of lowest priority model task
*/
int ext_priority = qsched_get_priority_min(QSCHED_FIFO);
int min_priority = ext_priority + 2;
int max_priority = qsched_get_priority_max(QSCHED_FIFO) - 0;
qsigset_t signal_set;
qsigaction_t action;
int_T stack_size = 0; /* default stack size */
(void) ssPrintf("Entered main(argc=%d, argv=%p)\n", argc, argv);
for (count = 0; count < argc; count++) {
(void) ssPrintf(" argv[%d] = %s\n", count, argv[count]);
}
scheduling_priority = 2; /* default priority */
if (scheduling_priority < min_priority)
scheduling_priority = min_priority;
else if (scheduling_priority > max_priority)
scheduling_priority = max_priority;
/*
* Parse the standard RTW parameters. Let all unrecognized parameters
* pass through to external mode for parsing. NULL out all args handled
* so that the external mode parsing can ignore them.
*/
for (count = 1; count < argc; ) {
const char *option = argv[count++];
char extraneous_characters[2];
if ((strcmp(option, "-tf") == 0) && (count != argc)) {/* final time */
const char * tf_argument = argv[count++];
double time_value; /* use a double for the sscanf since real_T may be a float or a double depending on the platform */
if (strcmp(tf_argument, "inf") == 0) {
time_value = RUN_FOREVER;
} else {
int items = sscanf(tf_argument, "%lf%1s", &time_value,
extraneous_characters);
if ((items != 1) || (time_value < 0.0) ) {
(void) fprintf(stderr,
"final_time must be a positive, real value or inf.\n");
parseError = true;
break;
}
}
final_time = (real_T) time_value;
argv[count-2] = NULL;
argv[count-1] = NULL;
} else if ((strcmp(option, "-pri") == 0) && (count != argc)) {/* base priority */
const char * tf_argument = argv[count++];
int priority; /* use an int for the sscanf since int_T may be the wrong size depending on the platform */
int items = sscanf(tf_argument, "%d%1s", &priority, extraneous_characters);
if ((items != 1) || (priority < min_priority) ) {
(void) fprintf(stderr,
"priority must be a greater than or equal to %d.\n",
min_priority);
parseError = true;
break;
}
if (priority > max_priority) {
(void) fprintf(stderr, "priority must be less than or equal to %d.\n",
max_priority);
parseError = true;
break;
}
scheduling_priority = priority;
argv[count-2] = NULL;
argv[count-1] = NULL;
} else if ((strcmp(option, "-ss") == 0) && (count != argc)) {/* stack size */
const char * stack_argument = argv[count++];
int stack; /* use an int for the sscanf since int_T may be the wrong size depending on the platform */
int items = sscanf(stack_argument, "%d%1s", &stack, extraneous_characters);
if ((items != 1) || (stack < QTHREAD_STACK_MIN) ) {
(void) fprintf(stderr,
"stack size must be a integral value greater than or equal to %d.\n",
QTHREAD_STACK_MIN);
parseError = true;
break;
}
stack_size = (int_T)stack;
argv[count-2] = NULL;
argv[count-1] = NULL;
} else if ((strcmp(option, "-d") == 0) && (count != argc)) {/* current directory */
const char * path_name = argv[count++];
_chdir(path_name);
argv[count-2] = NULL;
argv[count-1] = NULL;
}
}
rtExtModeQuarcParseArgs(argc, (const char **) argv, "shmem://Crane:1");
/*
* Check for unprocessed ("unhandled") args.
*/
for (count = 1; count < argc; count++) {
if (argv[count] != NULL) {
(void) fprintf(stderr, "Unexpected command line argument: \"%s\".\n",
argv[count]);
parseError = TRUE;
}
}
if (parseError) {
(void) fprintf(stderr,
"\nUsage: Crane -option1 val1 -option2 val2 -option3 ...\n\n");
(void) fprintf(stderr,
"\t-tf 20 - sets final time to 20 seconds\n");
(void) fprintf(stderr,
"\t-d C:\\data - sets current directory to C:\\data\n");
(void) fprintf(stderr,
"\t-pri 5 - sets the minimum thread priority\n");
(void) fprintf(stderr,
"\t-ss 65536 - sets the stack size for model threads\n");
(void) fprintf(stderr,
"\t-w - wait for host to connect before starting\n");
(void) fprintf(stderr,
"\t-uri shmem://mymodel - set external mode URL to \"shmem://mymodel\"\n");
(void) fprintf(stderr, "\n");
return (exit_failure);
}
/****************************
* Initialize global memory *
****************************/
(void)memset(&GBLbuf, 0, sizeof(GBLbuf));
/************************
* Initialize the model *
************************/
rt_InitInfAndNaN(sizeof(real_T));
S = Crane();
if (rtmGetErrorStatus(S) != NULL) {
(void) fprintf(stderr, "Error during model registration: %s\n",
rtmGetErrorStatus(S));
return (exit_failure);
}
if (final_time >= 0.0 || final_time == RUN_FOREVER) {
rtmSetTFinal(S, final_time);
} else {
rtmSetTFinal(S, rtInf);
}
action.sa_handler = control_c_handler;
action.sa_flags = 0;
qsigemptyset(&action.sa_mask);
qsigaction(SIGINT, &action, NULL);
qsigaction(SIGBREAK, &action, NULL);
qsigemptyset(&signal_set);
qsigaddset(&signal_set, SIGINT);
qsigaddset(&signal_set, SIGBREAK);
qthread_sigmask(QSIG_UNBLOCK, &signal_set, NULL);
initialize_sizes(S);
initialize_sample_times(S);
status = rt_SimInitTimingEngine(rtmGetNumSampleTimes(S),
rtmGetStepSize(S),
rtmGetSampleTimePtr(S),
rtmGetOffsetTimePtr(S),
rtmGetSampleHitPtr(S),
rtmGetSampleTimeTaskIDPtr(S),
rtmGetTStart(S),
&rtmGetSimTimeStep(S),
&rtmGetTimingData(S));
if (status != NULL) {
(void) fprintf(stderr, "Failed to initialize sample time engine: %s\n",
status);
return (exit_failure);
}
rt_CreateIntegrationData(S);
fflush(stdout);
if (rtExtModeQuarcStartup(rtmGetRTWExtModeInfo(S),
rtmGetNumSampleTimes(S),
&rtmGetStopRequested(S),
ext_priority, /* external mode thread priority */
stack_size,
SS_HAVESTDIO)) {
(void) ssPrintf("\n** starting the model **\n");
start(S);
if (rtmGetErrorStatus(S) == NULL) {
/*************************************************************************
* Execute the model.
*************************************************************************/
if (rtmGetTFinal(S) == RUN_FOREVER) {
(void) ssPrintf("\n**May run forever. Model stop time set to infinity.**\n");
}
timeout.seconds = (t_long) (rtmGetStepSize(S));
timeout.nanoseconds = (t_int) ((rtmGetStepSize(S) - timeout.seconds) *
1000000000L);
result = qtimer_event_create(¬ify.notify_value.event);
if (result == 0) {
t_timer timer;
scheduling.sched_priority = scheduling_priority;
qthread_setschedparam(qthread_self(), QSCHED_FIFO, &scheduling);
notify.notify_type = TIMER_NOTIFY_EVENT;
result = qtimer_create(¬ify, &timer);
if (result == 0) {
result = qtimer_begin_resolution(timer, &timeout);
if (result == 0) {
t_period actual_timeout;
(void) ssPrintf("Creating main thread with priority %d and period %g...\n",
scheduling_priority, rtmGetStepSize(S));
result = qtimer_get_actual_period(timer, &timeout, &actual_timeout);
if (result == 0 && (timeout.nanoseconds !=
actual_timeout.nanoseconds || timeout.seconds !=
actual_timeout.seconds))
(void) ssPrintf("*** Actual period will be %g ***\n",
actual_timeout.seconds + 1e-9 *
actual_timeout.nanoseconds);
fflush(stdout);
result = qtimer_set_time(timer, &timeout, true);
if (result == 0) {
/* Enter the periodic loop */
while (result == 0) {
if (GBLbuf.stopExecutionFlag || rtmGetStopRequested(S)) {
break;
}
if (rtmGetTFinal(S) != RUN_FOREVER && rtmGetTFinal(S) - rtmGetT
(S) <= rtmGetT(S)*DBL_EPSILON) {
break;
}
if (qtimer_get_overrun(timer) > 0) {
(void) fprintf(stderr,
"Sampling rate is too fast for base rate\n");
fflush(stderr);
}
rt_OneStep(S);
result = qtimer_event_wait(notify.notify_value.event);
}
/* disarm the timer */
qtimer_cancel(timer);
if (rtmGetStopRequested(S) == false && rtmGetErrorStatus(S) ==
NULL) {
/* Execute model last time step if final time expired */
rt_OneStep(S);
}
(void) ssPrintf("Main thread exited\n");
} else {
msg_get_error_messageA(NULL, result, GBLbuf.submessage, sizeof
(GBLbuf.submessage));
string_format(GBLbuf.message, sizeof(GBLbuf.message),
"Unable to set base rate. %s", GBLbuf.submessage);
rtmSetErrorStatus(S, GBLbuf.message);
}
qtimer_end_resolution(timer);
} else {
msg_get_error_messageA(NULL, result, GBLbuf.submessage, sizeof
(GBLbuf.submessage));
string_format(GBLbuf.message, sizeof(GBLbuf.message),
"Sampling period of %lg is too fast for the system clock. %s",
rtmGetStepSize(S), GBLbuf.submessage);
rtmSetErrorStatus(S, GBLbuf.message);
}
qtimer_delete(timer);
} else {
msg_get_error_messageA(NULL, result, GBLbuf.submessage, sizeof
(GBLbuf.submessage));
string_format(GBLbuf.message, sizeof(GBLbuf.message),
"Unable to create timer for base rate. %s",
GBLbuf.submessage);
rtmSetErrorStatus(S, GBLbuf.message);
}
} else {
msg_get_error_messageA(NULL, result, GBLbuf.submessage, sizeof
(GBLbuf.submessage));
string_format(GBLbuf.message, sizeof(GBLbuf.message),
"Unable to create timer event for base rate. %s",
GBLbuf.submessage);
rtmSetErrorStatus(S, GBLbuf.message);
}
GBLbuf.stopExecutionFlag = 1;
}
} else {
rtmSetErrorStatus(S, "Unable to initialize external mode.");
}
rtExtSetReturnStatus(rtmGetErrorStatus(S));
rtExtModeQuarcCleanup(rtmGetNumSampleTimes(S));
/********************
* Cleanup and exit *
********************/
if (rtmGetErrorStatus(S) != NULL) {
(void) fprintf(stderr, "%s\n", rtmGetErrorStatus(S));
exit_code = exit_failure;
}
(void) ssPrintf("Invoking model termination function...\n");
terminate(S);
(void) ssPrintf("Exiting real-time code\n");
return (exit_code);
}
void init_protocol(SimStruct *S)
{
/* Open file descriptors.
*
* NOTE: This must be done /after/ the fork, or there'll be issues with
* the forked process. Likely, these things are inherited.
*/
hs_input_fd = open(hs_input_fifo, O_WRONLY);
hs_output_fd = open(hs_output_fifo, O_RDONLY);
if (hs_input_fd < 0 || hs_output_fd < 0) {
ssSetErrorStatus(S, "Error opening file descriptor.");
return;
}
//-- Init protocol struct and call handshake procedure ----------------//
protocol = Protocol_init(hs_input_fd, hs_output_fd);
if (protocol == NULL) {
ssPrintf("Protocol_init returned NULL pointer.\n");
ssSetErrorStatus (S, "Protocol_init: PROTOCOL_MEM_ERROR");
return;
}
switch(Protocol_handshake(protocol)) {
case PROTOCOL_MEM_ERROR:
free(protocol);
ssSetErrorStatus(S, "Protocol_handshake: PROTOCOL_MEM_ERROR");
return;
case MEM_ERROR:
free(protocol);
ssSetErrorStatus(S, "Protocol_handshake: MEM_ERROR");
return;
case PROTOCOL_ERROR:
free(protocol);
ssSetErrorStatus(S, "Protocol_handshake: PROTOCOL_ERROR");
return;
case PROTOCOL_SUCCESS:
default: ;
}
/* Set mask port labels.
* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
*
* XXX NOTE:
* Decided on a fixed command size of 256 until I figure this out. If
* a 256-byte string is used the null-termination imposed by calloc
* will fail and we'll get a segfault.
* XXX
*/
const char *block_label = ssGetPath(S);
#ifdef MATLAB_MEX_FILE
/* This code adds labels to the model and prevents
* generated C code from compiling */
for (uint8_t i = 0; i < protocol->p_input_labels; i++) {
char *cmd = calloc(256, sizeof(char));
sprintf(cmd, "setMaskLabel('%s', '%s', %u, '%s', %u);",
block_label, protocol->p_input_labels_str[i], i + 1,
"input", i == 0);
mexEvalString(cmd);
free(cmd);
}
for (uint8_t i = 0; i < protocol->p_output_labels; i++) {
char *cmd = calloc(256, sizeof(char));
sprintf(cmd, "setMaskLabel('%s', '%s', %u, '%s', %u);",
block_label, protocol->p_output_labels_str[i], i + 1,
"output", 0);
mexEvalString(cmd);
free(cmd);
}
#endif
/* - Set up some intermediate storage (MATLAB crux).
* - Run the simulator one step (need some outputs to be ready since
* Simulink updates the model in a backwards order).
*
* TODO: Error handling for calloc
*/
intermediate = calloc(protocol->p_input_width, sizeof(double));
Protocol_send_data(protocol, intermediate);
sim_running = true;
}
void report(const char* message, SimStruct *S)
{
ssPrintf("%f: %s (%s).\n", ssGetTime(S), message, SimState(S));
}
