// ------------------------------------------------------------------------------ // Main // ------------------------------------------------------------------------------ int main(int argc, char **argv) { // This program uses throw, wrap one big try/catch here ros::init(argc, argv, "ros_ca_flight"); // vxo=0; // vyo=0; // vyi=0; // vxi=0; // xi=0; // yi=0; // yo=0; // xo=0; // xt=0; // yt=0; dt = 0.1; // ros::nodeHandler ros::NodeHandle nh_; // own_imu_sub = nh_.subscribe("odroid1/mavros/imu/data",1,own_vel_callback); // own_pose_sub = nh_.subscribe("odometry",1,own_pose_callback); // own_pose_sub = nh_.subscribe("/demo_odom",1,own_pose_demo_callback); // own_pose_sub = nh_.subscribe("odroid1/odometry",1,own_pose_callback); // intruder_imu_sub = nh_.subscribe("odroid2/mavros/imu/data",1,intruder_vel_callback); // intruder_pose_sub = nh_.subscribe("odroid2/odometry",1,intruder_pose_callback); // own_pose_sub = nh_.subscribe("mavros/local_position/local",1,gps_own_pose_callback); own_pose_sub = nh_.subscribe("mavros/position/local",1,gps_own_pose_callback); own_vel_sub = nh_.subscribe("mavros/global_position/gps_vel",1,gps_own_vel_callback); own_ptam_sub = nh_.subscribe("vslam/pose",1,ptam_own_pose_callback); // intruder_pose_sub = nh_.subscribe("odroid1/mavros/local_position/local",1,gps_intruder_pose_callback); intruder_pose_sub = nh_.subscribe("odroid1/mavros/position/local",1,gps_intruder_pose_callback); intruder_vel_sub = nh_.subscribe("odroid1/mavros/global_position/gps_vel",1,gps_intruder_vel_callback); set_pt_vel_pub = nh_.advertise<geometry_msgs::TwistStamped>("/mavros/cmd_vel",1); set_pt_vel_pub_1 = nh_.advertise<geometry_msgs::TwistStamped>("/mavros/setpoint_velocity/cmd_vel",1); set_pt_vel_pub_2 = nh_.advertise<geometry_msgs::TwistStamped>("/mavros/setpoint/cmd_vel",1); FILE *fpIn, *fpData; int numVerticies, numElements; // Needed in decide_Algorithm() but allocated or evaluated // in start_Algorithm(): int numDims, numActions, *elementsDim; FILE *fpOut, *fpLog; double *neighY; double **discMat, **neighX; struct cd_grid **gridQsa; // This should hold a vector of grids, one for each action struct stat st = {0}; struct tm *t; time_t timeVar; char str_time[100]; char log_destination[150]; char results_destination[150]; // Variables simply used in the algorithm functions, may be redeclared in each function: int i, j, k; int err, keyinput, exitCondition; if (VERBOSE) printf("Reading parameter file...\n"); /* Open and check the files */ // fpIn = fopen("CA2_params.prm","r"); // fpData = fopen("CA2_data.dat","r"); fpIn = fopen(argv[1],"r"); fpData = fopen(argv[2],"r"); // fpIn = fopen("paramtemp.txt","r"); // fpData = fopen("datatemp.txt","r"); // fpIn = fopen("test.txt","r"); // fpData = fopen("data.txt","r"); // fpIn = fopen("param8Dim141201.txt","r"); // fpData = fopen("data8Dim141201.txt","r"); if (stat("logs", &st) == -1) mkdir("logs", 0777); timeVar = time(NULL); t = localtime(&timeVar); strftime(str_time, sizeof(str_time), "%H%M%S",t); strcpy(results_destination,"./logs/CA2_results_"); strcat(results_destination,str_time); strcat(results_destination,".out"); fpOut = fopen(results_destination,"w"); strcpy(log_destination,"./logs/CA2_results_"); strcat(log_destination,str_time); strcat(log_destination,".log"); fpLog = fopen(log_destination,"w"); //printf("Output file name: %s", results_destination); /* Check for errors opening files */ if (fpIn == NULL) { fprintf(stderr, "Can't open input file param\n"); exit(1); } if (fpData == NULL) { fprintf(stderr, "Can't open data file data\n"); exit(1); } if (fpLog == NULL) { fprintf(stderr, "Can't open log file: %s\n", log_destination); exit(1); } if (fpOut == NULL) { fprintf(stderr, "Can't open output file results out: %s\n", results_destination); exit(1); } /* Read in parameter file */ // Number of dimensions: fscanf(fpIn, "%d", &numDims); // Number of grid points (states) in each dimension: numElements = 1; elementsDim = (int *)malloc(numDims*sizeof(int)); discMat = (double **)malloc(numDims*sizeof(double)); for (i=0; i<numDims; i++) { fscanf(fpIn, "%d", &elementsDim[i]); //printf("Dimension %d = %d\n", i, elementsDim[i]); numElements *= elementsDim[i]; discMat[i] = (double *)malloc(elementsDim[i]*sizeof(double)); } // Values of states at each grid point: for (i=0; i<numDims; i++) { for (j=0; j<elementsDim[i]; j++) { fscanf(fpIn, "%lf", &discMat[i][j]); } } // Number of actions: fscanf(fpIn, "%d", &numActions); fclose(fpIn); if (VERBOSE) printf("Done reading parameter file\n"); /* Finished reading parameter file */ /* Create the grid data structure to hold contents as specified in the parameter file */ //cell_init = (void *)malloc(numElements*sizeof(double)); // gridInst = (struct cd_grid **) malloc(sizeof(struct cd_grid)); double var; struct cd_grid **gridInst; int actionInd; int *subs; gridInst = (struct cd_grid **) malloc(sizeof(struct cd_grid)); gridQsa = (struct cd_grid **)malloc(numActions*sizeof(struct cd_grid)); subs = (int *)malloc(numDims*sizeof(int)); // To make this a single block of text for every possible value of numDims, need to be able to // fill the "grid" with elements directly using the index and subscripts. There may already be // a function in cd_grid to do this... if (VERBOSE) printf("Reading data file\n"); if (numDims==3) { double Q[elementsDim[0]][elementsDim[1]][elementsDim[2]]; /* Using the contents of the parameter file, read and parse the data file */ for (actionInd=0; actionInd<numActions; actionInd++) { // I should probably be able to read in the elements directly to Q if it's allocated into a contiguous // memory block. Could just use the index value, rather than going from an index to a subscript and // back to and index? Just need to verify that this works, then make sure my revision matches this version. for (i=0;i<numElements;i++) { fscanf(fpData, "%lf", &var); err = ind2subs(elementsDim, numDims, i, subs); Q[subs[0]][subs[1]][subs[2]] = var; } // After the number of dimensions, the size of each dimension must be passed in as a series of arguments. cd_grid_create_fill(gridInst, Q, sizeof(double), numDims, elementsDim[0], elementsDim[1], elementsDim[2]); gridQsa[actionInd] = *gridInst; } } if (numDims==6) { double Q[elementsDim[0]][elementsDim[1]][elementsDim[2]][elementsDim[3]][elementsDim[4]][elementsDim[5]]; for (actionInd=0; actionInd<numActions; actionInd++) { for (i=0;i<numElements;i++) { fscanf(fpData, "%lf", &var); err = ind2subs(elementsDim, numDims, i, subs); Q[subs[0]][subs[1]][subs[2]][subs[3]][subs[4]][subs[5]] = var; } cd_grid_create_fill(gridInst, Q, sizeof(double), numDims, elementsDim[0], elementsDim[1], elementsDim[2], elementsDim[3], elementsDim[4], elementsDim[5]); gridQsa[actionInd] = *gridInst; } } if (numDims==8) { double Q[elementsDim[0]][elementsDim[1]][elementsDim[2]][elementsDim[3]][elementsDim[4]][elementsDim[5]][elementsDim[6]][elementsDim[7]]; for (actionInd=0; actionInd<numActions; actionInd++) { for (i=0;i<numElements;i++) { fscanf(fpData, "%lf", &var); err = ind2subs(elementsDim, numDims, i, subs); Q[subs[0]][subs[1]][subs[2]][subs[3]][subs[4]][subs[5]][subs[6]][subs[7]] = var; } cd_grid_create_fill(gridInst, Q, sizeof(double), numDims, elementsDim[0], elementsDim[1], elementsDim[2], elementsDim[3], elementsDim[4], elementsDim[5], elementsDim[6], elementsDim[7]); gridQsa[actionInd] = *gridInst; } } // // Test the read-in data: // double testArray[numElements]; // for (i=0;i<numElements;i++){ // err = ind2subs(elementsDim, numDims, i, subs); // testArray[i] = *(double *)cd_grid_get_subs(gridQsa[0], subs); // printf("%lf\n", testArray[i]); // } fclose(fpData); free(gridInst); free(subs); if (VERBOSE) printf("Done reading data file\n"); // Allocate space for the interpolations needed to decide actions: numVerticies = pow(2,numDims); neighX = (double **)malloc(numDims*sizeof(double)); if (neighX) for (i=0;i<numDims;i++) neighX[i]=(double *)malloc(numVerticies*sizeof(double)); neighY = (double *)malloc(numVerticies*sizeof(double)); /*********************************************************** *** The following would be in decide_Algorithm() *** ************************************************************/ int bestActionInd; double maxQsa; int stepCounter; double *qVals, *currentState, *indicies; // These allocations may be done on every call to decide_Algorithm(), they're not large qVals = (double *)malloc(numActions*sizeof(double)); currentState = (double *)malloc(numDims*sizeof(double)); indicies = (double *)malloc(numDims*sizeof(double)); exitCondition = 0; stepCounter = 0; printf("BEGINNING SIMULATION\n"); printf("\n"); // try // { int result = top(argc,argv); // return result; // } // catch ( int error ) // { fprintf(stderr,"main threw exception %i \n" , error); // return error; // } // if (!(nh_.ok())) // ros::spin(); // if ((nh_.ok())&&(!exitCondition)) int NOMINAL_MODE_LAST; NOMINAL_MODE_LAST = 1; while (!exitCondition) { // while ((!exitCondition)&&(nh_.ok())) // read the current state from MAVLink (or a file, for now) err = readState(currentState, numDims, (double)stepCounter); // std::cerr<<"It actually reached here"<<std::endl; // find indicies into the grid using the current state err = getIndicies(indicies, currentState, numDims, elementsDim, discMat); // for each action: int actionInd; for (actionInd=0; actionInd<numActions; actionInd++) { // read in the neighbor verticies to the data structure and read in the values at the verticies err = getNeighbors(indicies, &gridQsa[actionInd], neighX, neighY, numDims, discMat); // interpolate within the data file using the current state (call interpN()) and store qVals[actionInd] = interpN(numDims, currentState, neighX, neighY, EXTRAPMODE); if (VERBOSE) { printf("Indicies:\n"); printf("["); for (i=0;i<numDims;i++) { printf("%lf, ", indicies[i]); } printf("]\n"); printf("Neighbors:\n"); printf("["); for (i=0;i<numDims;i++) { for(j=0;j<numVerticies; j++) { printf("%lf, ", neighX[i][j]); } printf("\n"); } printf("Neighbor values:\n"); printf("["); for (i=0;i<numVerticies;i++) { printf("%lf, ", neighY[i]); } printf("]\n"); printf("Interpolated Value: %lf\n", qVals[actionInd]); } } // calculate the best action bestActionInd = 0; maxQsa = qVals[0]; for (i=1;i<numActions;i++) { if (qVals[i] > maxQsa) { maxQsa = qVals[i]; bestActionInd = i; } } // write the action to MAVLink (or a file, for now) if (intruderThreat(currentState)) { err = writeCAAction(bestActionInd, currentState, numDims, fpOut); if (NOMINAL_MODE_LAST) { // We were just in nominal mode, so record the current position as the start of the desired trajectory xt = xo; yt = yo; printf("Setting nominal trajectory start point: %lf %lf\n\n", xt, yt); NOMINAL_MODE_LAST = 0; } } else { err = writeNominalAction(fpOut); NOMINAL_MODE_LAST = 1; } // log the current state and action to a log file err = writeLogs(fpLog, stepCounter, currentState, numDims, bestActionInd); // Increment the counter (proxy for the current time when writing to file) stepCounter++; // While debugging, stop after a predetermined number of steps: // if (stepCounter>=MAXSIMSTEPS) // { printf("ENDING SIMULATION\n\n"); // exitCondition = 1; // } // if (time_to_exit){ // exitCondition = 1; // } if (nh_.ok()) ros::spinOnce(); // ros::spin(); else exitCondition = 1; usleep(1000000*dt); } free(currentState); free(qVals); free(indicies); for (i=0;i<numActions;i++) { cd_grid_destroy(gridQsa[i]); } free(gridQsa); for (i=0; i<numDims; i++) { free(neighX[i]); free(discMat[i]); } free(discMat); free(elementsDim); free(neighX); free(neighY); /* close the log and output files */ fclose(fpOut); fclose(fpLog); printf("Exiting\n"); return 0; }
void perMain() { #if defined(PCBSKY9X) && !defined(REVA) calcConsumption(); #endif checkSpeakerVolume(); checkEeprom(); sdMountPoll(); writeLogs(); handleUsbConnection(); checkTrainerSettings(); checkBattery(); uint8_t evt = getEvent(false); if (evt && (g_eeGeneral.backlightMode & e_backlight_mode_keys)) backlightOn(); // on keypress turn the light on checkBacklight(); #if defined(NAVIGATION_STICKS) uint8_t sticks_evt = getSticksNavigationEvent(); if (sticks_evt) evt = sticks_evt; #endif #if defined(USB_MASS_STORAGE) if (usbPlugged()) { // disable access to menus lcd_clear(); menuMainView(0); lcdRefresh(); return; } #endif #if defined(LUA) uint32_t t0 = get_tmr10ms(); static uint32_t lastLuaTime = 0; uint16_t interval = (lastLuaTime == 0 ? 0 : (t0 - lastLuaTime)); lastLuaTime = t0; if (interval > maxLuaInterval) { maxLuaInterval = interval; } // run Lua scripts that don't use LCD (to use CPU time while LCD DMA is running) luaTask(0, RUN_MIX_SCRIPT | RUN_FUNC_SCRIPT | RUN_TELEM_BG_SCRIPT, false); // wait for LCD DMA to finish before continuing, because code from this point // is allowed to change the contents of LCD buffer // // WARNING: make sure no code above this line does any change to the LCD display buffer! // lcdRefreshWait(); // draw LCD from menus or from Lua script // run Lua scripts that use LCD bool standaloneScriptWasRun = luaTask(evt, RUN_STNDAL_SCRIPT, true); bool refreshScreen = true; if (!standaloneScriptWasRun) { refreshScreen = !luaTask(evt, RUN_TELEM_FG_SCRIPT, true); } t0 = get_tmr10ms() - t0; if (t0 > maxLuaDuration) { maxLuaDuration = t0; } if (!standaloneScriptWasRun) #else lcdRefreshWait(); // WARNING: make sure no code above this line does any change to the LCD display buffer! const bool refreshScreen = true; #endif { // normal GUI from menus const char *warn = s_warning; uint8_t menu = s_menu_count; if (refreshScreen) { lcd_clear(); } if (menuEvent) { m_posVert = menuEvent == EVT_ENTRY_UP ? g_menuPos[g_menuStackPtr] : 0; m_posHorz = 0; evt = menuEvent; menuEvent = 0; AUDIO_MENUS(); } g_menuStack[g_menuStackPtr]((warn || menu) ? 0 : evt); if (warn) DISPLAY_WARNING(evt); if (menu) { const char * result = displayMenu(evt); if (result) { menuHandler(result); putEvent(EVT_MENU_UP); } } drawStatusLine(); } lcdRefresh(); #if defined(REV9E) && !defined(SIMU) topLcdRefreshStart(); setTopFirstTimer(getValue(MIXSRC_FIRST_TIMER+g_model.topLcdTimer)); setTopSecondTimer(g_eeGeneral.globalTimer + sessionTimer); setTopRssi(TELEMETRY_RSSI()); setTopBatteryValue(g_vbat100mV); setTopBatteryState(GET_TXBATT_BARS(), IS_TXBATT_WARNING()); topLcdRefreshEnd(); #endif #if defined(REV9E) && !defined(SIMU) bluetoothWakeup(); #endif #if defined(PCBTARANIS) if (requestScreenshot) { requestScreenshot = false; writeScreenshot(); } #endif }