/*
* 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;
}
void terminateTask(void *arg)
{
int i;
int ret;
sem_wait(&termSem);
terminatingmodel = 1;
printf("**terminating the model**\n");
fflush(stdout);
/* Wait for baseRate task to complete */
ret = pthread_join(baseRateThread, (void *)&threadJoinStatus);
CHECK_STATUS(ret, 0, "pthread_join");
rt_StopDataLogging(MATFILE, main_M->rtwLogInfo);
/* Disable rt_OneStep() here */
/* Terminate model */
main_terminate();
sem_post(&stopSem);
}
int main(int argc, char *argv[])
{
long iter, repeat = 0;
double interval_sec = (double)1/20;
struct timespec start, end;
int opt;
/*
* get command line options
*/
while ((opt = getopt(argc, argv, "i:h")) != -1) {
switch (opt) {
case 'i': /* iterations */
repeat = strtol(optarg, NULL, 0);
PDEBUG("repeat=%ld\n", repeat);
break;
case 'h':
usage(argc, argv);
break;
}
}
/* Initialize model */
EKF_IFS_2_initialize();
/* Initialize hardware */
InitIMU(); /* vectornav */
InitSerial(); /* arduino */
clock_gettime(CLOCK_REALTIME, &start);
iter = 0;
while (1) {
double remain_us;
uint64_t tmpdiff;
/* Get sensor data */
GetIMUData(&EKF_IFS_2_U);
/* Get Arduino Data */
GetSerialData(&EKF_IFS_2_U);
/* Get moving points Data */
InitMovingWaypoints(&EKF_IFS_2_U);
/* Get waypoints Data */
InitStaticWaypoints(&EKF_IFS_2_U);
/* Get Servo deflection Data */
InitOther(&EKF_IFS_2_U);
/* Step the model */
EKF_IFS_2_step();
/* Output to the motor controller */
SendSerialData(&EKF_IFS_2_Y);
/* Time book keeping */
clock_gettime(CLOCK_REALTIME, &end);
tmpdiff = get_elapsed(&start, &end);
remain_us = (interval_sec * 1000000 - tmpdiff / 1000);
if (remain_us > 0) {
usleep((useconds_t)remain_us);
}
clock_gettime(CLOCK_REALTIME, &start);
iter++;
PDEBUG("iter %ld took %" PRIu64 "us\n", iter, tmpdiff/1000);
PDEBUG("Out: throttle=%f elevator=%f aileron=%f rudder=%f\n",
EKF_IFS_2_Y.ControlSurfaceCommands.throttle_cmd,
EKF_IFS_2_Y.ControlSurfaceCommands.elevator_cmd,
EKF_IFS_2_Y.ControlSurfaceCommands.aileron_cmd,
EKF_IFS_2_Y.ControlSurfaceCommands.rudder_cmd);
if (iter >= repeat)
break;
}
/* Matfile logging */
rt_StopDataLogging(MATFILE, EKF_IFS_2_M->rtwLogInfo);
/* Terminate model */
EKF_IFS_2_terminate();
/* Close hardware */
CloseIMU();
CloseSerial();
return 0;
}
