apps/compass/compass.c

Go to the documentation of this file.

/*!***************************************************************************
*! FILE NAME  : compass.c
*! DESCRIPTION: converts output from the compass module (now OS5000) to Exif
*!              in Elphel cameras 
*! Copyright (C) 2002-2007 Elphel, Inc.
*! -----------------------------------------------------------------------------**
*!  This program is free software: you can redistribute it and/or modify
*!  it under the terms of the GNU General Public License as published by
*!  the Free Software Foundation, either version 3 of the License, or
*!  (at your option) any later version.
*!
*!  This program is distributed in the hope that it will be useful,
*!  but WITHOUT ANY WARRANTY; without even the implied warranty of
*!  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*!  GNU General Public License for more details.
*!
*!  You should have received a copy of the GNU General Public License
*!  along with this program.  If not, see <http://www.gnu.org/licenses/>.
*! -----------------------------------------------------------------------------**
*!  $Log: compass.c,v $
*!  Revision 1.1  2008/04/07 09:16:31  elphel
*!  converting OS-5000 compass output to Exif
*!
*/
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <termios.h>
#include <asm/elphel/exifa.h>
#include <asm/byteorder.h> //endians

#define EXIF_DEV "/dev/exif_meta"
#define PROGRAM_SERIAL 0
#define D(x)
//#define D(x) x

void exif_init_compass_meta(struct meta_CompassInfo_t *meta) {
   meta->CompassDirection_nom=   __cpu_to_be32((int) 0);
   meta->CompassDirection_denom= __cpu_to_be32((int) EXIF_GPS_COMPASS_DENOM);
   meta->CompassPitchRef=       'N';
   meta->CompassPitch_nom=       __cpu_to_be32((int) 0);
   meta->CompassPitch_denom=     __cpu_to_be32((int) EXIF_GPS_COMPASS_DENOM);
   meta->CompassRollRef=        'E';
   meta->CompassRoll_nom=        __cpu_to_be32((int) 0);
   meta->CompassRoll_denom=      __cpu_to_be32((int) EXIF_GPS_COMPASS_DENOM);
}


int main(int argc, char *argv[]) {
   FILE *fcompass;
   int fd_exif,i;
   char response[256];
   int  response_len;
   char * cp;
   char * cp_cs; // pointer to nmea CS 
   char * cp_c;  // course
   char * cp_r;  // roll
   char * cp_p;  // pitch
   char * cp_t;  // temperature
   double dc=0, dp=0,dr=0,dt=0;
   int icourse,ipitch,iroll;
   unsigned char cs_calc, cs_read;
   int    upside_down=-1; 
   struct meta_CompassInfo_t meta;
#if PROGRAM_SERIAL
   int fd_compass;
   struct termios termio;
#endif
   exif_init_compass_meta(&meta);
   if (argc < 2) {
      printf("Usage: %s compass_device [u]\n", argv[0]);
     exit (-1);
   }
   if (argc >2) {
     if (argv[2][0]=='u') upside_down=1;
   }

   fd_exif = open(EXIF_DEV, O_RDWR);
   if (fd_exif<=0) {
     fprintf(stderr,"Can not open device file "EXIF_DEV);
     exit (-1);
   }
#if PROGRAM_SERIAL
   fd_compass = open(argv[1], O_RDWR);
   if (fd_compass == -1) {
      perror("Cannot open compass device");
     close(fd_exif);
     exit (-1);
   }
   tcgetattr(fd_compass, &termio);
//int cfsetispeed(struct termios *termios_p, speed_t speed);
//int cfsetospeed(struct termios *termios_p, speed_t speed);
//B19200
   cfsetospeed(&termio, B19200);
   tcsetattr(fd_compass, TCIOFLUSH, &termio);
#endif

#if PROGRAM_SERIAL
   fcompass=fopen(argv[1], "r+"); // probably "r" is enough - we do not want to control it here
#else
   fcompass=fopen(argv[1], "r");
#endif
   if (!fcompass) {
      perror("Cannot open compass device!");
     exit (-1);
   }
   
   while (1) {
     fgets(response,sizeof(response),fcompass);
     if ((cp=strchr(response,'\n'))) cp[0]='\0';
     if ((cp=strchr(response,'\r'))) cp[0]='\0';
//     D(printf("response: >>%s<<\n",response));
     response_len=strlen(response);
     if (response_len>0) {
       if (response_len>4) {
         if ((cp_cs=strchr(response,'*'))) {
           cp_cs[0]='\0';
           cp_cs++;
           cs_read= strtol(cp_cs, NULL, 16);
           cs_calc=0;
           for (i=1; response[i]; i++) cs_calc^=response[i];
//   unsigned char cs_calc, cs_read;
           if (cs_calc==cs_read ) {
             D(printf ("Got NMEA string >>%s<<\n",response+1));
             cp_c=strchr(response,'C');  // course
             cp_r=strchr(response,'R');  // roll
             cp_p=strchr(response,'P');  // pitch
             cp_t=strchr(response,'T');  // temperature
             if (cp_c) {cp_c[0]='\0';cp_c++;}
             if (cp_r) {cp_r[0]='\0';cp_r++;}
             if (cp_p) {cp_p[0]='\0';cp_p++;}
             if (cp_t) {cp_t[0]='\0';cp_t++;}
//need to be after all letters where found and zeroed
             if (cp_c) dc=strtod(cp_c, NULL);
             if (cp_r) dr=strtod(cp_r, NULL);
             if (cp_p) dp=strtod(cp_p, NULL);
             if (cp_t) dt=strtod(cp_t, NULL);
             D(printf("course=%f, roll=%f, pitch=%f, temperature=%f\n",dc,dr,dp,dt));
//fill Exif fields (no temperature)
//          int icourse,ipitch,iroll;
             if (upside_down<0) {
               upside_down= ((dr>90) || (dr<-90))? 1:0;
             }
             if (upside_down) {
               dr+=180.0;
               if (dr>180) dr-=360;
             }
             icourse= (dc* EXIF_GPS_COMPASS_DENOM) +0.5;
             ipitch=  (dp* EXIF_GPS_COMPASS_DENOM) +0.5;
             iroll=   (dr* EXIF_GPS_COMPASS_DENOM) +0.5;
             meta.CompassDirection_nom=   __cpu_to_be32((int) icourse);
             if (ipitch<0) {
               meta.CompassPitchRef=  EXIF_COMPASS_PITCH_ASCII[1];
               ipitch= -ipitch;
             } else meta.CompassPitchRef=  EXIF_COMPASS_PITCH_ASCII[0];
             meta.CompassPitch_nom=       __cpu_to_be32((int) ipitch);
             if (iroll<0) {
               meta.CompassRollRef=  EXIF_COMPASS_ROLL_ASCII[1];
               iroll= -iroll;
             } else meta.CompassRollRef=  EXIF_COMPASS_ROLL_ASCII[0];
             meta.CompassRoll_nom=       __cpu_to_be32((int) iroll);

             lseek (fd_exif,Exif_GPSInfo_CompassDirection,SEEK_END); // position file pointer at the beginning of the data field for Compass
             write (fd_exif, &meta, sizeof(meta));
           } else D(printf("Checksum mismatch: read 0x%x, calculated 0x%x\n",(int)cs_read,(int)cs_calc));
         } else D(printf("No '*' in response, ignoring\n"));
      } else D(printf("response to short (%d), ignoring\n",response_len));
    }
   }
return 0;
}

---