Example #1
0
/*
 * 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;
}
Example #2
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);
}
Example #3
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;
}