int main( int argc,
char *argv[])
{
MrmInitialize();
appshell = XtAppInitialize(&appContext,"GreatSPN_Motif",NULL,0,&argc,argv,fallback,NULL,0);
#ifdef Linux
/* Enables the application to talk with editres.
For debug purposes only */
/* XmdRegisterEditres(appshell);*/
#endif
OpenHierarchy();
if (MrmRegisterNames (regvec, regnum)
!= MrmSUCCESS)
XtError("can't register names\n");
RegisterArcChangeDialog();
RegisterColorChangeDialog();
RegisterPlaceChangeDialog();
RegisterTransChangeDialog();
RegisterShowDialog();
RegisterResultDialog();
RegisterRateDialog();
RegisterPrintDialog();
RegisterMDGrammarDialog();
RegisterMarkDialog();
RegisterViewLayerDialog();
RegisterEditLayerDialog();
RegisterConsoleDialog();
RegisterCommentDialog();
RegisterZooomMenuCallbacks();
RegisterMenuCallbacks();
RegisterRescale();
RegisterGrid();
RegisterMenuValues();
RegisterOptionsDialog();
RegisterSimulationDialog();
RegisterSwnSimOptionsDialog();
RegisterSwnRGOptionsDialog();
RegisterSwnUnfoldOptionsDialog();
/* puts(" ... start FetchWidget"); fflush(stdout);*/
mainwin = FetchWidget(appshell,"MainWin");
/* puts(" ... start InitMainWindow"); fflush(stdout);*/
InitMainWindow();
/* puts(" ... start InitWorkWindow"); fflush(stdout);*/
InitWorkWindow();
/* puts(" ... start InitFonts"); fflush(stdout);*/
InitFonts();
/* puts(" ... start InitMenuBar"); fflush(stdout);*/
InitMenuBar();
/* puts(" ... start InitPopups"); fflush(stdout);*/
InitPopups();
/* puts(" ... start XtManageChild"); fflush(stdout);*/
XtManageChild( mainwin );
/* puts(" ... start XtRealizeWidget"); fflush(stdout);*/
XtRealizeWidget( appshell );
/* puts(" ... start InitFilesPath"); fflush(stdout);*/
InitFilesPath();
/* puts(" ... start InitGSPNShellErrors"); fflush(stdout);*/
InitGSPNShellErrors();
/* puts(" ... start gdiInitEngine"); fflush(stdout);*/
gdiInitEngine(appshell);
/* puts(" ... start gdiInitDrawingAreaGC"); fflush(stdout);*/
gdiInitDrawingAreaGC();
/* puts(" ... start InitOther"); fflush(stdout);*/
InitOther();
/*
InitMsgDB();
*/
/* puts(" ... start XmUpdateDisplay"); fflush(stdout);*/
XmUpdateDisplay(mainwin);
/* puts(" ... start XtAppMainLoop"); fflush(stdout);*/
XtAppMainLoop(appContext);
return 0;
}
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;
}
/*******************************************************************************//**
* @implements InitComponents
**********************************************************************************/
RESULT InitComponents(void)
{
// init guard
if(Guard_Init()==FAIL)
return FAIL;
// init utils
if(Utils_Init()==FAIL)
return FAIL;
// init MCU
if(MCU_Init()==FAIL)
return FAIL;
// init scheduler
if(Scheduler_Init()==FAIL)
return FAIL;
// init power management system
#ifdef USE_PWR
if(PWR_Init()==FAIL)
return FAIL;
#endif
// init timers
#ifdef USE_TIMERS
if(Timers_Init()==FAIL)
return FAIL;
#endif
// init LEDs
#ifdef USE_LEDS
if(LEDs_Init()==FAIL)
return FAIL;
#endif
// init buttons
#ifdef USE_BUTTONS
if(Buttons_Init()==FAIL)
return FAIL;
#endif
// init UART
#ifdef USE_UART
if(UART_Init()==FAIL)
return FAIL;
#endif
// init SPI
#ifdef USE_SPI
if(SPI_Init()==FAIL)
return FAIL;
#endif
// init TWI
#ifdef USE_TWI
if(TWI_Init()==FAIL)
return FAIL;
#endif
// init OWI
#ifdef USE_OWI
if(OWI_Init()==FAIL)
return FAIL;
#endif
// init other components
if(InitOther()==FAIL)
return FAIL;
// init sensors
#ifdef USE_SENSORS
if(Sensors_Init()==FAIL)
return FAIL;
#endif
// init NWK
#ifdef USE_NWK
if(NWKLayer_Init()==FAIL)
return FAIL;
#endif
// return success
return SUCCESS;
}
