00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00033 #include <unistd.h>
00034 #include <stdio.h>
00035 #include <stdlib.h>
00036 #include <signal.h>
00037 #include <fcntl.h>
00038 #include <sys/uio.h>
00039 #include <errno.h>
00040 #include <sys/types.h>
00041 #include <sys/socket.h>
00042 #include <sys/stat.h>
00043
00044
00045 #include <time.h>
00046 #include <string.h>
00047
00048 #include <netinet/in.h>
00049 #include <sys/mman.h>
00050 #include <sys/ioctl.h>
00051
00052 #include <asm/elphel/c313a.h>
00053 #include <asm/elphel/ext353.h>
00054 #include <asm/byteorder.h>
00055
00056
00057 #include <ogg/ogg.h>
00058 #include "ogmstreams.h"
00059
00060 #include "camogm_kml.h"
00061 #include "camogm.h"
00062 const char ExifDirFileName[]="/dev/exif_metadir";
00063
00065 int camogm_init_kml(void) {
00066 return 0;
00067 }
00068 void camogm_free_kml(void) {
00069 }
00070
00071
00072
00074
00076
00077
00078
00079
00080
00083
00084
00085
00086
00087
00088
00089 int camogm_start_kml(void) {
00090
00094 struct exif_dir_table_t dir_table_entry;
00095 int fd_ExifDir;
00096 int indx;
00097 for (indx=0;indx<ExifKmlNumber;indx++) state->kml_exif[indx].ltag=0;
00099 fd_ExifDir=open(ExifDirFileName,O_RDONLY);
00100 if (fd_ExifDir<0) {
00101 D0(fprintf (debug_file,"Error opening %s\n", ExifDirFileName));
00102 return -CAMOGM_FRAME_FILE_ERR;
00103 }
00104 while (read(fd_ExifDir, &dir_table_entry, sizeof(dir_table_entry))>0) {
00105 switch (dir_table_entry.ltag) {
00106 case Exif_Photo_DateTimeOriginal: indx= Exif_Photo_DateTimeOriginal_Index; break;
00107 case Exif_GPSInfo_GPSLatitudeRef: indx= Exif_GPSInfo_GPSLatitudeRef_Index; break;
00108 case Exif_GPSInfo_GPSLatitude: indx= Exif_GPSInfo_GPSLatitude_Index ; break;
00109 case Exif_GPSInfo_GPSLongitudeRef: indx= Exif_GPSInfo_GPSLongitudeRef_Index ; break;
00110 case Exif_GPSInfo_GPSLongitude: indx= Exif_GPSInfo_GPSLongitude_Index; break;
00111 case Exif_GPSInfo_GPSAltitudeRef: indx= Exif_GPSInfo_GPSAltitudeRef_Index; break;
00112 case Exif_GPSInfo_GPSAltitude: indx= Exif_GPSInfo_GPSAltitude_Index; break;
00113 case Exif_GPSInfo_GPSTimeStamp: indx= Exif_GPSInfo_GPSTimeStamp_Index; break;
00114 case Exif_GPSInfo_GPSDateStamp: indx= Exif_GPSInfo_GPSDateStamp_Index; break;
00115 case Exif_GPSInfo_CompassDirectionRef: indx= Exif_GPSInfo_CompassDirectionRef_Index; break;
00116 case Exif_GPSInfo_CompassDirection: indx= Exif_GPSInfo_CompassDirection_Index; break;
00117 case Exif_GPSInfo_CompassPitchRef: indx= Exif_GPSInfo_CompassPitchRef_Index; break;
00118 case Exif_GPSInfo_CompassPitch: indx= Exif_GPSInfo_CompassPitch_Index; break;
00119 case Exif_GPSInfo_CompassRollRef: indx= Exif_GPSInfo_CompassRollRef_Index; break;
00120 case Exif_GPSInfo_CompassRoll: indx= Exif_GPSInfo_CompassRoll_Index; break;
00121 default: indx=-1;
00122 }
00123 if (indx>=0) {
00124 memcpy(&(state->kml_exif[indx]),&dir_table_entry,sizeof(dir_table_entry));
00125 D2(fprintf(debug_file,"indx=%02d, ltag=0x%05x, len=0x%03x, src=0x%03x, dst=0x%03x\n",indx, \
00126 (int) dir_table_entry.ltag, \
00127 (int) dir_table_entry.len, \
00128 (int) dir_table_entry.src, \
00129 (int) dir_table_entry.dst));
00130 }
00131 }
00132 close (fd_ExifDir);
00133 sprintf(state->kml_path,"%s%010ld_%06ld.kml",state->path_prefix,state->this_frame_params.timestamp_sec,state->this_frame_params.timestamp_usec);
00134 if (!((state->kml_file=fopen(state->kml_path,"w+"))) ){
00135 D0(fprintf (debug_file, "Error opening %s for writing\n", state->kml_path));
00136 return -CAMOGM_FRAME_FILE_ERR;
00137 }
00139 fprintf (state->kml_file,"<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n" \
00140 "<kml xmlns=\"http://earth.google.com/kml/2.2\">\n");
00141 fprintf (state->kml_file,"<Document>\n");
00142 state->kml_used= 1;
00143 return 0;
00144 }
00145
00146 int camogm_frame_kml(void){
00147 char JPEGFileName[300];
00148 char * filename;
00149 int fd_JPEG;
00150 int i,j;
00151 ssize_t iovlen,l;
00152 struct iovec chunks_iovec[7];
00153 char datestr[11];
00154 double longitude=0.0, latitude=0.0, altitude=0.0, heading=0.0, tilt=0.0, roll=0.0, pitch=0.0;
00155 int hours=0, minutes=0;
00156 double seconds=0.0;
00157 int * ip;
00158 if (state->kml_file) {
00159 i=state->this_frame_params.timestamp_sec - (state->kml_last_ts + state->kml_period);
00160 if ((i>1) || ((i==0) && ( state->this_frame_params.timestamp_usec > state->kml_last_uts ))) {
00161
00162 state->kml_last_ts=state->this_frame_params.timestamp_sec;
00163 state->kml_last_uts=state->this_frame_params.timestamp_usec;
00164 if (state->format==CAMOGM_FORMAT_JPEG) {
00165 strcpy (JPEGFileName, state->path);
00166 } else {
00167 sprintf(JPEGFileName,"%s%010ld_%06ld.jpeg",state->path_prefix,state->this_frame_params.timestamp_sec,state->this_frame_params.timestamp_usec);
00168 if (((fd_JPEG=open(JPEGFileName,O_RDWR | O_CREAT, 0777)))>=0){
00169 l=0;
00170 for (i=0; i< (state->chunk_index)-1; i++) {
00171 chunks_iovec[i].iov_base=state->packetchunks[i+1].chunk;
00172 chunks_iovec[i].iov_len= state->packetchunks[i+1].bytes;
00173 l+=chunks_iovec[i].iov_len;
00174 }
00175 iovlen=writev(fd_JPEG,chunks_iovec, (state->chunk_index)-1);
00176 if (iovlen < l) {
00177 j=errno;
00178 D0(fprintf(debug_file,"writev error %d (returned %d, expected %d)\n",j,iovlen,l));
00179 close (fd_JPEG);
00180 return -CAMOGM_FRAME_FILE_ERR;
00181 }
00182 close (fd_JPEG);
00183 } else {
00184 D0(fprintf (debug_file, "Error opening %s for writing, returned %d, errno=%d\n", JPEGFileName,fd_JPEG,errno));
00185 return -CAMOGM_FRAME_FILE_ERR;
00186 }
00187 }
00188
00190 filename=strrchr(JPEGFileName, '/');
00191 filename[0]='\0';
00192 filename++;
00195 if (state->kml_exif[Exif_GPSInfo_GPSDateStamp_Index].ltag==Exif_GPSInfo_GPSDateStamp) {
00196 memcpy(datestr, &(state->ed[state->kml_exif[Exif_GPSInfo_GPSDateStamp_Index].dst]) ,10);
00197 datestr[4]='-';datestr[7]='-';datestr[10]='\0';
00198 }
00199 if (state->kml_exif[Exif_GPSInfo_GPSTimeStamp_Index].ltag==Exif_GPSInfo_GPSTimeStamp) {
00200 ip= (int *) &(state->ed[state->kml_exif[Exif_GPSInfo_GPSTimeStamp_Index].dst]);
00201 hours= __cpu_to_be32( ip[0]);
00202 minutes= __cpu_to_be32( ip[2]);
00203 seconds= (1.0*(__cpu_to_be32( ip[4])+1))/__cpu_to_be32( ip[5]);
00204 D2(fprintf (debug_file,"(when) 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x\n",ip[0],ip[1],ip[2],ip[3],ip[4],ip[5]));
00205 D2(fprintf (debug_file,"(when) 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x\n",__cpu_to_be32(ip[0]), \
00206 __cpu_to_be32(ip[1]), \
00207 __cpu_to_be32(ip[2]), \
00208 __cpu_to_be32(ip[3]), \
00209 __cpu_to_be32(ip[4]), \
00210 __cpu_to_be32(ip[5])));
00211 }
00212 D1(fprintf (debug_file,"when=%sT%02d:%02d:%05.2fZ\n",datestr, hours, minutes, seconds));
00213
00215 if (state->kml_exif[Exif_GPSInfo_GPSLongitude_Index].ltag==Exif_GPSInfo_GPSLongitude) {
00216 ip= (int *) &(state->ed[state->kml_exif[Exif_GPSInfo_GPSLongitude_Index].dst]);
00217 longitude=__cpu_to_be32( ip[0])/(1.0*__cpu_to_be32( ip[1])) + __cpu_to_be32( ip[2])/(60.0*__cpu_to_be32( ip[3]));
00218 if ((state->kml_exif[Exif_GPSInfo_GPSLongitudeRef_Index].ltag==Exif_GPSInfo_GPSLongitudeRef) &&
00219 (state->ed[state->kml_exif[Exif_GPSInfo_GPSLongitudeRef_Index].dst] != 'E')) longitude=-longitude;
00220 D2(fprintf (debug_file,"(longitude) 0x%x 0x%x 0x%x 0x%x '%c'\n",ip[0],ip[1],ip[2],ip[3],state->ed[state->kml_exif[Exif_GPSInfo_GPSLongitudeRef_Index].dst]));
00221 }
00222
00223 if (state->kml_exif[Exif_GPSInfo_GPSLatitude_Index].ltag==Exif_GPSInfo_GPSLatitude) {
00224 ip= (int *) &(state->ed[state->kml_exif[Exif_GPSInfo_GPSLatitude_Index].dst]);
00225 latitude=__cpu_to_be32( ip[0])/(1.0*__cpu_to_be32( ip[1])) + __cpu_to_be32( ip[2])/(60.0*__cpu_to_be32( ip[3]));
00226 if ((state->kml_exif[Exif_GPSInfo_GPSLatitudeRef_Index].ltag==Exif_GPSInfo_GPSLatitudeRef) &&
00227 (state->ed[state->kml_exif[Exif_GPSInfo_GPSLatitudeRef_Index].dst] != 'N')) latitude=-latitude;
00228 D2(fprintf (debug_file,"(latitude) 0x%x 0x%x 0x%x 0x%x '%c'\n",ip[0],ip[1],ip[2],ip[3],state->ed[state->kml_exif[Exif_GPSInfo_GPSLatitudeRef_Index].dst]?'-':'+'));
00229 }
00231 if (state->kml_exif[Exif_GPSInfo_GPSAltitude_Index].ltag==Exif_GPSInfo_GPSAltitude) {
00232 ip= (int *) &(state->ed[state->kml_exif[Exif_GPSInfo_GPSAltitude_Index].dst]);
00233 altitude=(1.0*__cpu_to_be32( ip[0]))/__cpu_to_be32( ip[1]);
00234 if ((state->kml_exif[Exif_GPSInfo_GPSAltitudeRef_Index].ltag==Exif_GPSInfo_GPSAltitudeRef) &&
00235 (state->ed[state->kml_exif[Exif_GPSInfo_GPSAltitudeRef_Index].dst] != '\0')) altitude=-altitude;
00236 D2(fprintf (debug_file,"(altitude) 0x%x 0x%x '%c'\n",ip[0],ip[1],state->ed[state->kml_exif[Exif_GPSInfo_GPSAltitudeRef_Index].dst]));
00237 }
00238
00239 D1(fprintf (debug_file,"longitude=%f, latitude=%f, altitude=%f\n",longitude, latitude, altitude));
00240
00242 if (state->kml_exif[Exif_GPSInfo_CompassDirection_Index].ltag==Exif_GPSInfo_CompassDirection) {
00243 ip= (int *) &(state->ed[state->kml_exif[Exif_GPSInfo_CompassDirection_Index].dst]);
00244 heading=(1.0*__cpu_to_be32( ip[0]))/__cpu_to_be32( ip[1]);
00245 D2(fprintf (debug_file,"(heading) 0x%x 0x%x\n",ip[0],ip[1]));
00246 }
00248 if (state->kml_exif[Exif_GPSInfo_CompassRoll_Index].ltag==Exif_GPSInfo_CompassRoll) {
00249 ip= (int *) &(state->ed[state->kml_exif[Exif_GPSInfo_CompassRoll_Index].dst]);
00250 roll=__cpu_to_be32( ip[0])/(1.0*__cpu_to_be32( ip[1])) + __cpu_to_be32( ip[2])/(60.0*__cpu_to_be32( ip[3]));
00251 if ((state->kml_exif[Exif_GPSInfo_CompassRollRef_Index].ltag==Exif_GPSInfo_CompassRollRef) &&
00252 (state->ed[state->kml_exif[Exif_GPSInfo_CompassRollRef_Index].dst] != EXIF_COMPASS_ROLL_ASCII[0])) roll=-roll;
00253 D2(fprintf (debug_file,"(roll) 0x%x 0x%x '%c'\n",ip[0],ip[1],state->ed[state->kml_exif[Exif_GPSInfo_CompassRollRef_Index].dst]));
00254 }
00255
00256 if (state->kml_exif[Exif_GPSInfo_CompassPitch_Index].ltag==Exif_GPSInfo_CompassPitch) {
00257 ip= (int *) &(state->ed[state->kml_exif[Exif_GPSInfo_CompassPitch_Index].dst]);
00258 pitch=__cpu_to_be32( ip[0])/(1.0*__cpu_to_be32( ip[1])) + __cpu_to_be32( ip[2])/(60.0*__cpu_to_be32( ip[3]));
00259 if ((state->kml_exif[Exif_GPSInfo_CompassPitchRef_Index].ltag==Exif_GPSInfo_CompassPitchRef) &&
00260 (state->ed[state->kml_exif[Exif_GPSInfo_CompassPitchRef_Index].dst] != EXIF_COMPASS_PITCH_ASCII[0])) pitch=-pitch;
00261 D2(fprintf (debug_file,"(pitch) 0x%x 0x%x '%c'\n",ip[0],ip[1],state->ed[state->kml_exif[Exif_GPSInfo_CompassPitchRef_Index].dst]));
00262 }
00264 tilt=roll+90.0;
00265 if (tilt <0.0) tilt=0;
00266 else if (tilt > 180.0) tilt=180.0;
00267 D2(fprintf (debug_file,"heading=%f, roll=%f, pitch=%f, tilt=%f\n",heading, roll, pitch, tilt));
00268
00270 altitude=(state->kml_height_mode?altitude:0.0)+state->kml_height;
00271
00272
00274 fprintf(state->kml_file,"<PhotoOverlay>\n" \
00275 " <shape>rectangle</shape>\n" \
00276 " <TimeStamp>\n" \
00277 " <when>%sT%02d:%02d:%05.2fZ</when>\n" \
00278 " </TimeStamp>\n" \
00279 " <Icon>\n" \
00280 " <href>%s</href>\n" \
00281 " </Icon>\n" \
00282 " <Camera>\n" \
00283 " <longitude>%f</longitude>\n" \
00284 " <latitude>%f</latitude>\n" \
00285 " <altitude>%f</altitude>\n" \
00286 " <heading>%f</heading>\n" \
00287 " <tilt>%f</tilt>\n" \
00288 " <roll>%f</roll>\n" \
00289 " <altitudeMode>%s</altitudeMode>\n" \
00290 " </Camera>\n" \
00291 " <ViewVolume>\n" \
00292 " <leftFov>%f</leftFov>\n" \
00293 " <rightFov>%f</rightFov>\n" \
00294 " <bottomFov>%f</bottomFov>\n" \
00295 " <topFov>%f</topFov>\n" \
00296 " <near>%f</near>\n" \
00297 " </ViewVolume>\n" \
00298 "</PhotoOverlay>\n", \
00299 datestr, hours, minutes, seconds, \
00300 filename, longitude,latitude,altitude,heading,tilt,roll,state->kml_height_mode?"absolute":"relativeToGround", \
00301 -(state->kml_horHalfFov), state->kml_horHalfFov, -(state->kml_vertHalfFov), state->kml_vertHalfFov, state->kml_near);
00302 }
00303 }
00304 return 0;
00305 }
00306
00307
00308 int camogm_end_kml(void){
00309
00310 if (state->kml_file) {
00311 fprintf (state->kml_file,"</Document>\n");
00312 fprintf (state->kml_file,"</kml>\n");
00313 fclose(state->kml_file);
00314 state->kml_file=NULL;
00315 }
00316 return 0;
00317 }
00318