Code for autonomous rover for Sparkfun AVC. DataBus won 3rd in 2012 and the same code was used on Troubled Child, a 1986 Jeep Grand Wagoneer to win 1st in 2014.
Dependencies: mbed Watchdog SDFileSystem DigoleSerialDisp
Diff: logging/logging.cpp
- Revision:
- 15:01fb4916a5cd
- Parent:
- 3:42f3821c4e54
- Child:
- 23:a34af501ea89
diff -r 86ee4d840f6b -r 01fb4916a5cd logging/logging.cpp --- a/logging/logging.cpp Thu Nov 29 16:59:39 2018 +0000 +++ b/logging/logging.cpp Thu Nov 29 17:11:34 2018 +0000 @@ -1,37 +1,16 @@ +#include "SystemState.h" +#include "globals.h" #include "logging.h" -#include "SDHCFileSystem.h" +#include "SDFileSystem.h" #include "SerialGraphicLCD.h" -extern Serial pc; -extern SerialGraphicLCD lcd; +// TODO 2 set up logging out of low priority interrupt handler -SDFileSystem sd(p5, p6, p7, p8, "log"); // mosi, miso, sclk, cs +//SDFileSystem sd(p5, p6, p7, p8, "log"); // mosi, miso, sclk, cs static FILE *logp; -void clearState( SystemState *s ) -{ - s->millis = 0; - s->current = s->voltage = 0.0; - s->g[0] = s->g[1] = s->g[2] = 0; - s->gyro[0] = s->gyro[1] = s->gyro[2] = 0; - s->gTemp = 0; - s->a[0] = s->a[1] = s->a[2] = 0; - s->m[0] = s->m[1] = s->m[2] = 0; - s->gHeading = s->cHeading = 0.0; - //s->roll = s->pitch = s->yaw =0.0; - s->gpsLatitude = s->gpsLongitude = s->gpsCourse_deg = s->gpsSpeed_mps = s->gpsHDOP = 0.0; - //s->gpsLatitude2 = s->gpsLongitude2 = s->gpsCourse_deg2 = s->gpsSpeed_mps2 = s->gpsHDOP2 = 0.0; - s->lrEncDistance = s->rrEncDistance = 0.0; - s->lrEncSpeed = s->rrEncSpeed = s->encHeading = 0.0; - s->estHeading = s->estLatitude = s->estLongitude = 0.0; - //s->estNorthing = s->estEasting = - s->estX = s->estY = 0.0; - s->nextWaypoint = 0; - s->bearing = s->distance = 0.0; -} - Timer logtimer; -extern int bufCount; +//extern int bufCount; /* void logData( const SystemState s ) { @@ -130,97 +109,106 @@ // If I use arduino style print routines, logging takes ~1000 / ~8000 usec // the big sprintf takes ~ 700-750 usec all by itself -void logData( const SystemState s ) +void logData( SystemState *s ) { //char buf[256]; - unsigned int t1, t2; - logtimer.start(); - logtimer.reset(); - t1 = logtimer.read_us(); - printInt(logp, s.millis); - fputc(',',logp); - printFloat(logp, s.current, 2); - fputc(',',logp); - printFloat(logp, s.voltage, 2); - fputc(',',logp); - for (int q=0; q < 3; q++) { - printFloat(logp, s.gyro[q], 6); + //unsigned int t1, t2; + //logtimer.start(); + //logtimer.reset(); + //t1 = logtimer.read_us(); + + if (s) { + printInt(logp, s->millis); + fputc(',',logp); + printFloat(logp, s->current, 2); + fputc(',',logp); + printFloat(logp, s->voltage, 2); fputc(',',logp); - } - printInt(logp, s.gTemp); - fputc(',',logp); - for (int q=0; q < 3; q++) { - printInt(logp, s.a[q]); + for (int q=0; q < 3; q++) { + printFloat(logp, s->gyro[q], 6); + fputc(',',logp); + } + printInt(logp, s->gTemp); fputc(',',logp); - } - /* - for (int q=0; q < 3; q++) { - printInt(logp, s.m[q]); + for (int q=0; q < 3; q++) { + printInt(logp, s->a[q]); + fputc(',',logp); + } + /* + for (int q=0; q < 3; q++) { + printInt(logp, s->m[q]); + fputc(',',logp); + } + */ + printFloat(logp, s->gHeading, 2); fputc(',',logp); - } - */ - printFloat(logp, s.gHeading, 2); - fputc(',',logp); - // GPS 1 - fprintf(logp, "%.7f,%.7f,", s.gpsLatitude, s.gpsLongitude); - //printFloat(logp, s.gpsLatitude, 7); - //fputc(',',logp); - //printFloat(logp, s.gpsLongitude, 7); - //fputc(',',logp); - printFloat(logp, s.gpsCourse_deg, 2); - fputc(',',logp); - printFloat(logp, s.gpsSpeed_mps, 2); - fputc(',',logp); - printFloat(logp, s.gpsHDOP, 1); - fputc(',',logp); - printInt(logp, s.gpsSats); - fputc(',',logp); - // Encoders - printFloat(logp, s.lrEncDistance, 7); - fputc(',',logp); - printFloat(logp, s.rrEncDistance, 7); - fputc(',',logp); - printFloat(logp, s.lrEncSpeed, 2); - fputc(',',logp); - printFloat(logp, s.rrEncSpeed, 2); - fputc(',',logp); - printFloat(logp, s.encHeading, 2); - fputc(',',logp); - // Estimates - printFloat(logp, s.estHeading, 2); - fputc(',',logp); - printFloat(logp, s.estLagHeading, 2); - fputc(',',logp); - printFloat(logp, s.estLatitude, 7); - fputc(',',logp); - printFloat(logp, s.estLongitude, 7); - fputc(',',logp); - printFloat(logp, s.estX, 4); - fputc(',',logp); - printFloat(logp, s.estY, 4); - fputc(',',logp); - // Nav - printInt(logp, s.nextWaypoint); - fputc(',',logp); - printFloat(logp, s.bearing, 2); - fputc(',',logp); - printFloat(logp, s.distance, 3); - fputc(',',logp); - printFloat(logp, s.steerAngle, 3); - fputc(',',logp); - printFloat(logp, s.errHeading, 3); - fputc(',',logp); - fputc('\n',logp); + // GPS 1 + fprintf(logp, "%.7f,%.7f,", s->gpsLatitude, s->gpsLongitude); + //printFloat(logp, s->gpsLatitude, 7); + //fputc(',',logp); + //printFloat(logp, s->gpsLongitude, 7); + //fputc(',',logp); + printFloat(logp, s->gpsCourse_deg, 2); + fputc(',',logp); + printFloat(logp, s->gpsSpeed_mps, 2); + fputc(',',logp); + printFloat(logp, s->gpsHDOP, 1); + fputc(',',logp); + printInt(logp, s->gpsSats); + fputc(',',logp); + // Encoders + printFloat(logp, s->lrEncDistance, 7); + fputc(',',logp); + printFloat(logp, s->rrEncDistance, 7); + fputc(',',logp); + printFloat(logp, s->lrEncSpeed, 2); + fputc(',',logp); + printFloat(logp, s->rrEncSpeed, 2); + fputc(',',logp); + printFloat(logp, s->encHeading, 2); + fputc(',',logp); + // Estimates + printFloat(logp, s->estHeading, 2); + fputc(',',logp); + printFloat(logp, s->estLagHeading, 2); + fputc(',',logp); + printFloat(logp, s->estLatitude, 7); + fputc(',',logp); + printFloat(logp, s->estLongitude, 7); + fputc(',',logp); + printFloat(logp, s->estX, 4); + fputc(',',logp); + printFloat(logp, s->estY, 4); + fputc(',',logp); + // Nav + printInt(logp, s->nextWaypoint); + fputc(',',logp); + printFloat(logp, s->bearing, 2); + fputc(',',logp); + printFloat(logp, s->distance, 3); + fputc(',',logp); + printFloat(logp, s->steerAngle, 3); + fputc(',',logp); + printFloat(logp, s->errHeading, 3); + fputc(',',logp); + printFloat(logp, s->LABrg, 2); + fputc(',',logp); + printFloat(logp, s->LAx, 4); + fputc(',',logp); + printFloat(logp, s->LAy, 4); + fputc('\n',logp); + fflush(logp); - t2 = logtimer.read_us(); - //fprintf(stdout, "%d\n", t2-t1); + //t2 = logtimer.read_us(); + //fprintf(stdout, "%d\n", t2-t1); + } return; } -FILE *openlog(char *prefix) +FILE *openlog(const char *prefix) { FILE *fp = 0; char myname[64]; @@ -228,7 +216,8 @@ pc.printf("Opening file...\n"); while (fp == 0) { - if ((fp = fopen("/log/test.txt", "w")) == 0) { + sprintf(myname, "%s/test.txt", LOGDIR); + if ((fp = fopen(myname, "w")) == 0) { pc.printf("Waiting for filesystem to come online..."); wait(0.200); lcd.pos(0,1); @@ -238,7 +227,7 @@ fclose(fp); for (int i = 0; i < 1000; i++) { - sprintf(myname, "/log/%s%03d.csv", prefix, i); + sprintf(myname, "%s/%s%03d.csv", LOGDIR, prefix, i); if ((fp = fopen(myname, "r")) == 0) { break; } else { @@ -250,7 +239,7 @@ pc.printf("file write failed: %s\n", myname); } else { - // TODO -- set error message, get rid of writing to terminal + // TODO 3 set error message, get rid of writing to terminal //status = true; pc.printf("opened %s for writing\n", myname); @@ -281,4 +270,4 @@ void closeLogfile(void) { if (logp) fclose(logp); -} \ No newline at end of file +}