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;
}
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;
}