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 #include <sys/types.h>
00029 #include <sys/stat.h>
00030 #include <fcntl.h>
00031 #include <stdio.h>
00032 #include <unistd.h>
00033 #include <stdlib.h>
00034 #include <string.h>
00035 #include <termios.h>
00036 #include <asm/elphel/exifa.h>
00037 #include <asm/byteorder.h>
00038
00039 #define EXIF_DEV "/dev/exif_meta"
00040 #define PROGRAM_SERIAL 0
00041 #define D(x)
00042
00043
00044 void exif_init_compass_meta(struct meta_CompassInfo_t *meta) {
00045 meta->CompassDirection_nom= __cpu_to_be32((int) 0);
00046 meta->CompassDirection_denom= __cpu_to_be32((int) EXIF_GPS_COMPASS_DENOM);
00047 meta->CompassPitchRef= 'N';
00048 meta->CompassPitch_nom= __cpu_to_be32((int) 0);
00049 meta->CompassPitch_denom= __cpu_to_be32((int) EXIF_GPS_COMPASS_DENOM);
00050 meta->CompassRollRef= 'E';
00051 meta->CompassRoll_nom= __cpu_to_be32((int) 0);
00052 meta->CompassRoll_denom= __cpu_to_be32((int) EXIF_GPS_COMPASS_DENOM);
00053 }
00054
00055
00056 int main(int argc, char *argv[]) {
00057 FILE *fcompass;
00058 int fd_exif,i;
00059 char response[256];
00060 int response_len;
00061 char * cp;
00062 char * cp_cs;
00063 char * cp_c;
00064 char * cp_r;
00065 char * cp_p;
00066 char * cp_t;
00067 double dc=0, dp=0,dr=0,dt=0;
00068 int icourse,ipitch,iroll;
00069 unsigned char cs_calc, cs_read;
00070 int upside_down=-1;
00071 struct meta_CompassInfo_t meta;
00072 #if PROGRAM_SERIAL
00073 int fd_compass;
00074 struct termios termio;
00075 #endif
00076 exif_init_compass_meta(&meta);
00077 if (argc < 2) {
00078 printf("Usage: %s compass_device [u]\n", argv[0]);
00079 exit (-1);
00080 }
00081 if (argc >2) {
00082 if (argv[2][0]=='u') upside_down=1;
00083 }
00084
00085 fd_exif = open(EXIF_DEV, O_RDWR);
00086 if (fd_exif<=0) {
00087 fprintf(stderr,"Can not open device file "EXIF_DEV);
00088 exit (-1);
00089 }
00090 #if PROGRAM_SERIAL
00091 fd_compass = open(argv[1], O_RDWR);
00092 if (fd_compass == -1) {
00093 perror("Cannot open compass device");
00094 close(fd_exif);
00095 exit (-1);
00096 }
00097 tcgetattr(fd_compass, &termio);
00099
00100
00101
00103 cfsetospeed(&termio, B19200);
00104 tcsetattr(fd_compass, TCIOFLUSH, &termio);
00105 #endif
00106
00107 #if PROGRAM_SERIAL
00108 fcompass=fopen(argv[1], "r+");
00109 #else
00110 fcompass=fopen(argv[1], "r");
00111 #endif
00112 if (!fcompass) {
00113 perror("Cannot open compass device!");
00114 exit (-1);
00115 }
00116
00117 while (1) {
00118 fgets(response,sizeof(response),fcompass);
00119 if ((cp=strchr(response,'\n'))) cp[0]='\0';
00120 if ((cp=strchr(response,'\r'))) cp[0]='\0';
00121
00123 response_len=strlen(response);
00124 if (response_len>0) {
00125 if (response_len>4) {
00126 if ((cp_cs=strchr(response,'*'))) {
00127 cp_cs[0]='\0';
00128 cp_cs++;
00129 cs_read= strtol(cp_cs, NULL, 16);
00130 cs_calc=0;
00131 for (i=1; response[i]; i++) cs_calc^=response[i];
00132
00133 if (cs_calc==cs_read ) {
00134 D(printf ("Got NMEA string >>%s<<\n",response+1));
00135 cp_c=strchr(response,'C');
00136 cp_r=strchr(response,'R');
00137 cp_p=strchr(response,'P');
00138 cp_t=strchr(response,'T');
00139 if (cp_c) {cp_c[0]='\0';cp_c++;}
00140 if (cp_r) {cp_r[0]='\0';cp_r++;}
00141 if (cp_p) {cp_p[0]='\0';cp_p++;}
00142 if (cp_t) {cp_t[0]='\0';cp_t++;}
00143
00144 if (cp_c) dc=strtod(cp_c, NULL);
00145 if (cp_r) dr=strtod(cp_r, NULL);
00146 if (cp_p) dp=strtod(cp_p, NULL);
00147 if (cp_t) dt=strtod(cp_t, NULL);
00148 D(printf("course=%f, roll=%f, pitch=%f, temperature=%f\n",dc,dr,dp,dt));
00149
00150
00151 if (upside_down<0) {
00152 upside_down= ((dr>90) || (dr<-90))? 1:0;
00153 }
00154 if (upside_down) {
00155 dr+=180.0;
00156 if (dr>180) dr-=360;
00157 }
00158 icourse= (dc* EXIF_GPS_COMPASS_DENOM) +0.5;
00159 ipitch= (dp* EXIF_GPS_COMPASS_DENOM) +0.5;
00160 iroll= (dr* EXIF_GPS_COMPASS_DENOM) +0.5;
00161 meta.CompassDirection_nom= __cpu_to_be32((int) icourse);
00162 if (ipitch<0) {
00163 meta.CompassPitchRef= EXIF_COMPASS_PITCH_ASCII[1];
00164 ipitch= -ipitch;
00165 } else meta.CompassPitchRef= EXIF_COMPASS_PITCH_ASCII[0];
00166 meta.CompassPitch_nom= __cpu_to_be32((int) ipitch);
00167 if (iroll<0) {
00168 meta.CompassRollRef= EXIF_COMPASS_ROLL_ASCII[1];
00169 iroll= -iroll;
00170 } else meta.CompassRollRef= EXIF_COMPASS_ROLL_ASCII[0];
00171 meta.CompassRoll_nom= __cpu_to_be32((int) iroll);
00172
00173 lseek (fd_exif,Exif_GPSInfo_CompassDirection,SEEK_END);
00174 write (fd_exif, &meta, sizeof(meta));
00175 } else D(printf("Checksum mismatch: read 0x%x, calculated 0x%x\n",(int)cs_read,(int)cs_calc));
00176 } else D(printf("No '*' in response, ignoring\n"));
00177 } else D(printf("response to short (%d), ignoring\n",response_len));
00178 }
00179 }
00180 return 0;
00181 }