logging.c

00001 #include "logging.h"
00002 #include "SDHCFileSystem.h"
00003 #include "SerialGraphicLCD.h"
00004 
00005 extern Serial pc;
00006 extern SerialGraphicLCD lcd;
00007 
00008 SDFileSystem sd(p5, p6, p7, p8, "log"); // mosi, miso, sclk, cs
00009 static FILE *logp;
00010 
00011 
00012 void clearState( SystemState *s ) {
00013     s->millis = 0;
00014     s->current = s->voltage = 0.0;
00015     s->g[0] = s->g[1] = s->g[2] = 0;
00016     s->gTemp = 0;
00017     s->a[0] = s->a[1] = s->a[2] = 0;
00018     s->m[0] = s->m[1] = s->m[2] = 0;
00019     s->gHeading = s->cHeading = 0.0;
00020     //s->roll = s->pitch = s->yaw =0.0;
00021     s->gpsLatitude = s->gpsLongitude = s->gpsCourse = s->gpsHDOP = 0.0;
00022     s->lrEncDistance = s->rrEncDistance = 0.0;
00023     s->lrEncSpeed = s->rrEncSpeed = s->encHeading = 0.0;
00024     s->estHeading = s->estLatitude = s->estLongitude = 0.0;
00025     //s->estNorthing = s->estEasting = 
00026     s->estX = s->estY = 0.0;
00027     s->nextWaypoint = 0;
00028     s->bearing = s->distance = 0.0;
00029 }
00030 
00031 //void logData( SystemState s, void (*logString)(char *s) ) {
00032 void logData( SystemState s ) {
00033     char buf[256];
00034 
00035     sprintf(buf, "%d,%.1f,%.1f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%.2f,,,,,%.6f,%.6f,%.1f,%.1f,%.1f,%d,%.7f,%.7f,%.2f,%.2f,%.1f,%.1f,%.6f,%.6f,,,%.4f,%.4f,%d,%.1f,%.3f,%.5f,%.5f,%.3f,%.3f,%.3f,%.3f\n",
00036         s.millis,
00037         s.current, s.voltage,
00038         s.g[0], s.g[1], s.g[2],
00039         s.gTemp,
00040         s.a[0], s.a[1], s.a[2],
00041         s.m[0], s.m[1], s.m[2],
00042         s.gHeading, //s.cHeading,
00043         //s.roll, s.pitch, s.yaw,
00044         s.gpsLatitude, s.gpsLongitude, s.gpsCourse, s.gpsSpeed*0.44704, s.gpsHDOP, s.gpsSats, // convert gps speed to m/s
00045         s.lrEncDistance, s.rrEncDistance, s.lrEncSpeed, s.rrEncSpeed, s.encHeading,
00046         s.estHeading, s.estLatitude, s.estLongitude,
00047         // s.estNorthing, s.estEasting, 
00048         s.estX, s.estY,
00049         s.nextWaypoint, s.bearing, s.distance, s.gbias, s.errAngle,
00050         s.leftRanger, s.rightRanger, s.centerRanger,
00051         s.crossTrackErr
00052     );
00053 
00054     if (logp)
00055         fprintf(logp, buf);
00056 
00057     return;    
00058 }
00059 
00060 
00061 FILE *openlog(char *prefix)
00062 {
00063     FILE *fp = 0;
00064     char myname[64];
00065     
00066     pc.printf("Opening file...\n");
00067 
00068     while (fp == 0) {
00069         if ((fp = fopen("/log/test.txt", "w")) == 0) {
00070             pc.printf("Waiting for filesystem to come online...");
00071             wait(0.200);
00072             lcd.pos(0,1);
00073             lcd.printf("%-16s", "Waiting for fs");
00074         }
00075     }    
00076     fclose(fp);
00077 
00078     for (int i = 0; i < 1000; i++) {
00079         sprintf(myname, "/log/%s%03d.csv", prefix, i);
00080         if ((fp = fopen(myname, "r")) == 0) {
00081             break;
00082         } else {
00083             fclose(fp);
00084         }
00085     }
00086     fp = fopen(myname, "w");
00087     if (fp == 0) {
00088         pc.printf("file write failed: %s\n", myname);
00089     } else {
00090     
00091         // TODO -- set error message, get rid of writing to terminal
00092         
00093         //status = true;
00094         pc.printf("opened %s for writing\n", myname);
00095         lcd.pos(0,1);
00096         lcd.printf("%-16s", myname);
00097     }
00098     
00099     return fp;
00100 }
00101 
00102 
00103 // Find the next unused filename of the form logger##.csv where # is 0-9
00104 //
00105 bool initLogfile() 
00106 {    
00107     bool status = false;
00108     
00109     logp = openlog("log");
00110     
00111     if (logp != 0) {
00112         status = true;
00113         fprintf(logp, "s.millis, s.current, s.voltage, s.gx, s.gy, s.gz, s.gTemp, s.ax, s.ay, s.az, s.mx, s.my, s.mz, s.gHeading, s.cHeading, s.roll, s.pitch, s.yaw, s.gpsLatitude, s.gpsLongitude, s.gpsCourse, s.gpsSpeed, s.gpsHDOP, s.lrEncDistance, s.rrEncDistance, s.lrEncSpeed, s.rrEncSpeed, s.encHeading, s.estHeading, s.estLatitude, s.estLongitude, s.estNorthing, s.estEasting, s.estX, s.estY, s.nextWaypoint, s.bearing, s.distance, s.gbias, s.errAngle, s.leftRanger, s.rightRanger, s.centerRanger, s.crossTrackErr\n");
00114     }
00115     
00116     return status;
00117 }
00118 
00119 void closeLogfile(void)
00120 {
00121     if (logp) fclose(logp);
00122 }