void SDK_matlabMainLoop()
{
static unsigned short uart_count = 1; //counter for uart communication
/* put your own c-code here */
rt_OneStep(); //call RTW function rt_OneStep
//ctrl_mode is set in rt_one_step
/* put your own c-code here */
//don't touch anything below here
//debug packet handling
if (uart_count==0 && xbee_send_flag) //call function for uart transmission with 50 Hz
{
matlab_debug.cpu_load = HL_Status.cpu_load;
matlab_debug.battery_voltage = HL_Status.battery_voltage_1;
UART_Matlab_SendPacket(&matlab_debug, sizeof(matlab_debug), 'd');
}
uart_count++;
uart_count%=ControllerCyclesPerSecond/50;
//save parameters only while not flying
if ((!RO_ALL_Data.flying) && (triggerSaveMatlabParams))
{
triggerSaveMatlabParams=0;
lpc_aci_SavePara();
lpc_aci_WriteParatoFlash();
}
}
/*
* The example "main" function illustrates what is required by your
* application code to initialize, execute, and terminate the generated code.
* Attaching rt_OneStep to a real-time clock is target specific. This example
* illustates how you do this relative to initializing the model.
*/
int_T main(int_T argc, const char *argv[])
{
/* Unused arguments */
(void)(argc);
(void)(argv);
/* Pack model data into RTM */
DroneRS_Compensator_M->ModelData.defaultParam = &DroneRS_Compensator_P;
DroneRS_Compensator_M->ModelData.blockIO = &DroneRS_Compensator_B;
DroneRS_Compensator_M->ModelData.dwork = &DroneRS_Compensator_DW;
/* Initialize model */
DroneRS_Compensator_initialize(DroneRS_Compensator_M,
&DroneRS_Compensator_U_controlModePosVSAtt_flagin,
DroneRS_Compensator_U_pos_refin, DroneRS_Compensator_U_attRS_refin,
&DroneRS_Compensator_U_ddx, &DroneRS_Compensator_U_ddy,
&DroneRS_Compensator_U_ddz, &DroneRS_Compensator_U_p,
&DroneRS_Compensator_U_q, &DroneRS_Compensator_U_r,
&DroneRS_Compensator_U_altitude_sonar, &DroneRS_Compensator_U_prs,
DroneRS_Compensator_U_opticalFlowRS_datin,
DroneRS_Compensator_U_sensordatabiasRS_datin,
DroneRS_Compensator_U_posVIS_datin, &DroneRS_Compensator_U_usePosVIS_flagin,
DroneRS_Compensator_U_batteryStatus_datin,
DroneRS_Compensator_Y_motorsRS_cmdout, &DroneRS_Compensator_Y_X,
&DroneRS_Compensator_Y_Y, &DroneRS_Compensator_Y_Z,
&DroneRS_Compensator_Y_yaw, &DroneRS_Compensator_Y_pitch,
&DroneRS_Compensator_Y_roll, &DroneRS_Compensator_Y_dx,
&DroneRS_Compensator_Y_dy, &DroneRS_Compensator_Y_dz,
&DroneRS_Compensator_Y_pb, &DroneRS_Compensator_Y_qb,
&DroneRS_Compensator_Y_rb,
&DroneRS_Compensator_Y_controlModePosVSAtt_flagout,
DroneRS_Compensator_Y_poseRS_refout, &DroneRS_Compensator_Y_ddxb,
&DroneRS_Compensator_Y_ddyb, &DroneRS_Compensator_Y_ddzb,
&DroneRS_Compensator_Y_pa, &DroneRS_Compensator_Y_qa,
&DroneRS_Compensator_Y_ra, &DroneRS_Compensator_Y_altitude_sonarb,
&DroneRS_Compensator_Y_prsb, DroneRS_Compensator_Y_opticalFlowRS_datout,
DroneRS_Compensator_Y_sensordatabiasRS_datout,
DroneRS_Compensator_Y_posVIS_datout,
&DroneRS_Compensator_Y_usePosVIS_flagout,
DroneRS_Compensator_Y_batteryStatus_datout);
/* The MAT-file logging option selected; therefore, simulating
* the model step behavior (in non real-time). Running this
* code produces results that can be loaded into MATLAB.
*/
while (rtmGetErrorStatus(DroneRS_Compensator_M) == (NULL)) {
rt_OneStep(DroneRS_Compensator_M);
}
/* Matfile logging */
rt_StopDataLogging(MATFILE, DroneRS_Compensator_M->rtwLogInfo);
/* Disable rt_OneStep() here */
return 0;
}
interrupt void TINT0_isr(void)
{
volatile unsigned int PIEIER1_stack_save = PieCtrlRegs.PIEIER1.all;
PieCtrlRegs.PIEIER1.all &= ~64; /*disable group1 lower/equal priority interrupts*/
asm(" RPT #5 || NOP"); /*wait 5 cycles */
IFR &= ~1; /*eventually disable lower/equal priority pending interrupts*/
PieCtrlRegs.PIEACK.all = 1; /*ACK to allow other interrupts from the same group to fire*/
IER |= 1;
EINT; /*global interrupt enable*/
rt_OneStep();
DINT; /* disable global interrupts during context switch, CPU will enable global interrupts after exiting ISR */
PieCtrlRegs.PIEIER1.all = PIEIER1_stack_save;/*restore PIEIER register that was modified*/
}
int_T main(int_T argc, const char_T *argv[])
{
/* How many steps to run? */
if (argc != 2)
{
printf("Usage: microwave_type [STEPS]\n");
}
int num_steps = strtol(argv[1], NULL, 0);
if (errno || num_steps == 0)
{
perror("Error parsing num steps.\n");
exit(1);
}
/* Initialize model */
microwave_combined_initialize(1); // used to have an argument: 1.
/* Create symbolic inputs beforehand */
//make_input_symbolic();
int step_num;
ExternalInputs_microwave_combin inputs[num_steps];
int i;
for (i=0; i<num_steps; i++)
{
make_input_symbolic( &(inputs[i]));
}
/* Attach rt_OneStep to a timer or interrupt service routine with
* period 1.0 seconds (the model's base sample time) here. The
* call syntax for rt_OneStep is
*
* rt_OneStep();
*/
step_num = 0;
while (rtmGetErrorStatus(microwave_combined_M) == (NULL) && step_num < num_steps) {
/* Perform other application tasks here */
rt_OneStep( &(inputs[step_num]));
step_num++;
}
/* Disable rt_OneStep() here */
/* Terminate model */
microwave_combined_terminate();
return 0;
}
int main(void)
{
PLLFBD = 38; /* configure clock speed */
CLKDIV = 1;
/* Initialize model */
controlMCUSlugsMKIINewNav_initialize(1);
for (;;) {
/* Associate rt_OneStep() with a timer that executes at the base rate of the model */
while (!_T1IF) ;
_T1IF = 0;
rt_OneStep();
if (_T1IF)
CalculusTimeStep = PR1; /* Overload */
else
CalculusTimeStep = TMR1;
}
}
void Run_Localization(t_controller * x, int samples) {
struct timeval tim;
gettimeofday(&tim, NULL);
double t1=tim.tv_sec+(tim.tv_usec/1000000.0) - x->start_time;
memcpy(&(PHAT_U.Channel1[0]), &(x->channel[0][0]),
sizeof(float)*OUTPUT_BUFFER_SIZE);
memcpy(&(PHAT_U.Channel2[0]), &(x->channel[1][0]),
sizeof(float)*OUTPUT_BUFFER_SIZE);
memcpy(&(PHAT_U.Channel3[0]), &(x->channel[2][0]),
sizeof(float)*OUTPUT_BUFFER_SIZE);
memcpy(&(PHAT_U.Channel4[0]), &(x->channel[3][0]),
sizeof(float)*OUTPUT_BUFFER_SIZE);
memcpy(&(PHAT_U.Channel5[0]), &(x->channel[4][0]),
sizeof(float)*OUTPUT_BUFFER_SIZE);
memcpy(&(PHAT_U.Channel6[0]), &(x->channel[5][0]),
sizeof(float)*OUTPUT_BUFFER_SIZE);
memcpy(&(PHAT_U.Channel7[0]), &(x->channel[6][0]),
sizeof(float)*OUTPUT_BUFFER_SIZE);
//Calculate energy
double energy = 0;
int i;
for(i=0; i < OUTPUT_BUFFER_SIZE; i++) {
energy += pow(PHAT_U.Channel1[i],2);
}
if(energy < 0.7) return;
fwrite(&samples, sizeof(int), 1, x->scorefile);
float f_energy = (float) energy;
fwrite(&f_energy, sizeof(float), 1, x->scorefile);
//Calculate PHAT time-series
rt_OneStep();
int ret;
float maxVal;
ret = doSrpPhat(x, &maxVal);
gettimeofday(&tim, NULL);
double t2=tim.tv_sec+(tim.tv_usec/1000000.0) - x->start_time;
printf("time took: %2.2f ms\n", (t2-t1)*1000.0);
}
int main(void)
{
nrk_setup_ports();
volatile boolean_T noErr;
float modelBaseRate = 0.5;
float systemClock = 0;
SystemCoreClockUpdate();
UNUSED(modelBaseRate);
UNUSED(systemClock);
rtmSetErrorStatus(simulinkmodeltoblinkLed_M, 0);
simulinkmodeltoblinkLed_initialize();
/* No scheduler to configure */
;
noErr =
rtmGetErrorStatus(simulinkmodeltoblinkLed_M) == (NULL);
/* No interrupts to enable */
;
;
while (noErr ) {
rt_OneStep();
noErr =
rtmGetErrorStatus(simulinkmodeltoblinkLed_M) == (NULL);
}
/* Disable rt_OneStep() here */
/* Terminate model */
simulinkmodeltoblinkLed_terminate();
;
nrk_led_set (RED_LED);
return 0;
}
int main(void)
{
volatile boolean_T runModel = 1;
float modelBaseRate = 0.1;
float systemClock = 100;
SystemCoreClockUpdate();
UNUSED(modelBaseRate);
UNUSED(systemClock);
rtmSetErrorStatus(Controller_M, 0);
Controller_initialize();
runModel =
rtmGetErrorStatus(Controller_M) == (NULL);
while (runModel) {
rt_OneStep();
runModel =
rtmGetErrorStatus(Controller_M) == (NULL);
}
/* Disable rt_OneStep() here */
/* Terminate model */
Controller_terminate();
return 0;
}
void SDK_mainloop(void)
{
sdkCycleStartTime = T1TC;
WO_SDK.ctrl_mode = 0x02; // attitude and throttle control
WO_SDK.disable_motor_onoff_by_stick = 0;
sdkLoops++;
parseRxFifo();
// check for new LL command packet
if (packetCmdLL->updated)
{
cmdLLNew = 1;
packetCmdLL->updated = 0;
}
// check if LL commands arrive at max every CMD_MAX_PERIOD ms
if ((sdkLoops % CMD_MAX_PERIOD) == 0)
{
if (cmdLLNew == 1)
{
cmdLLNew = 0;
cmdLLValid++;
}
else
{
cmdLLValid--;
}
if (cmdLLValid < -2)
cmdLLValid = -2; // min 3 packets required
else if (cmdLLValid > 3)
cmdLLValid = 3; // fall back after 3 missed packets
}
// check for motor start/stop packet
if (packetMotors->updated)
{
motor_state = motors.motors;
motor_state_count = 0;
packetMotors->updated = 0;
}
// check for camera control commands
if (packetCamera->updated)
{
int cam_pitch = camera.desired_cam_pitch * 1000;
int cam_roll = camera.desired_cam_roll * 1000;
PTU_set_desired_pitch(cam_pitch);
PTU_set_desired_roll(cam_roll);
packetCamera->updated = 0;
}
// check for new HL command packet
if (packetCmdHL->updated)
{
packetCmdHL->updated = 0;
// SSDK operates in NED, need to convert from ENU
extPositionCmd.heading = -extPositionCmd.heading + 360000;
extPositionCmd.y = -extPositionCmd.y;
extPositionCmd.z = -extPositionCmd.z;
extPositionCmd.vY = -extPositionCmd.vY;
extPositionCmd.vZ = -extPositionCmd.vZ;
extPositionCmd.vYaw = -extPositionCmd.vYaw;
if (extPositionCmd.bitfield & EXT_POSITION_CMD_BODYFIXED)
{
float s_yaw, c_yaw;
c_yaw = approxCos((float)extPosition.heading / 1000 / 180 * M_PI);
s_yaw = approxCos(M_halfPI - (float)extPosition.heading / 1000 / 180 * M_PI);
if (extPositionCmd.bitfield & EXT_POSITION_CMD_VELOCITY)
{
float vx = extPositionCmd.vX;
float vy = extPositionCmd.vY;
extPositionCmd.vX = c_yaw * vx - s_yaw * vy;
extPositionCmd.vY = s_yaw * vx + c_yaw * vy;
}
else
{
float x = extPositionCmd.x;
float y = extPositionCmd.y;
extPositionCmd.x = c_yaw * x - s_yaw * y + extPosition.x;
extPositionCmd.y = s_yaw * x + c_yaw * y + extPosition.y;
extPositionCmd.z += extPosition.z;
extPositionCmd.heading += extPosition.heading;
if(extPositionCmd.heading > 360000)
extPositionCmd.heading -= 360000;
// should not happen ...
else if(extPositionCmd.heading < 0)
extPositionCmd.heading += 360000;
}
}
}
// handle parameter packet
if (packetSSDKParams->updated == 1)
{
packetSSDKParams->updated = 0;
statusData.have_SSDK_parameters = 1;
ssdk_status.have_parameters = 1;
}
// decide which position/state input we take for position control
// SSDK operates in NED --> convert from ENU
switch(hli_config.mode_state_estimation){
case HLI_MODE_STATE_ESTIMATION_HL_SSDK:
extPositionValid = 1;
extPosition.bitfield = 0;
extPosition.count = ext_position_update.count;
extPosition.heading = -ext_position_update.heading + 360000;
extPosition.x = ext_position_update.x;
extPosition.y = -ext_position_update.y;
extPosition.z = -ext_position_update.z;
extPosition.vX = ext_position_update.vX;
extPosition.vY = -ext_position_update.vY;
extPosition.vZ = -ext_position_update.vZ;
extPosition.qualX = ext_position_update.qualX;
extPosition.qualY = ext_position_update.qualY;
extPosition.qualZ = ext_position_update.qualZ;
extPosition.qualVx = ext_position_update.qualVx;
extPosition.qualVy = ext_position_update.qualVy;
extPosition.qualVz = ext_position_update.qualVz;
break;
case HLI_MODE_STATE_ESTIMATION_EXT:
extPositionValid = 1;
extPosition.bitfield = EXT_POSITION_BYPASS_FILTER;
extPosition.count = ext_position_update.count;
extPosition.heading = -ext_position_update.heading + 360000;
extPosition.x = ext_position_update.x;
extPosition.y = -ext_position_update.y;
extPosition.z = -ext_position_update.z;
extPosition.vX = ext_position_update.vX;
extPosition.vY = -ext_position_update.vY;
extPosition.vZ = -ext_position_update.vZ;
extPosition.qualX = ext_position_update.qualX;
extPosition.qualY = ext_position_update.qualY;
extPosition.qualZ = ext_position_update.qualZ;
extPosition.qualVx = ext_position_update.qualVx;
extPosition.qualVy = ext_position_update.qualVy;
extPosition.qualVz = ext_position_update.qualVz;
break;
case HLI_MODE_STATE_ESTIMATION_HL_EKF:
DEKF_step(&dekf, timestamp);
if(DEKF_getInitializeEvent(&dekf) == 1)
ssdk_reset_state = 1;
extPositionValid = 1;
break;
default:
extPositionValid = 0;
}
// dekf initialize state machine
// sets the acc/height/gps switch to 0 for 10 loops so that refmodel gets reset to the new state
if (ssdk_reset_state >= 1 && ssdk_reset_state < 10)
{
RO_RC_Data.channel[0] = 2048;
RO_RC_Data.channel[1] = 2048;
RO_RC_Data.channel[2] = 2048;
RO_RC_Data.channel[3] = 2048;
RO_RC_Data.channel[5] = 0;
ssdk_reset_state++;
}
else
{
ssdk_reset_state = 0;
}
// execute ssdk - only executed if ssdk parameters are available
// reads position reference from extPosition
// reads position/velocity command from extPositionCmd
// finally writes to WO_CTRL_Input. therefore, make sure to overwrite it after this call if you don't want to have its output
rt_OneStep();
// --- write commands to LL ------------------------------------------------
short motorsRunning = LL_1khz_attitude_data.status2 & 0x1;
if (motor_state == -1 || motor_state == 2)
{ // motors are either stopped or running --> normal operation
// commands are always written to LL by the Matlab controller, decide if we need to overwrite them
if (extPositionValid > 0 && statusData.have_SSDK_parameters == 1 && hli_config.mode_position_control == HLI_MODE_POSCTRL_HL)
{
WO_CTRL_Input.ctrl = hli_config.position_control_axis_enable;
WO_SDK.ctrl_enabled = 1;
// limit yaw rate:
if(WO_CTRL_Input.yaw > 1000)
WO_CTRL_Input.yaw = 1000;
else if(WO_CTRL_Input.yaw < -1000)
WO_CTRL_Input.yaw = -1000;
}
else if (cmdLLValid > 0 && (hli_config.mode_position_control == HLI_MODE_POSCTRL_LL || hli_config.mode_position_control == HLI_MODE_POSCTRL_OFF))
{
writeCommand(cmdLL.x, -cmdLL.y, -cmdLL.yaw, cmdLL.z, hli_config.position_control_axis_enable, 1);
}
else
{
writeCommand(0, 0, 0, 0, 0, 0);
}
}
// start / stop motors, allow commands max for 1.5 s
else if (motor_state == 1)
{
if (motor_state_count < 1500)
{
if (!motorsRunning)
writeCommand(0, 0, 2047, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1); // start/stop sequence, set ctrl to acc so that motors will be safely started
else if (motorsRunning)
{
writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
motor_state = 2;
}
}
else
{
writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
motor_state = -1;
}
motor_state_count++;
}
else if (motor_state == 0)
{
if (motor_state_count < 1500)
{
if (motorsRunning)
writeCommand(0, 0, 2047, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1); // start/stop sequence, set ctrl to acc so that motors will be safely shut down
else if (!motorsRunning)
{
writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
motor_state = -1;
}
}
else
{
writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
motor_state = -1;
}
motor_state_count++;
}
else
{
// undefined state, disable everything
writeCommand(0, 0, 0, 0, 0, 0);
}
// TODO: thrust limit in case something really goes wrong, may be removed
if (WO_CTRL_Input.thrust > 4095)
WO_CTRL_Input.thrust = 4095;
// ------------------------------------------------------------------------
// --- send data to UART 0 ------------------------------------------------
if (checkTxPeriod(subscription.imu))
{
sendImuData();
}
if (checkTxPeriod(subscription.rcdata))
{
sendRcData();
}
if (checkTxPeriod(subscription.gps))
{
sendGpsData();
}
if ((sdkLoops + 20) % 500 == 0)
{
sendStatus();
// writePacket2Ringbuffer(HLI_PACKET_ID_SSDK_STATUS, (unsigned char*)&ssdk_status, sizeof(ssdk_status));
}
if (checkTxPeriod(subscription.ssdk_debug))
{
ssdk_debug.timestamp = timestamp;
writePacket2Ringbuffer(HLI_PACKET_ID_SSDK_DEBUG, (unsigned char*)&ssdk_debug, sizeof(ssdk_debug));
}
if (checkTxPeriod(subscription.ekf_state))
{
// sendEkfState();
DEKF_sendState(&dekf, timestamp);
}
if (checkTxPeriod(subscription.mag))
{
sendMagData();
}
//
UART_send_ringbuffer();
synchronizeTime();
if (packetBaudrate->updated)
{
packetBaudrate->updated = 0;
while (!UART0_txEmpty())
;
}
// ------------------------------------------------------------------------
unsigned int dt;
if (T1TC < sdkCycleStartTime)
dt = (processorClockFrequency() - sdkCycleStartTime) + T1TC;
else
dt = T1TC - sdkCycleStartTime;
cpuLoad = ControllerCyclesPerSecond * ((dt * 1e2) / processorClockFrequency()); // cpu load in %
watchdog();
}
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 model_step(uint8_t timerID)
{
rt_OneStep();
// printf("got inside model_step /n");
/* Get model outputs here */
}
void SDK_mainloop(void)
{
sdkCycleStartTime = T1TC;
WO_SDK.ctrl_mode = 0x00; //0x00: absolute angle and throttle control
sdkLoops++;
parseRxFifo();
// check for new LL command packet
if (packetCmdLL->updated)
{
cmdLLNew = 1;
packetCmdLL->updated = 0;
}
// check if LL commands arrive at max every CMD_MAX_PERIOD ms
if ((sdkLoops % CMD_MAX_PERIOD) == 0)
{
if (cmdLLNew == 1)
{
cmdLLNew = 0;
cmdLLValid++;
}
else
{
cmdLLValid--;
}
if (cmdLLValid < -2)
cmdLLValid = -2; // min 3 packets required
else if (cmdLLValid > 3)
cmdLLValid = 3; // fall back after 3 missed packets
}
// check for motor start/stop packet
if (packetMotors->updated)
{
motor_state = motors.motors;
motor_state_count = 0;
packetMotors->updated = 0;
}
// check for new HL command packet
if (packetCmdHL->updated)
{
packetCmdHL->updated = 0;
// SSDK operates in NED, need to convert from ENU
extPositionCmd.heading = -extPositionCmd.heading + 360000;
extPositionCmd.y = -extPositionCmd.y;
extPositionCmd.z = -extPositionCmd.z;
extPositionCmd.vY = -extPositionCmd.vY;
extPositionCmd.vZ = -extPositionCmd.vZ;
extPositionCmd.vYaw = -extPositionCmd.vYaw;
}
// handle parameter packet
if (packetSSDKParams->updated == 1)
{
packetSSDKParams->updated = 0;
statusData.have_SSDK_parameters = 1;
ssdk_status.have_parameters = 1;
}
// decide which position/state input we take for position control
// SSDK operates in NED --> convert from ENU
switch(config.mode_state_estimation){
case HLI_MODE_STATE_ESTIMATION_HL_SSDK:
extPositionValid = 1;
extPosition.bitfield = 0;
extPosition.count = ext_position_update.count;
extPosition.heading = -ext_position_update.heading + 360000;
extPosition.x = ext_position_update.x;
extPosition.y = -ext_position_update.y;
extPosition.z = -ext_position_update.z;
extPosition.vX = ext_position_update.vX;
extPosition.vY = -ext_position_update.vY;
extPosition.vZ = -ext_position_update.vZ;
extPosition.qualX = ext_position_update.qualX;
extPosition.qualY = ext_position_update.qualY;
extPosition.qualZ = ext_position_update.qualZ;
extPosition.qualVx = ext_position_update.qualVx;
extPosition.qualVy = ext_position_update.qualVy;
extPosition.qualVz = ext_position_update.qualVz;
break;
case HLI_MODE_STATE_ESTIMATION_EXT:
extPositionValid = 1;
extPosition.bitfield = EXT_POSITION_BYPASS_FILTER;
extPosition.count = ext_position_update.count;
extPosition.heading = -ext_position_update.heading + 360000;
extPosition.x = ext_position_update.x;
extPosition.y = -ext_position_update.y;
extPosition.z = -ext_position_update.z;
extPosition.vX = ext_position_update.vX;
extPosition.vY = -ext_position_update.vY;
extPosition.vZ = -ext_position_update.vZ;
extPosition.qualX = ext_position_update.qualX;
extPosition.qualY = ext_position_update.qualY;
extPosition.qualZ = ext_position_update.qualZ;
extPosition.qualVx = ext_position_update.qualVx;
extPosition.qualVy = ext_position_update.qualVy;
extPosition.qualVz = ext_position_update.qualVz;
break;
default:
extPositionValid = 0;
}
// execute ssdk - only executed if ssdk parameters are available
// reads position reference from extPosition
// reads position/velocity command from extPositionCmd
// finally writes to WO_CTRL_Input. therefore, make sure to overwrite it after this call if you don't want to have its output
rt_OneStep();
// --- write commands to LL ------------------------------------------------
short motorsRunning = LL_1khz_attitude_data.status2 & 0x1;
if (motor_state == -1 || motor_state == 2)
{ // motors are either stopped or running --> normal operation
// commands are always written to LL by the Matlab controller, decide if we need to overwrite them
if (extPositionValid > 0 && statusData.have_SSDK_parameters == 1 && config.mode_position_control == HLI_MODE_POSCTRL_HL)
{
WO_CTRL_Input.ctrl = config.position_control_axis_enable;
WO_SDK.ctrl_enabled = 1;
}
else if (cmdLLValid > 0 && (config.mode_position_control == HLI_MODE_POSCTRL_LL || config.mode_position_control == HLI_MODE_POSCTRL_OFF))
{
writeCommand(cmdLL.x, cmdLL.y, cmdLL.yaw, cmdLL.z, config.position_control_axis_enable, 1);
}
else
{
writeCommand(0, 0, 0, 0, 0, 0);
}
}
// start / stop motors, allow commands max for 1.5 s
else if (motor_state == 1)
{
if (motor_state_count < 1500)
{
if (!motorsRunning)
writeCommand(0, 0, 2047, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1); // start/stop sequence, set ctrl to acc so that motors will be safely started
else if (motorsRunning)
{
writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
motor_state = 2;
}
}
else
{
writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
motor_state = -1;
}
motor_state_count ++;
}
else if (motor_state == 0)
{
if (motor_state_count < 1500)
{
if (motorsRunning)
writeCommand(0, 0, 2047, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1); // start/stop sequence, set ctrl to acc so that motors will be safely shut down
else if (!motorsRunning)
{
writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
motor_state = -1;
}
}
else
{
writeCommand(0, 0, 0, 0, HLI_YAW_BIT | HLI_THRUST_BIT, 1);
motor_state = -1;
}
motor_state_count++;
}
else
{
// undefined state, disable everything
writeCommand(0, 0, 0, 0, 0, 0);
}
// TODO: thrust limit in case something really goes wrong, may be removed
if (WO_CTRL_Input.thrust > 4095)
WO_CTRL_Input.thrust = 4095;
// ------------------------------------------------------------------------
// --- send data to UART 0 ------------------------------------------------
if (checkTxPeriod(subscription.imu))
{
sendImuData();
}
if (checkTxPeriod(subscription.rcdata))
{
sendRcData();
}
if (checkTxPeriod(subscription.gps))
{
sendGpsData();
}
if ((sdkLoops + 20) % 500 == 0)
{
sendStatus();
writePacket2Ringbuffer(HLI_PACKET_ID_SSDK_STATUS, (unsigned char*)&ssdk_status, sizeof(ssdk_status));
}
if (checkTxPeriod(subscription.ssdk_debug))
{
ssdk_debug.timestamp = timestamp;
writePacket2Ringbuffer(HLI_PACKET_ID_SSDK_DEBUG, (unsigned char*)&ssdk_debug, sizeof(ssdk_debug));
}
//
UART_send_ringbuffer();
synchronizeTime();
if (packetBaudrate->updated)
{
packetBaudrate->updated = 0;
while (!UART0_txEmpty())
;
}
// ------------------------------------------------------------------------
unsigned int dt;
if (T1TC < sdkCycleStartTime)
dt = (processorClockFrequency() - sdkCycleStartTime) + T1TC;
else
dt = T1TC - sdkCycleStartTime;
cpuLoad = ControllerCyclesPerSecond * ((dt * 1e2) / processorClockFrequency()); // cpu load in %
watchdog();
}
void step_PID(void){
#ifdef DEBUG_POS_CONTROLLER_OUTPUTS
visualization_msgs::Marker marker, marker_loc;
nav_msgs::Odometry odomSpeed;
#endif
if(_enabled){
rt_OneStep();
twist.linear.x=Pos_Controller_Y.speedcmd[0]*10;
twist.linear.y=Pos_Controller_Y.speedcmd[1]*10;
twist.linear.z=Pos_Controller_Y.speedcmd[2]*10;
twist.angular.z=Pos_Controller_Y.yawcmd;
#ifdef DEBUG_POS_CONTROLLER_OUTPUTS
epsilon.position.x=-Pos_Controller_U.position[0]+Pos_Controller_U.consigne[0];
epsilon.position.y=-Pos_Controller_U.position[1]+Pos_Controller_U.consigne[1];
epsilon.position.z=-Pos_Controller_U.position[2]+Pos_Controller_U.consigne[2];
//assert length are equals in absolute and drone speed
real_T err=pow(Pos_Controller_Y.speedcmd[0],2)+pow(Pos_Controller_Y.speedcmd[1],2)-pow(Pos_Controller_Y.absolute_speedcmd[0],2)-pow(Pos_Controller_Y.absolute_speedcmd[1],2);
if(err>0.00001 || err <-0.00001){
ROS_ERROR("change rep wrong! %f",err);
}
//debug speed as odometry message
odomSpeed.header.frame_id="/nav";
odomSpeed.header.stamp=ros::Time::now();
odomSpeed.pose.pose.position.x=Pos_Controller_U.position[0];
odomSpeed.pose.pose.position.y=Pos_Controller_U.position[1];
odomSpeed.pose.pose.position.z=Pos_Controller_U.position[2];
odomSpeed.twist.twist.linear.x=Pos_Controller_Y.absolute_speedcmd[0];
odomSpeed.pose.pose.position.y=Pos_Controller_Y.absolute_speedcmd[1];
odomSpeed.pose.pose.position.z=Pos_Controller_Y.absolute_speedcmd[2];
odomSpeedPublisher.publish(odomSpeed);
{
geometry_msgs::Point p;
marker.header.frame_id = "nav";
marker.header.stamp = ros::Time();
marker.ns = "abs_speed_display";
marker.id = 0;
marker.type = visualization_msgs::Marker::ARROW;
marker.action = visualization_msgs::Marker::ADD;
p.x=Pos_Controller_U.position[0];
p.y=Pos_Controller_U.position[1];
p.z=Pos_Controller_U.position[2];
marker.points.push_back(p);
p.x=Pos_Controller_U.position[0]+Pos_Controller_Y.absolute_speedcmd[0]*10;
p.y=Pos_Controller_U.position[1]+Pos_Controller_Y.absolute_speedcmd[1]*10;
p.z=Pos_Controller_U.position[2]+Pos_Controller_Y.absolute_speedcmd[2]*0.2;
marker.points.push_back(p);
marker.scale.x=0.1;
marker.scale.y=0.2;
marker.color.g = 1.0f;
marker.color.a = 1.0;
vis_pub.publish( marker );
marker_loc.header.frame_id = "/base_link";
marker_loc.header.stamp = ros::Time();
marker_loc.ns = "drone_speed_display";
marker_loc.id = 0;
marker_loc.type = visualization_msgs::Marker::ARROW;
marker_loc.action = visualization_msgs::Marker::ADD;
p.x=0;
p.y=0;
p.z=0;
marker_loc.points.push_back(p);
p.x=Pos_Controller_Y.speedcmd[0]*10;
p.y=Pos_Controller_Y.speedcmd[1]*10;
p.z=Pos_Controller_Y.speedcmd[2]*0.2;
marker_loc.points.push_back(p);
marker_loc.scale.x=0.1;
marker_loc.scale.y=0.2;
//marker_loc.scale.z=0.2;
marker_loc.color.r = 0.7f;
marker_loc.color.b = 0.7f;
marker_loc.color.a = 1.0;
vis_pub.publish( marker_loc );
}
epsPublisher.publish(epsilon);
#endif
twistPublisher.publish(twist);
}
}
/*
* ======== clk0Fxn =======
*/
Void clk0Fxn(UArg arg0)
{
/* Base rate */
rt_OneStep();
}
/* Function: main =============================================================
*
* Abstract:
* Execute model on a generic target such as a workstation.
*/
int_T main(int_T argc, const char_T *argv[])
{
RT_MODEL *S;
const char *status;
real_T finaltime = -2.0;
int_T oldStyle_argc;
const char_T *oldStyle_argv[5];
/******************************
* MathError Handling for BC++ *
******************************/
#ifdef BORLAND
signal(SIGFPE, (fptr)divideByZero);
#endif
/*******************
* Parse arguments *
*******************/
if ((argc > 1) && (argv[1][0] != '-')) {
/* old style */
if ( argc > 3 ) {
displayUsage();
exit(EXIT_FAILURE);
}
oldStyle_argc = 1;
oldStyle_argv[0] = argv[0];
if (argc >= 2) {
oldStyle_argc = 3;
oldStyle_argv[1] = "-tf";
oldStyle_argv[2] = argv[1];
}
if (argc == 3) {
oldStyle_argc = 5;
oldStyle_argv[3] = "-port";
oldStyle_argv[4] = argv[2];
}
argc = oldStyle_argc;
argv = oldStyle_argv;
}
{
/* new style: */
double tmpDouble;
char_T tmpStr2[200];
int_T count = 1;
int_T parseError = FALSE;
/*
* 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.
*/
while(count < argc) {
const char_T *option = argv[count++];
/* final time */
if ((strcmp(option, "-tf") == 0) && (count != argc)) {
const char_T *tfStr = argv[count++];
sscanf(tfStr, "%200s", tmpStr2);
if (strcmp(tmpStr2, "inf") == 0) {
tmpDouble = RUN_FOREVER;
} else {
char_T tmpstr[2];
if ( (sscanf(tmpStr2,"%lf%1s", &tmpDouble, tmpstr) != 1) ||
(tmpDouble < 0.0) ) {
(void)printf("finaltime must be a positive, real value or inf\n");
parseError = TRUE;
break;
}
}
finaltime = (real_T) tmpDouble;
argv[count-2] = NULL;
argv[count-1] = NULL;
}
}
if (parseError) {
(void)printf("\nUsage: %s -option1 val1 -option2 val2 -option3 "
"...\n\n", QUOTE(MODEL));
(void)printf("\t-tf 20 - sets final time to 20 seconds\n");
exit(EXIT_FAILURE);
}
rtExtModeParseArgs(argc, argv, NULL);
/*
* Check for unprocessed ("unhandled") args.
*/
{
int i;
for (i=1; i<argc; i++) {
if (argv[i] != NULL) {
printf("Unexpected command line argument: %s\n",argv[i]);
exit(EXIT_FAILURE);
}
}
}
}
/****************************
* Initialize global memory *
****************************/
(void)memset(&GBLbuf, 0, sizeof(GBLbuf));
/************************
* Initialize the model *
************************/
rt_InitInfAndNaN(sizeof(real_T));
S = MODEL();
if (rtmGetErrorStatus(S) != NULL) {
(void)fprintf(stderr,"Error during model registration: %s\n",
rtmGetErrorStatus(S));
exit(EXIT_FAILURE);
}
if (finaltime >= 0.0 || finaltime == RUN_FOREVER) rtmSetTFinal(S,finaltime);
MdlInitializeSizes();
MdlInitializeSampleTimes();
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);
exit(EXIT_FAILURE);
}
rt_CreateIntegrationData(S);
/*
#ifdef UseMMIDataLogging
rt_FillStateSigInfoFromMMI(rtmGetRTWLogInfo(S),&rtmGetErrorStatus(S));
rt_FillSigLogInfoFromMMI(rtmGetRTWLogInfo(S),&rtmGetErrorStatus(S));
#endif*/
/* GBLbuf.errmsg = rt_StartDataLogging(rtmGetRTWLogInfo(S),
rtmGetTFinal(S),
rtmGetStepSize(S),
&rtmGetErrorStatus(S));
if (GBLbuf.errmsg != NULL) {
(void)fprintf(stderr,"Error starting data logging: %s\n",GBLbuf.errmsg);
return(EXIT_FAILURE);
}*//*removed datalogging*/
rtExtModeCheckInit(rtmGetNumSampleTimes(S));
rtExtModeWaitForStartPkt(rtmGetRTWExtModeInfo(S),
rtmGetNumSampleTimes(S),
(boolean_T *)&rtmGetStopRequested(S));
(void)printf("\n** starting the model **\n");
MdlStart();
if (rtmGetErrorStatus(S) != NULL) {
GBLbuf.stopExecutionFlag = 1;
}
/*************************************************************************
* Execute the model. You may attach rtOneStep to an ISR, if so replace *
* the call to rtOneStep (below) with a call to a background task *
* application. *
*************************************************************************/
if (rtmGetTFinal(S) == RUN_FOREVER) {
printf ("\n**May run forever. Model stop time set to infinity.**\n");
}
stepTime=(FPS*CLOCKS_PER_SEC)/1000;
nextStart = clock();
nextStart+=stepTime;
while (!GBLbuf.stopExecutionFlag &&
(rtmGetTFinal(S) == RUN_FOREVER ||
rtmGetTFinal(S)-rtmGetT(S) > rtmGetT(S)*DBL_EPSILON)) {
rtExtModePauseIfNeeded(rtmGetRTWExtModeInfo(S),
rtmGetNumSampleTimes(S),
(boolean_T *)&rtmGetStopRequested(S));
if( clock() >= nextStart)
{
if( stepTime > 0)
{
printf("***Execution slower than requested rate: Actual speed =%d ms***\n",(1000*(stepTime+clock()-nextStart))/CLOCKS_PER_SEC);
nextStart=clock();
}
}
while (clock() < nextStart) {}
nextStart+=stepTime;
if (rtmGetStopRequested(S)) break;
rt_OneStep(S);
}
if (!GBLbuf.stopExecutionFlag && !rtmGetStopRequested(S)) {
/* Execute model last time step */
rt_OneStep(S);
}
/********************
* Cleanup and exit *
********************/
/*
#ifdef UseMMIDataLogging
rt_CleanUpForStateLogWithMMI(rtmGetRTWLogInfo(S));
rt_CleanUpForSigLogWithMMI(rtmGetRTWLogInfo(S));
#endif
rt_StopDataLogging(MATFILE,rtmGetRTWLogInfo(S));*/
rtExtModeShutdown(rtmGetNumSampleTimes(S));
if (GBLbuf.errmsg) {
(void)fprintf(stderr,"%s\n",GBLbuf.errmsg);
exit(EXIT_FAILURE);
}
if (rtmGetErrorStatus(S) != NULL) {
(void)fprintf(stderr,"ErrorStatus set: \"%s\"\n", rtmGetErrorStatus(S));
exit(EXIT_FAILURE);
}
if (GBLbuf.isrOverrun) {
(void)fprintf(stderr,
"%s: ISR overrun - base sampling rate is too fast\n",
QUOTE(MODEL));
exit(EXIT_FAILURE);
}
#ifdef MULTITASKING
else {
int_T i;
for (i=1; i<NUMST; i++) {
if (GBLbuf.overrunFlags[i]) {
(void)fprintf(stderr,
"%s ISR overrun - sampling rate is too fast for "
"sample time index %d\n", QUOTE(MODEL), i);
exit(EXIT_FAILURE);
}
}
}
#endif
MdlTerminate();
return(EXIT_SUCCESS);
} /* end main */