Esempio n. 1
0
void demo_7_main_core_0() {

	int threads = TOTAL_PROC_NUM;

	// Initialize obstacle map from heightmap
	if (1) {
		logStart("initialize obstacle map");

		// Recode heightmap to global_obstacles
		int a, b, c;

		for (a = 0; a < WORLD_SIZE_X; a++) {
			for (b = 0; b < WORLD_SIZE_Y; b++) {
				int height = getHeightmapValue(a, b);

				// some regions cannot be flown over
				if (height > 174)
					height = 255;

				for (c = 0; c < height; c++) {
					global_obstacles[a][b][c] = 1;
				}
				for (c = height; c < WORLD_SIZE_Z; c++) {
					global_obstacles[a][b][c] = 0;
				}
			}
		}

		logEnd("initialize obstacle map");
	}

	// Set goal
	setGoal(WORLD_SIZE_X - 3, WORLD_SIZE_Y - 3, getHeightmapValue(WORLD_SIZE_X - 3, WORLD_SIZE_Y - 3) + 2);

	// Set current position
	setPosition(3, 3, getHeightmapValue(3, 3) + 2);

	// Init weightmap from obstacle map
	if (1) {
		int a, b, c;

		logStart("initialize weightmap from obstacle map");

		for (a = 0; a < WORLD_SIZE_X; a++) {
			for (b = 0; b < WORLD_SIZE_Y; b++) {
				for (c = 0; c < WORLD_SIZE_Z; c++) {
					if (isGoal(a, b, c)) {
						world[a][b][c] = WEIGHT_SINK;
					} else if (isObstacle(a, b, c)) {
						world[a][b][c] = WEIGHT_OBSTACLE;
					} else {
						world[a][b][c] = WEIGHT_INIT;
					}
				}
			}
		}

		logEnd("initialize weightmap from obstacle map");
	}

	// Propagate weights with selected algorithm
	promote_world(ITERATIONS, threads);

	// Find waypoints to goal
	findWaypoints();

	printf("Iterations: %i\n", ITERATIONS);

	return;
}
Esempio n. 2
0
int main(int argc, char* argv[]) {
    // Initialize shared memory
    if (ipc.init(false))
        exit(1);

    // Initialize nominal beaconContainer
    parseWaypoints();
    
    // Create camera semaphore/ mutex
    sem_init(&semCam, 0, 0);
    if (pthread_mutex_init(&mutexImageData, NULL) != 0) {
        printf("\n Camera mutex init failed\n");
        exit(1);
    }
    
    // Start camera thread
    pthread_t tid;
    int err = pthread_create(&tid, NULL, &camThread, NULL);
    if (err != 0) {
        printf("\nCan't create camera thread :[%s]", strerror(err));
        exit(1);
    }
    
    lastPhotogramMicros = getMicroSec();
    
    // Main loop
    for(;;photogram++) {        
        TRACE(DEBUG, "\nPHOTOGRAM %d, milliseconds: %ld\n", photogram, getMillisSinceStart());
        
        // Update time variables
        photogramMicros = getMicroSec();
        lastPhotogramSpentTime = getSpent(photogramMicros - lastPhotogramMicros);
        microsSinceStart += lastPhotogramSpentTime;
        lastPhotogramMicros = photogramMicros;
        
        // Get drone roll, pitch and yaw
        attitudeT attitude, delayedAttitude;
        ipc.getAttitude(attitude);
        
        attitudeSignal.push(attitude, lastPhotogramSpentTime);
        delayedAttitude = attitudeSignal.getSignal();
        
        droneRoll = delayedAttitude.roll * 0.01;
        dronePitch = delayedAttitude.pitch * 0.01;
        droneYaw = delayedAttitude.yaw * 0.01;
        
        // Update estimated servo values
        rollStatusServo.update(lastPhotogramSpentTime);
        pitchStatusServo.update(lastPhotogramSpentTime);
        
        if (rollStatusServo.inBigStep() || pitchStatusServo.inBigStep()) {
            sem_wait(&semCam);
            continue;
        }
       
        // Update estimated camera roll and pitch
        roll = droneRoll + rollStatusServo.getEstimatedAngle();
        pitch = dronePitch + pitchStatusServo.getEstimatedAngle();
        
        // Switch beacon containers to preserve one history record
        if (beaconContainer == &beaconContainerA) {
            beaconContainer = &beaconContainerB;
            oldBeaconContainer = &beaconContainerA;
        }
        else {
            beaconContainer = &beaconContainerA;
            oldBeaconContainer = &beaconContainerB;
        }
        beaconContainer->clean();
  
        // Wait until new frame is received
        sem_wait(&semCam);
        
        // Where the magic happens
        findBeacons();
        groupBeacons();
        findWaypoints();
        computePosition();
    }
    StopCamera();
    printf("Finished.\n");
    
    return 0;
}