void setear_buffer(buffer_info_t *buffer, BMP *bmp) { buffer->bytes = bmp_data(bmp); buffer->width = bmp_width(bmp); buffer->height = bmp_height(bmp); buffer->width_with_padding = bmp_bytes_per_row(bmp); }
static int file_to_fb(const char * srcpath) { int ret = -1; BMP_READ * bmp = NULL; struct FB * fb = NULL; int sw, sh; int srcbpp, dstbpp; void * pdata = NULL, * bmpdata = NULL; RGB_CONVERT_FUN convert_func = NULL; do { bmp = bmp_open(srcpath); if (!bmp) { break; } fb = fb_create(0); if (!fb) { break; } sw = bmp_width(bmp); sh = bmp_height(bmp); bmpdata = bmp_data(bmp); srcbpp = bmp_bpp(bmp); dstbpp = fb_bpp(fb); convert_func = get_convert_func(srcbpp, dstbpp); if (convert_func) { pdata = convert_func(bmpdata, sw, sh); bmpdata = pdata; } if (!bmp_forward(bmp)) { line_reversal(bmpdata, sw, sh, dstbpp); } rgb_copy(bmpdata, fb_bits(fb), sw, sh, fb_width(fb), fb_height(fb), dstbpp); ret = 0; } while (0); fb_destory(fb); bmp_close(bmp); if (pdata) { free(pdata); } return ret; }
BMP* generador_constante(int filas, int columnas, uint8_t r, uint8_t g, uint8_t b, uint8_t a) { int i, j; BMP* res; BMPV5H* header; filas = filas + 4 - (filas % 4); // Lo llevamos al multiplo de 4 mas cercano y mas grande. columnas = columnas + 4 - (columnas % 4); // Lo llevamos al multiplo de 4 mas cercano y mas grande. header = get_BMPV5H(columnas, filas); res = bmp_create(header, 0); uint8_t* data = bmp_data(res); for(i = 0; i < filas; i++) { for(j = 0; j < columnas; j++) { data[j*filas*4 + 0] = b; data[j*filas*4 + 1] = g; data[j*filas*4 + 2] = r; data[j*filas*4 + 3] = a; } } return res; }
static int file_to_file(const char * srcpath, const char * dstpath, int output_rgb) { int ret = -1; BMP_READ * bmp = NULL; int w, h; int srcbpp, dstbpp; void * pdata = NULL, * bmpdata = NULL; RGB_CONVERT_FUN convert_func = NULL; do { bmp = bmp_open(srcpath); if (!bmp) { break; } w = bmp_width(bmp); h = bmp_height(bmp); bmpdata = bmp_data(bmp); srcbpp = bmp_bpp(bmp); dstbpp = g_rgbbpp[output_rgb]; convert_func = get_convert_func(srcbpp, dstbpp); if (convert_func) { pdata = convert_func(bmpdata, w, h); bmpdata = pdata; } if (!bmp_forward(bmp)) { line_reversal(bmpdata, w, h, dstbpp); } ret = save_bmp(dstpath, w, h, bmpdata, dstbpp); } while (0); bmp_close(bmp); if (pdata) { free(pdata); } return ret; }
BMP* generador_aleatorio(int filas, int columnas) { int i, j; BMP* res; BMPV5H* header; filas = filas + 4 - (filas % 4); // Lo llevamos al multiplo de 4 mas cercano y mas grande. columnas = columnas + 4 - (columnas % 4); // Lo llevamos al multiplo de 4 mas cercano y mas grande. header = get_BMPV5H(columnas, filas); srand(time(NULL)); res = bmp_create(header, 0); uint8_t* data = bmp_data(res); for(i = 0; i < filas; i++) { for(j = 0; j < columnas; j++) { data[j*filas*4 + 0] = rand() % 256; data[j*filas*4 + 1] = rand() % 256; data[j*filas*4 + 2] = rand() % 256; data[j*filas*4 + 3] = rand() % 256; } } return res; }
BMP* generador_todoscolores() { int i, j, filas, columnas; BMP* res; BMPV5H* header; filas = 255*255; columnas = 255*255; filas = filas + 4 - (filas % 4); // Lo llevamos al multiplo de 4 mas cercano y mas grande. columnas = columnas + 4 - (columnas % 4); // Lo llevamos al multiplo de 4 mas cercano y mas grande. header = get_BMPV5H(columnas, filas); res = bmp_create(header, 0); uint8_t* data = bmp_data(res); for(i = 0; i < filas; i++) { for(j = 0; j < columnas; j++) { data[j*filas*4 + 0] = j % 256; data[j*filas*4 + 1] = j / 256; data[j*filas*4 + 2] = i % 256; data[j*filas*4 + 3] = i / 256; } } return res; }
int main(void) { int i, runTime; FILE * logFile; char filename[64]; char temp[64]; struct timespec gettime_now; struct timespec start_time; int64_t last_heartbeat, heartbeat_difference; pthread_t thread[3]; pthread_mutex_t gps_lock; pthread_mutex_t i2c_lock; int policy; struct sched_param schedule; unsigned short lastTach = 0; unsigned short tachDiff = 0; unsigned short tach = 0; int64_t tach2 = 0; char sampleCount = 0; const unsigned char sampleCounts = 6; const unsigned short rpmPerCount = 1800 / 3 / sampleCounts; // samplesPerMinute(1800) / pthread_mutex_init(&tachLock, NULL); SituationData * mySituation; mySituation = gps_data(); pthread_mutex_init(&mySituation->lock, NULL); SituationData snapGps; DataQData * myDataQ; myDataQ = dataq_data(); pthread_mutex_init(&myDataQ->lock, NULL); DataQData snapDataQ; MPUData * myMPUData; myMPUData = mpu_data(); pthread_mutex_init(&myMPUData->lock, NULL); MPUData snapMPUData; BMP180Data * myBMPData; myBMPData = bmp_data(); pthread_mutex_init(&myBMPData->lock, NULL); BMP180Data snapBMPData; if ( pthread_create(&thread[0], NULL, run_gps, NULL) ) { fprintf(stderr, "Failed to create GPS thread.\r\n"); return -1; } pthread_setname_np(thread[0], "GPS"); if ( pthread_create(&thread[1], NULL, run_dataq, NULL) ) { fprintf(stderr, "Failed to create DataQ thread.\r\n"); return -2; } pthread_setname_np(thread[1], "DataQ"); if ( pthread_create(&thread[2], NULL, run_i2c1, NULL) ) { fprintf(stderr, "Failed to create MPU6050/BMP180 thread\r\n"); return -3; } pthread_setname_np(thread[2], "MPU6050_BMP180"); if (wiringPiSetupGpio() >= 0) { if (wiringPiISR(TACH_PIN, INT_EDGE_RISING, &tachInterrupt) < 0) { fprintf(stderr, "Unable to setup Tachometer ISR\r\n"); } } else { fprintf(stderr, "Unable to setup wiringPi\r\n"); } usleep(2000000); while(1) { // Wait for recording signal while((snapDataQ.digital & 0x02) || !(snapDataQ.running)) { usleep(200000); pthread_mutex_lock(&myDataQ->lock); snapDataQ.digital = myDataQ->digital; snapDataQ.running = myDataQ->running; pthread_mutex_unlock(&myDataQ->lock); } // Set log file name clock_gettime(CLOCK_REALTIME, &gettime_now); sprintf(filename, "/mnt/sda1/logs/race%02d%02d%02d.csv", (gettime_now.tv_sec % (60 * 60 * 24)) % 24, (gettime_now.tv_sec % (60 * 60)) % 60, gettime_now.tv_sec % 60); // Open log file logFile = fopen(filename, "a"); if(logFile == NULL) { fprintf(stderr, "Failed to open log file for writing\r\n"); return(-1); } // Write csv header once fprintf(logFile, "runTime,"); fprintf(logFile, "a2dRun,"); fprintf(logFile, "steer,"); fprintf(logFile, "throttle,"); fprintf(logFile, "brake,"); fprintf(logFile, "spare,"); fprintf(logFile, "event,"); // fprintf(logFile, "record,"); fprintf(logFile, "tach,"); fprintf(logFile, "tach2"); fprintf(logFile, "gpsRun,"); // fprintf(logFile, "lastFixTime,"); fprintf(logFile, "gpsLat,"); fprintf(logFile, "gpsLon,"); fprintf(logFile, "gpsAccuracy,"); // fprintf(logFile, "gpsQual,"); fprintf(logFile, "gpsAlt,"); // fprintf(logFile, "gpsVertVel,"); fprintf(logFile, "gpsVertAcc,"); fprintf(logFile, "gpsSpeed,"); fprintf(logFile, "gpsHeading,"); fprintf(logFile, "gpsUTCdate,"); fprintf(logFile, "gpsUTCtime,"); fprintf(logFile, "gpsSatTracked,"); fprintf(logFile, "gpsSatSeen,"); fprintf(logFile, "mpuRun,"); fprintf(logFile, "Gx,"); fprintf(logFile, "Gy,"); fprintf(logFile, "Gz,"); fprintf(logFile, "Gtotal,"); fprintf(logFile, "GyroX,"); fprintf(logFile, "GyroY,"); fprintf(logFile, "GyroZ,"); fprintf(logFile, "mpuDieTemp,"); fprintf(logFile, "ambientTemp,"); fprintf(logFile, "pressureAlt"); fprintf(logFile, "\r\n"); fflush(logFile); clock_gettime(CLOCK_REALTIME, &gettime_now); last_heartbeat = gettime_now.tv_nsec; start_time.tv_sec = gettime_now.tv_sec; // continue to loop while recording signal is on while(!(snapDataQ.digital & 0x02)) { clock_gettime(CLOCK_REALTIME, &gettime_now); heartbeat_difference = gettime_now.tv_nsec - last_heartbeat; if (heartbeat_difference < 0) { heartbeat_difference += 1000000000; } // 30 Hz loop if (heartbeat_difference > 33333333) { // update last heartbeat based on 30 Hz, not period (with jitter) when loop actually executed last_heartbeat += 33333333; if (last_heartbeat > 1000000000) { last_heartbeat -= 1000000000; } // log proces time stamp of sample fprintf(logFile, "% 4d.%03d", gettime_now.tv_sec - start_time.tv_sec, gettime_now.tv_nsec/1000000); // process tach counter once every 6 30 Hz loops (5 Hz) if (++sampleCount >= sampleCounts) { sampleCount = 0; pthread_mutex_lock(&tachLock); tachDiff = tachCounter - lastTach; pthread_mutex_unlock(&tachLock); lastTach += tachDiff; if (tachDiff < sampleCounts * 20) // skip overflow (> 20,000 RPM), about once every 20 minutes { tach = tachDiff * rpmPerCount; } } // process tach timer every 30 Hz loop (average last 8 sparks) pthread_mutex_lock(&tachLock); seven_spark_time = spark_time[spark_count] - spark_time[spark_count ? spark_count - 1 : 7]; pthread_mutex_unlock(&tachLock); if (seven_spark_time < 0 ) { seven_spark_time += 1000000000; } if (seven_spark_time) { tach2 = 140000000 / ( (seven_spark_time + 500) / 1000); } else { tack2 = 0; } // take snapshot of DataQ data generated in seperate thread pthread_mutex_lock(&myDataQ->lock); snapDataQ.running = myDataQ->running; snapDataQ.analog[0] = myDataQ->analog[0]; snapDataQ.analog[1] = myDataQ->analog[1]; snapDataQ.analog[2] = myDataQ->analog[2]; snapDataQ.analog[3] = myDataQ->analog[3]; snapDataQ.digital = myDataQ->digital; pthread_mutex_unlock(&myDataQ->lock); // log DataQ data samples fprintf(logFile, ",%d", snapDataQ.running); fprintf(logFile, ",% 4d", snapDataQ.analog[0]); fprintf(logFile, ",% 4d", snapDataQ.analog[1]); fprintf(logFile, ",% 4d", snapDataQ.analog[2]); fprintf(logFile, ",% 4d", snapDataQ.analog[3]); fprintf(logFile, ",%d", snapDataQ.digital & 1); // fprintf(logFile, ",%d", (snapDataQ.digital >> 1) & 1); // log Tachometer sample fprintf(logFile, ",% 4d", tach); fprintf(logFile, ",% 4d", tach2); // take snapshot of GPS data generated in seperate thread pthread_mutex_lock(&mySituation->lock); snapGps.running = mySituation->running; snapGps.lastFixSinceMidnightUTC = mySituation->lastFixSinceMidnightUTC; snapGps.Lat = mySituation->Lat; snapGps.Lng = mySituation->Lng; snapGps.GeoidSep = mySituation->GeoidSep; snapGps.Accuracy = mySituation->Accuracy; snapGps.quality = mySituation->quality; snapGps.Alt = mySituation->Alt; snapGps.GPSVertVel = mySituation->GPSVertVel; snapGps.AccuracyVert = mySituation->AccuracyVert; snapGps.GroundSpeed = mySituation->GroundSpeed; snapGps.TrueCourse = mySituation->TrueCourse; snapGps.day = mySituation->day; snapGps.month = mySituation->month; snapGps.year = mySituation->year; snapGps.hour = mySituation->hour; snapGps.minute = mySituation->minute; snapGps.second = mySituation->second; snapGps.SatellitesTracked = mySituation->SatellitesTracked; snapGps.SatellitesSeen = mySituation->SatellitesSeen; pthread_mutex_unlock(&mySituation->lock); // log GPS samples fprintf(logFile, ",%d", snapGps.running); // fprintf(logFile, ",%d", snapGps.lastFixSinceMidnightUTC); // Time of last GPS fix, in seconds, since midnight UTC fprintf(logFile, ",%11.7f", snapGps.Lat); // GPS Latitude in degrees fprintf(logFile, ",%12.7f", snapGps.Lng); // GPS Longitude in degrees fprintf(logFile, ",%4.1f", snapGps.Accuracy); // GPS Horizontal accuracy in meters (2-sigma) // fprintf(logFile, ",%d", snapGps.quality); // GPS Fix quality {2 = WAAS/DGPS}} fprintf(logFile, ",%4.f", snapGps.Alt); // GPS Altitude in feet // fprintf(logFile, ",%.1f", snapGps.GPSVertVel); // GPS Vertical Velocity in ft/sec fprintf(logFile, ",%5.1f", snapGps.AccuracyVert); // GPS Vertical accuracy in meters fprintf(logFile, ",% 3d", snapGps.GroundSpeed); // GPS Ground speed in Knots fprintf(logFile, ",% 3d", snapGps.TrueCourse); // GPS True Course in degrees fprintf(logFile, ",%02d-%02d-%04d", snapGps.day, snapGps.month, snapGps.year); // GPS UTC date fprintf(logFile, ",%02d:%02d:%02d", snapGps.hour, snapGps.minute, snapGps.second); // GPS UTC time fprintf(logFile, ",% 2d", snapGps.SatellitesTracked); // GPS Satellites tracked (above horizon) fprintf(logFile, ",% 2d", snapGps.SatellitesSeen); // GPS Satellites seen (signal received) // take snapshot of MPU-6050 data generated in seperate thread pthread_mutex_lock(&myMPUData->lock); snapMPUData.running = myMPUData->running; snapMPUData.Gx = myMPUData->Gx; snapMPUData.Gy = myMPUData->Gy; snapMPUData.Gz = myMPUData->Gz; snapMPUData.Gtotal = myMPUData->Gtotal; snapMPUData.Gyrox = myMPUData->Gyrox; snapMPUData.Gyroy = myMPUData->Gyroy; snapMPUData.Gyroz = myMPUData->Gyroz; snapMPUData.Temperature = myMPUData->Temperature; pthread_mutex_unlock(&myMPUData->lock); // log MPU-6050 data samples fprintf(logFile, ",%d", snapMPUData.running); fprintf(logFile, ",%+.2f", snapMPUData.Gx); // MPU6050 Accelerometer X-axis fprintf(logFile, ",%+.2f", snapMPUData.Gy); // MPU6050 Accelerometer Y-axis fprintf(logFile, ",%+.2f", snapMPUData.Gz); // MPU6050 Accelerometer Z-axis fprintf(logFile, ",%+.2f", snapMPUData.Gtotal); // Calculated total Gs fprintf(logFile, ",%+.1f", snapMPUData.Gyrox); // Rotational rate about X-axis fprintf(logFile, ",%+.1f", snapMPUData.Gyroy); // Rotational rate about Y-axis fprintf(logFile, ",%+.1f", snapMPUData.Gyroz); // Rotational rate about Z-axis fprintf(logFile, ",%5.1f", snapMPUData.Temperature); // MPU6050 die temperature // take snapshot of BMP180 data generated in seperate thread pthread_mutex_lock(&myBMPData->lock); snapBMPData.temperature = myBMPData->temperature; snapBMPData.altitude = myBMPData->altitude; pthread_mutex_unlock(&myBMPData->lock); fprintf(logFile, ",%5.1f", snapBMPData.temperature); fprintf(logFile, ",%6.1f", snapBMPData.altitude); fprintf(logFile, "\r\n"); // create new line for next scan fflush(logFile); } usleep(2000); // wait before checking for next scan completion } // Recording signal is off, close log file fclose(logFile); } // nothing below this line ever executes, but is included for proper programming etiquette // send stop signal to sub-threads stop_gps(); stop_dataq(); stop_i2c1(); // clean up the mutexs pthread_mutex_destroy(&mySituation->lock); pthread_mutex_destroy(&myDataQ->lock); pthread_mutex_destroy(&myBMPData->lock); // wait for the sub-threads to complete before closing main process pthread_join(thread[0], NULL); pthread_join(thread[1], NULL); pthread_join(thread[2], NULL); return 0; // this line never executes }
int main(int argc, char* argv[]){ int i, j; // (0) leer parametros options opt; if (argc == 1) { print_help(argv[0]); return 0; } if (read_options(argc, argv, &opt)) { printf("ERROR reading parameters\n"); return 1; } int len1 = strlen(opt.file1); int len2 = strlen(opt.file2); if (strcmp(&(opt.file1[len1-4]),".bmp") || strcmp(&(opt.file2[len2-4]),".bmp")) { printf("ERROR: nombre del archivo\n"); return -1; } // (0.1) siempre armo el summary opt.summary = (int*)malloc(sizeof(int)*256); for (i=0; i<256; i++) { opt.summary[i]=0; } // (1) leer imagenes BMP* bmp1 = bmp_read(opt.file1); BMP* bmp2 = bmp_read(opt.file2); if (bmp1 == 0 || bmp1 == 0) { printf("ERROR: no se puede abrir el archivo\n"); return -1; } // (2) check tipo de archivo if (((BMPIH*)(bmp1->ih))->biSize != ((BMPIH*)(bmp1->ih))->biSize) { printf("ERROR: tipo de archivo diferente\n"); return -1; } // (3) check tamaño del archivo int w1 = ((BMPIH*)(bmp1->ih))->biWidth; int h1 = ((BMPIH*)(bmp1->ih))->biHeight; int c1 = ((BMPIH*)(bmp1->ih))->biBitCount; int w2 = ((BMPIH*)(bmp2->ih))->biWidth; int h2 = ((BMPIH*)(bmp2->ih))->biHeight; int c2 = ((BMPIH*)(bmp2->ih))->biBitCount; if (w1!=w2 || h1!=h2 || c1!=c2) { printf("ERROR: tamaño de archivo diferente\n"); return -1; } //printf("%i=%i %i=%i %i=%i\n",w1,w2,h1,h2,c1,c2); if (w1 % 4 != 0) { // TODO: soportar padding! printf("ERROR: padding no soportado\n"); return -1; } // (3) check el bit count TODO: only 24 o 32 if (c1 != 24 && c1 != 32) { printf("ERROR: (%i) bitcount distinto de 24 o 32\n", c1); return -1; } // (4) crear imagenes de diferencias BMP *bmpDiffR, *bmpDiffG, *bmpDiffB, *bmpDiffA; bmpDiffR = bmp_copy(bmp1, 0); bmpDiffG = bmp_copy(bmp1, 0); bmpDiffB = bmp_copy(bmp1, 0); if (c1 == 32) { bmpDiffA = bmp_copy(bmp1,0); } // (5) extraer data uint8_t *data1, *data2, *dataR, *dataG, *dataB, *dataA; data1 = bmp_data(bmp1); data2 = bmp_data(bmp2); dataR = bmp_data(bmpDiffR); dataG = bmp_data(bmpDiffG); dataB = bmp_data(bmpDiffB); if (c1 == 32) { dataA = bmp_data(bmpDiffA); } // (6) calcular diferencias if (c1 == 32) { for(j=0;j<h1;j++) { for(i=0;i<w1;i++) { int pos = (j*w1+i)*4; uint8_t R1 = data1[pos+3]; uint8_t G1 = data1[pos+2]; uint8_t B1 = data1[pos+1]; uint8_t A1 = data1[pos+0]; uint8_t R2 = data2[pos+3]; uint8_t G2 = data2[pos+2]; uint8_t B2 = data2[pos+1]; uint8_t A2 = data2[pos+0]; dataR[pos+0] = cmp(R1,R2,i,j,"R",&opt); dataR[pos+1] = dataR[pos+0]; dataR[pos+2] = dataR[pos+30]; dataR[pos+3] = 255; dataG[pos+0] = cmp(G1,G2,i,j,"G",&opt); dataG[pos+1] = dataG[pos+0]; dataG[pos+2] = dataG[pos+0]; dataG[pos+3] = 255; dataB[pos+0] = cmp(B1,B2,i,j,"B",&opt); dataB[pos+1] = dataB[pos+0]; dataB[pos+2] = dataB[pos+0]; dataB[pos+3] = 255; dataA[pos+0] = cmp(A1,A2,i,j,"A",&opt); dataA[pos+1] = dataA[pos+0]; dataA[pos+2] = dataA[pos+0]; dataA[pos+3] = 255; } } } else if(c1 == 24) { for(j=0;j<h1;j++) { for(i=0;i<w1;i++) { int pos = (j*w1+i)*3; uint8_t R1 = data1[pos+2]; uint8_t G1 = data1[pos+1]; uint8_t B1 = data1[pos+0]; uint8_t R2 = data2[pos+2]; uint8_t G2 = data2[pos+1]; uint8_t B2 = data2[pos+0]; dataR[pos+2] = cmp(R1,R2,i,j,"R",&opt); dataR[pos+1] = dataR[pos+2]; dataR[pos+0] = dataR[pos+2]; dataG[pos+2] = cmp(G1,G2,i,j,"G",&opt); dataG[pos+1] = dataG[pos+2]; dataG[pos+0] = dataG[pos+2]; dataB[pos+2] = cmp(B1,B2,i,j,"B",&opt); dataB[pos+1] = dataB[pos+2]; dataB[pos+0] = dataB[pos+2]; } } } // (7) mostrar summary if(opt.summaryop) { for(i=1;i<256;i++) { if(opt.summary[i]!=0) { printf("%i\t%i\n", i, opt.summary[i]); } } } // (8) guardar resultados if(opt.image) { char* strX = "diffX.bmp"; char* fileSto = malloc(strlen(opt.file1) + 5 + 1); strcpy(fileSto, opt.file1); strcpy(fileSto + len1 - 4, strX); fileSto[len1]='R'; bmp_save(fileSto, bmpDiffR); fileSto[len1]='G'; bmp_save(fileSto, bmpDiffG); fileSto[len1]='B'; bmp_save(fileSto, bmpDiffB); fileSto[len1]='A'; if (c1 == 32) { bmp_save(fileSto,bmpDiffA); } // (8.1) borrar las imagenes bmp_delete(bmp1); bmp_delete(bmp2); bmp_delete(bmpDiffR); bmp_delete(bmpDiffG); bmp_delete(bmpDiffB); if (c1 == 32) { bmp_delete(bmpDiffA); } } // (9) retorno error si encontre una diferencia for (i = opt.epsilon+1; i < 256; i++) { if (opt.summary[i] > 0) { return -1; } } return 0; }