void run() {
uint32_t cnt=0;
wf121.init("YETENet","yeteyete");
wf121.enableUDPConnection(0xFF01A8C0,12345);
while(true) {
cnt++;
topicCounter1.publish(cnt);
AT(NOW() + 100*MILLISECONDS);
}
}
void Camera::DetectSatellite() {
int horizontalLine[WIDTH];
int verticalLine[HEIGHT];
// Init Arrays
for (int x = 0; x < WIDTH; x++) {
horizontalLine[x] = 0;
}
for (int y = 0; y < HEIGHT; y++) {
verticalLine[y] = 0;
}
int pixel = 0;
// Sum up Picture to Lines
for (int y = 0; y < HEIGHT; y++) {
for (int x = 0; x < WIDTH; x++) {
if ((int) DCMI_Buffer[2 * x + 2 * y * WIDTH] > THRESHOLD) {
horizontalLine[x] += (int) DCMI_Buffer[2 * x + 2 * WIDTH * y];
verticalLine[y] += (int) DCMI_Buffer[2 * x + 2 * WIDTH * y];
pixel++;
}
}
}
if(pixel<MINPIXELTHRESHOLD) { //Cancel if object is too small
target.x=0;
target.y=0;
return;
}
// Find the position in the arrays where the sumed up values
// are Q1, HALF and Q3 of the overall sum. HALF is exactly
// the center of gravity of the picture, the span between
// Q1 and Q3 is a good indicatior for the angle of the obj.
int counter = 0;
int sum1 = 0;
int sum2 = 0;
for(int x = 0; x < WIDTH; x++) {
sum1 += horizontalLine[x];
}
int first_quarter = 0;
int third_quarter = 0;
int targetX = 0;
int spanX = 0;
while(sum2<Q3*sum1) {
sum2 += horizontalLine[counter];
counter++;
if((Q1*sum1<sum2)&&(first_quarter==0)){
first_quarter = counter;
}
if((HALF*sum1<sum2)&&(targetX==0)) {
targetX = counter;
}
if((Q3*sum1<sum2)&&(third_quarter==0)) {
third_quarter = counter;
}
}
spanX = third_quarter - first_quarter;
counter = 0;
sum1 = 0;
sum2 = 0;
for(int y = 0; y < HEIGHT; y++) {
sum1 += verticalLine[y];
}
first_quarter = 0;
third_quarter = 0;
int targetY = 0;
int spanY = 0;
while(sum2<Q3*sum1) {
sum2 += verticalLine[counter];
counter++;
if((Q1*sum1<sum2)&&(first_quarter==0)){
first_quarter = counter;
}
if((HALF*sum1<sum2)&&(targetY==0)) {
targetY = counter;
}
if((Q3*sum1<sum2)&&(third_quarter==0)) {
third_quarter = counter;
}
}
spanY = third_quarter - first_quarter;
// ----------------------------------
bool fireAngle = spanX>2*spanY;
target.y = targetX;
target.x = targetY;
cameraTargetTopic.publish(target);
cameraFireTopic.publish(fireAngle);
xprintf("Target: x:%d, y:%d\n", target.x, target.y);
xprintf("Fire: %d\n", fireAngle);
// ---------------------------------
}