Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.
Dependencies: Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo
logging/logging.c@0:826c6171fc1b, 2012-06-20 (annotated)
- Committer:
- shimniok
- Date:
- Wed Jun 20 14:57:48 2012 +0000
- Revision:
- 0:826c6171fc1b
Updated documentation
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shimniok | 0:826c6171fc1b | 1 | #include "logging.h" |
shimniok | 0:826c6171fc1b | 2 | #include "SDHCFileSystem.h" |
shimniok | 0:826c6171fc1b | 3 | #include "SerialGraphicLCD.h" |
shimniok | 0:826c6171fc1b | 4 | |
shimniok | 0:826c6171fc1b | 5 | extern Serial pc; |
shimniok | 0:826c6171fc1b | 6 | extern SerialGraphicLCD lcd; |
shimniok | 0:826c6171fc1b | 7 | |
shimniok | 0:826c6171fc1b | 8 | SDFileSystem sd(p5, p6, p7, p8, "log"); // mosi, miso, sclk, cs |
shimniok | 0:826c6171fc1b | 9 | static FILE *logp; |
shimniok | 0:826c6171fc1b | 10 | |
shimniok | 0:826c6171fc1b | 11 | |
shimniok | 0:826c6171fc1b | 12 | void clearState( SystemState *s ) { |
shimniok | 0:826c6171fc1b | 13 | s->millis = 0; |
shimniok | 0:826c6171fc1b | 14 | s->current = s->voltage = 0.0; |
shimniok | 0:826c6171fc1b | 15 | s->g[0] = s->g[1] = s->g[2] = 0; |
shimniok | 0:826c6171fc1b | 16 | s->gTemp = 0; |
shimniok | 0:826c6171fc1b | 17 | s->a[0] = s->a[1] = s->a[2] = 0; |
shimniok | 0:826c6171fc1b | 18 | s->m[0] = s->m[1] = s->m[2] = 0; |
shimniok | 0:826c6171fc1b | 19 | s->gHeading = s->cHeading = 0.0; |
shimniok | 0:826c6171fc1b | 20 | //s->roll = s->pitch = s->yaw =0.0; |
shimniok | 0:826c6171fc1b | 21 | s->gpsLatitude = s->gpsLongitude = s->gpsCourse = s->gpsHDOP = 0.0; |
shimniok | 0:826c6171fc1b | 22 | s->lrEncDistance = s->rrEncDistance = 0.0; |
shimniok | 0:826c6171fc1b | 23 | s->lrEncSpeed = s->rrEncSpeed = s->encHeading = 0.0; |
shimniok | 0:826c6171fc1b | 24 | s->estHeading = s->estLatitude = s->estLongitude = 0.0; |
shimniok | 0:826c6171fc1b | 25 | //s->estNorthing = s->estEasting = |
shimniok | 0:826c6171fc1b | 26 | s->estX = s->estY = 0.0; |
shimniok | 0:826c6171fc1b | 27 | s->nextWaypoint = 0; |
shimniok | 0:826c6171fc1b | 28 | s->bearing = s->distance = 0.0; |
shimniok | 0:826c6171fc1b | 29 | } |
shimniok | 0:826c6171fc1b | 30 | |
shimniok | 0:826c6171fc1b | 31 | //void logData( SystemState s, void (*logString)(char *s) ) { |
shimniok | 0:826c6171fc1b | 32 | void logData( SystemState s ) { |
shimniok | 0:826c6171fc1b | 33 | char buf[256]; |
shimniok | 0:826c6171fc1b | 34 | |
shimniok | 0:826c6171fc1b | 35 | 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", |
shimniok | 0:826c6171fc1b | 36 | s.millis, |
shimniok | 0:826c6171fc1b | 37 | s.current, s.voltage, |
shimniok | 0:826c6171fc1b | 38 | s.g[0], s.g[1], s.g[2], |
shimniok | 0:826c6171fc1b | 39 | s.gTemp, |
shimniok | 0:826c6171fc1b | 40 | s.a[0], s.a[1], s.a[2], |
shimniok | 0:826c6171fc1b | 41 | s.m[0], s.m[1], s.m[2], |
shimniok | 0:826c6171fc1b | 42 | s.gHeading, //s.cHeading, |
shimniok | 0:826c6171fc1b | 43 | //s.roll, s.pitch, s.yaw, |
shimniok | 0:826c6171fc1b | 44 | s.gpsLatitude, s.gpsLongitude, s.gpsCourse, s.gpsSpeed*0.44704, s.gpsHDOP, s.gpsSats, // convert gps speed to m/s |
shimniok | 0:826c6171fc1b | 45 | s.lrEncDistance, s.rrEncDistance, s.lrEncSpeed, s.rrEncSpeed, s.encHeading, |
shimniok | 0:826c6171fc1b | 46 | s.estHeading, s.estLatitude, s.estLongitude, |
shimniok | 0:826c6171fc1b | 47 | // s.estNorthing, s.estEasting, |
shimniok | 0:826c6171fc1b | 48 | s.estX, s.estY, |
shimniok | 0:826c6171fc1b | 49 | s.nextWaypoint, s.bearing, s.distance, s.gbias, s.errAngle, |
shimniok | 0:826c6171fc1b | 50 | s.leftRanger, s.rightRanger, s.centerRanger, |
shimniok | 0:826c6171fc1b | 51 | s.crossTrackErr |
shimniok | 0:826c6171fc1b | 52 | ); |
shimniok | 0:826c6171fc1b | 53 | |
shimniok | 0:826c6171fc1b | 54 | if (logp) |
shimniok | 0:826c6171fc1b | 55 | fprintf(logp, buf); |
shimniok | 0:826c6171fc1b | 56 | |
shimniok | 0:826c6171fc1b | 57 | return; |
shimniok | 0:826c6171fc1b | 58 | } |
shimniok | 0:826c6171fc1b | 59 | |
shimniok | 0:826c6171fc1b | 60 | |
shimniok | 0:826c6171fc1b | 61 | FILE *openlog(char *prefix) |
shimniok | 0:826c6171fc1b | 62 | { |
shimniok | 0:826c6171fc1b | 63 | FILE *fp = 0; |
shimniok | 0:826c6171fc1b | 64 | char myname[64]; |
shimniok | 0:826c6171fc1b | 65 | |
shimniok | 0:826c6171fc1b | 66 | pc.printf("Opening file...\n"); |
shimniok | 0:826c6171fc1b | 67 | |
shimniok | 0:826c6171fc1b | 68 | while (fp == 0) { |
shimniok | 0:826c6171fc1b | 69 | if ((fp = fopen("/log/test.txt", "w")) == 0) { |
shimniok | 0:826c6171fc1b | 70 | pc.printf("Waiting for filesystem to come online..."); |
shimniok | 0:826c6171fc1b | 71 | wait(0.200); |
shimniok | 0:826c6171fc1b | 72 | lcd.pos(0,1); |
shimniok | 0:826c6171fc1b | 73 | lcd.printf("%-16s", "Waiting for fs"); |
shimniok | 0:826c6171fc1b | 74 | } |
shimniok | 0:826c6171fc1b | 75 | } |
shimniok | 0:826c6171fc1b | 76 | fclose(fp); |
shimniok | 0:826c6171fc1b | 77 | |
shimniok | 0:826c6171fc1b | 78 | for (int i = 0; i < 1000; i++) { |
shimniok | 0:826c6171fc1b | 79 | sprintf(myname, "/log/%s%03d.csv", prefix, i); |
shimniok | 0:826c6171fc1b | 80 | if ((fp = fopen(myname, "r")) == 0) { |
shimniok | 0:826c6171fc1b | 81 | break; |
shimniok | 0:826c6171fc1b | 82 | } else { |
shimniok | 0:826c6171fc1b | 83 | fclose(fp); |
shimniok | 0:826c6171fc1b | 84 | } |
shimniok | 0:826c6171fc1b | 85 | } |
shimniok | 0:826c6171fc1b | 86 | fp = fopen(myname, "w"); |
shimniok | 0:826c6171fc1b | 87 | if (fp == 0) { |
shimniok | 0:826c6171fc1b | 88 | pc.printf("file write failed: %s\n", myname); |
shimniok | 0:826c6171fc1b | 89 | } else { |
shimniok | 0:826c6171fc1b | 90 | |
shimniok | 0:826c6171fc1b | 91 | // TODO -- set error message, get rid of writing to terminal |
shimniok | 0:826c6171fc1b | 92 | |
shimniok | 0:826c6171fc1b | 93 | //status = true; |
shimniok | 0:826c6171fc1b | 94 | pc.printf("opened %s for writing\n", myname); |
shimniok | 0:826c6171fc1b | 95 | lcd.pos(0,1); |
shimniok | 0:826c6171fc1b | 96 | lcd.printf("%-16s", myname); |
shimniok | 0:826c6171fc1b | 97 | } |
shimniok | 0:826c6171fc1b | 98 | |
shimniok | 0:826c6171fc1b | 99 | return fp; |
shimniok | 0:826c6171fc1b | 100 | } |
shimniok | 0:826c6171fc1b | 101 | |
shimniok | 0:826c6171fc1b | 102 | |
shimniok | 0:826c6171fc1b | 103 | // Find the next unused filename of the form logger##.csv where # is 0-9 |
shimniok | 0:826c6171fc1b | 104 | // |
shimniok | 0:826c6171fc1b | 105 | bool initLogfile() |
shimniok | 0:826c6171fc1b | 106 | { |
shimniok | 0:826c6171fc1b | 107 | bool status = false; |
shimniok | 0:826c6171fc1b | 108 | |
shimniok | 0:826c6171fc1b | 109 | logp = openlog("log"); |
shimniok | 0:826c6171fc1b | 110 | |
shimniok | 0:826c6171fc1b | 111 | if (logp != 0) { |
shimniok | 0:826c6171fc1b | 112 | status = true; |
shimniok | 0:826c6171fc1b | 113 | 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"); |
shimniok | 0:826c6171fc1b | 114 | } |
shimniok | 0:826c6171fc1b | 115 | |
shimniok | 0:826c6171fc1b | 116 | return status; |
shimniok | 0:826c6171fc1b | 117 | } |
shimniok | 0:826c6171fc1b | 118 | |
shimniok | 0:826c6171fc1b | 119 | void closeLogfile(void) |
shimniok | 0:826c6171fc1b | 120 | { |
shimniok | 0:826c6171fc1b | 121 | if (logp) fclose(logp); |
shimniok | 0:826c6171fc1b | 122 | } |