Michael Shimniok / Mbed 2 deprecated DataBus

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

logging/logging.cpp@1:cb84b477886c, 2013-05-28 (annotated)

Committer:
shimniok
Date:
Tue May 28 13:58:35 2013 +0000
Revision:
1:cb84b477886c
Parent:
0:a6a169de725f
Child:
2:fbc6e3cf3ed8
Changed initial next/prev waypoint to permit steering calc. Removed unnecessary code, added logging for steering angle to diagnose bug.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 0:a6a169de725f 1 #include "logging.h"
shimniok 0:a6a169de725f 2 #include "SDHCFileSystem.h"
shimniok 0:a6a169de725f 3 #include "SerialGraphicLCD.h"
shimniok 0:a6a169de725f 4 #include "b64.h"
shimniok 0:a6a169de725f 5
shimniok 0:a6a169de725f 6 extern Serial pc;
shimniok 0:a6a169de725f 7 extern SerialGraphicLCD lcd;
shimniok 0:a6a169de725f 8
shimniok 0:a6a169de725f 9 SDFileSystem sd(p5, p6, p7, p8, "log"); // mosi, miso, sclk, cs
shimniok 0:a6a169de725f 10 static FILE *logp;
shimniok 0:a6a169de725f 11
shimniok 0:a6a169de725f 12 void clearState( SystemState *s )
shimniok 0:a6a169de725f 13 {
shimniok 0:a6a169de725f 14 s->millis = 0;
shimniok 0:a6a169de725f 15 s->current = s->voltage = 0.0;
shimniok 0:a6a169de725f 16 s->g[0] = s->g[1] = s->g[2] = 0;
shimniok 0:a6a169de725f 17 s->gTemp = 0;
shimniok 0:a6a169de725f 18 s->a[0] = s->a[1] = s->a[2] = 0;
shimniok 0:a6a169de725f 19 s->m[0] = s->m[1] = s->m[2] = 0;
shimniok 0:a6a169de725f 20 s->gHeading = s->cHeading = 0.0;
shimniok 0:a6a169de725f 21 //s->roll = s->pitch = s->yaw =0.0;
shimniok 0:a6a169de725f 22 s->gpsLatitude = s->gpsLongitude = s->gpsCourse_deg = s->gpsSpeed_mps = s->gpsHDOP = 0.0;
shimniok 1:cb84b477886c 23 //s->gpsLatitude2 = s->gpsLongitude2 = s->gpsCourse_deg2 = s->gpsSpeed_mps2 = s->gpsHDOP2 = 0.0;
shimniok 0:a6a169de725f 24 s->lrEncDistance = s->rrEncDistance = 0.0;
shimniok 0:a6a169de725f 25 s->lrEncSpeed = s->rrEncSpeed = s->encHeading = 0.0;
shimniok 0:a6a169de725f 26 s->estHeading = s->estLatitude = s->estLongitude = 0.0;
shimniok 0:a6a169de725f 27 //s->estNorthing = s->estEasting =
shimniok 0:a6a169de725f 28 s->estX = s->estY = 0.0;
shimniok 0:a6a169de725f 29 s->nextWaypoint = 0;
shimniok 0:a6a169de725f 30 s->bearing = s->distance = 0.0;
shimniok 0:a6a169de725f 31 }
shimniok 0:a6a169de725f 32
shimniok 0:a6a169de725f 33 Timer logtimer;
shimniok 0:a6a169de725f 34 extern int bufCount;
shimniok 0:a6a169de725f 35
shimniok 0:a6a169de725f 36 /*
shimniok 0:a6a169de725f 37 void logData( const SystemState s ) {
shimniok 0:a6a169de725f 38 unsigned char buf[512]; // for now we really only need ~256 bytes but in case I add more to state...
shimniok 0:a6a169de725f 39 unsigned char *state = (unsigned char *) &s;
shimniok 0:a6a169de725f 40 //unsigned int t1, t2, t3;
shimniok 0:a6a169de725f 41 //logtimer.start();
shimniok 0:a6a169de725f 42 //logtimer.reset();
shimniok 0:a6a169de725f 43 if (logp) {
shimniok 0:a6a169de725f 44 //t1 = logtimer.read_us();
shimniok 0:a6a169de725f 45 encode(state, sizeof(s), buf, 0); // infinite line size
shimniok 0:a6a169de725f 46 //t2 = logtimer.read_us();
shimniok 0:a6a169de725f 47 fputs((char *) buf, logp);
shimniok 0:a6a169de725f 48 fputs("\n", logp);
shimniok 0:a6a169de725f 49 bufCount--;
shimniok 0:a6a169de725f 50 fprintf(stdout, "bufCount: %d\n", bufCount);
shimniok 0:a6a169de725f 51 //t3 = logtimer.read_us();
shimniok 0:a6a169de725f 52 //fprintf(stdout, "%d %d\n", t3-t2, t2-t1);
shimniok 0:a6a169de725f 53 }
shimniok 0:a6a169de725f 54 }
shimniok 0:a6a169de725f 55 */
shimniok 0:a6a169de725f 56
shimniok 0:a6a169de725f 57 // from Arduino source
shimniok 0:a6a169de725f 58 size_t printNumber(FILE *f, unsigned long n)
shimniok 0:a6a169de725f 59 {
shimniok 0:a6a169de725f 60 char buf[8 * sizeof(long) + 1]; // Assumes 8-bit chars plus zero byte.
shimniok 0:a6a169de725f 61 char *str = &buf[sizeof(buf) - 1];
shimniok 0:a6a169de725f 62
shimniok 0:a6a169de725f 63 *str = '\0';
shimniok 0:a6a169de725f 64
shimniok 0:a6a169de725f 65 do {
shimniok 0:a6a169de725f 66 unsigned long m = n;
shimniok 0:a6a169de725f 67 n /= 10;
shimniok 0:a6a169de725f 68 char c = m - 10 * n;
shimniok 0:a6a169de725f 69 *--str = c + '0';
shimniok 0:a6a169de725f 70 } while(n);
shimniok 0:a6a169de725f 71
shimniok 0:a6a169de725f 72 return fputs(str, f);
shimniok 0:a6a169de725f 73 }
shimniok 0:a6a169de725f 74
shimniok 0:a6a169de725f 75 // from Arduino source
shimniok 0:a6a169de725f 76 size_t printInt(FILE *f, long n)
shimniok 0:a6a169de725f 77 {
shimniok 0:a6a169de725f 78 int t = 0;
shimniok 0:a6a169de725f 79 if (n < 0) {
shimniok 0:a6a169de725f 80 t = fputc('-', f);
shimniok 0:a6a169de725f 81 n = -n;
shimniok 0:a6a169de725f 82 }
shimniok 0:a6a169de725f 83 return printNumber(f, n) + t;
shimniok 0:a6a169de725f 84 }
shimniok 0:a6a169de725f 85
shimniok 0:a6a169de725f 86 // from Arduino source
shimniok 0:a6a169de725f 87 size_t printFloat(FILE *f, double number, uint8_t digits)
shimniok 0:a6a169de725f 88 {
shimniok 0:a6a169de725f 89 size_t n=0;
shimniok 0:a6a169de725f 90
shimniok 0:a6a169de725f 91 if (isnan(number)) return fputs("nan", f);
shimniok 0:a6a169de725f 92 if (isinf(number)) return fputs("inf", f);
shimniok 0:a6a169de725f 93 if (number > 4294967040.0) return fputs("ovf", f); // constant determined empirically
shimniok 0:a6a169de725f 94 if (number <-4294967040.0) return fputs("ovf", f); // constant determined empirically
shimniok 0:a6a169de725f 95
shimniok 0:a6a169de725f 96 // Handle negative numbers
shimniok 0:a6a169de725f 97 if (number < 0.0) {
shimniok 0:a6a169de725f 98 n += fputc('-', f);
shimniok 0:a6a169de725f 99 number = -number;
shimniok 0:a6a169de725f 100 }
shimniok 0:a6a169de725f 101
shimniok 0:a6a169de725f 102 // Round correctly so that print(1.999, 2) prints as "2.00"
shimniok 0:a6a169de725f 103 double rounding = 0.5;
shimniok 0:a6a169de725f 104 for (uint8_t i=0; i < digits; ++i)
shimniok 0:a6a169de725f 105 rounding /= 10.0;
shimniok 0:a6a169de725f 106
shimniok 0:a6a169de725f 107 number += rounding;
shimniok 0:a6a169de725f 108
shimniok 0:a6a169de725f 109 // Extract the integer part of the number and print it
shimniok 0:a6a169de725f 110 unsigned long int_part = (unsigned long)number;
shimniok 0:a6a169de725f 111 double remainder = number - (double)int_part;
shimniok 0:a6a169de725f 112 n += printInt(f, int_part);
shimniok 0:a6a169de725f 113
shimniok 0:a6a169de725f 114 // Print the decimal point, but only if there are digits beyond
shimniok 0:a6a169de725f 115 if (digits > 0) {
shimniok 0:a6a169de725f 116 n += fputc('.', f);
shimniok 0:a6a169de725f 117 }
shimniok 0:a6a169de725f 118
shimniok 0:a6a169de725f 119 // Extract digits from the remainder one at a time
shimniok 0:a6a169de725f 120 while (digits-- > 0) {
shimniok 0:a6a169de725f 121 remainder *= 10.0;
shimniok 0:a6a169de725f 122 int toPrint = int(remainder);
shimniok 0:a6a169de725f 123 n += fputc(toPrint+'0', f);
shimniok 0:a6a169de725f 124 remainder -= toPrint;
shimniok 0:a6a169de725f 125 }
shimniok 0:a6a169de725f 126
shimniok 0:a6a169de725f 127 return n;
shimniok 0:a6a169de725f 128 }
shimniok 0:a6a169de725f 129
shimniok 0:a6a169de725f 130
shimniok 0:a6a169de725f 131 // If I use arduino style print routines, logging takes ~1000 / ~8000 usec
shimniok 0:a6a169de725f 132 // the big sprintf takes ~ 700-750 usec all by itself
shimniok 0:a6a169de725f 133 void logData( const SystemState s )
shimniok 0:a6a169de725f 134 {
shimniok 0:a6a169de725f 135 //char buf[256];
shimniok 0:a6a169de725f 136 unsigned int t1, t2;
shimniok 0:a6a169de725f 137 logtimer.start();
shimniok 0:a6a169de725f 138 logtimer.reset();
shimniok 0:a6a169de725f 139 t1 = logtimer.read_us();
shimniok 0:a6a169de725f 140 printInt(logp, s.millis);
shimniok 0:a6a169de725f 141 fputc(',',logp);
shimniok 0:a6a169de725f 142 printFloat(logp, s.current, 2);
shimniok 0:a6a169de725f 143 fputc(',',logp);
shimniok 0:a6a169de725f 144 printFloat(logp, s.voltage, 2);
shimniok 0:a6a169de725f 145 fputc(',',logp);
shimniok 0:a6a169de725f 146 for (int q=0; q < 3; q++) {
shimniok 0:a6a169de725f 147 printInt(logp, s.g[q]);
shimniok 0:a6a169de725f 148 fputc(',',logp);
shimniok 0:a6a169de725f 149 }
shimniok 0:a6a169de725f 150 printInt(logp, s.gTemp);
shimniok 0:a6a169de725f 151 fputc(',',logp);
shimniok 0:a6a169de725f 152 for (int q=0; q < 3; q++) {
shimniok 0:a6a169de725f 153 printInt(logp, s.a[q]);
shimniok 0:a6a169de725f 154 fputc(',',logp);
shimniok 0:a6a169de725f 155 }
shimniok 0:a6a169de725f 156 /*
shimniok 0:a6a169de725f 157 for (int q=0; q < 3; q++) {
shimniok 0:a6a169de725f 158 printInt(logp, s.m[q]);
shimniok 0:a6a169de725f 159 fputc(',',logp);
shimniok 0:a6a169de725f 160 }
shimniok 0:a6a169de725f 161 */
shimniok 0:a6a169de725f 162 printFloat(logp, s.gHeading, 2);
shimniok 0:a6a169de725f 163 fputc(',',logp);
shimniok 0:a6a169de725f 164
shimniok 0:a6a169de725f 165 // GPS 1
shimniok 0:a6a169de725f 166 fprintf(logp, "%.7f,%.7f,", s.gpsLatitude, s.gpsLongitude);
shimniok 0:a6a169de725f 167 //printFloat(logp, s.gpsLatitude, 7);
shimniok 0:a6a169de725f 168 //fputc(',',logp);
shimniok 0:a6a169de725f 169 //printFloat(logp, s.gpsLongitude, 7);
shimniok 0:a6a169de725f 170 //fputc(',',logp);
shimniok 0:a6a169de725f 171 printFloat(logp, s.gpsCourse_deg, 2);
shimniok 0:a6a169de725f 172 fputc(',',logp);
shimniok 0:a6a169de725f 173 printFloat(logp, s.gpsSpeed_mps, 2);
shimniok 0:a6a169de725f 174 fputc(',',logp);
shimniok 0:a6a169de725f 175 printFloat(logp, s.gpsHDOP, 1);
shimniok 0:a6a169de725f 176 fputc(',',logp);
shimniok 0:a6a169de725f 177 printInt(logp, s.gpsSats);
shimniok 0:a6a169de725f 178 fputc(',',logp);
shimniok 1:cb84b477886c 179 // Encoders
shimniok 0:a6a169de725f 180 printFloat(logp, s.lrEncDistance, 7);
shimniok 0:a6a169de725f 181 fputc(',',logp);
shimniok 0:a6a169de725f 182 printFloat(logp, s.rrEncDistance, 7);
shimniok 0:a6a169de725f 183 fputc(',',logp);
shimniok 0:a6a169de725f 184 printFloat(logp, s.lrEncSpeed, 2);
shimniok 0:a6a169de725f 185 fputc(',',logp);
shimniok 0:a6a169de725f 186 printFloat(logp, s.rrEncSpeed, 2);
shimniok 0:a6a169de725f 187 fputc(',',logp);
shimniok 0:a6a169de725f 188 printFloat(logp, s.encHeading, 2);
shimniok 0:a6a169de725f 189 fputc(',',logp);
shimniok 1:cb84b477886c 190 // Estimates
shimniok 0:a6a169de725f 191 printFloat(logp, s.estHeading, 2);
shimniok 0:a6a169de725f 192 fputc(',',logp);
shimniok 1:cb84b477886c 193 printFloat(logp, s.estLagHeading, 2);
shimniok 1:cb84b477886c 194 fputc(',',logp);
shimniok 0:a6a169de725f 195 printFloat(logp, s.estLatitude, 7);
shimniok 0:a6a169de725f 196 fputc(',',logp);
shimniok 0:a6a169de725f 197 printFloat(logp, s.estLongitude, 7);
shimniok 0:a6a169de725f 198 fputc(',',logp);
shimniok 0:a6a169de725f 199 printFloat(logp, s.estX, 4);
shimniok 0:a6a169de725f 200 fputc(',',logp);
shimniok 0:a6a169de725f 201 printFloat(logp, s.estY, 4);
shimniok 0:a6a169de725f 202 fputc(',',logp);
shimniok 1:cb84b477886c 203 // Nav
shimniok 0:a6a169de725f 204 printInt(logp, s.nextWaypoint);
shimniok 0:a6a169de725f 205 fputc(',',logp);
shimniok 0:a6a169de725f 206 printFloat(logp, s.bearing, 2);
shimniok 0:a6a169de725f 207 fputc(',',logp);
shimniok 0:a6a169de725f 208 printFloat(logp, s.distance, 3);
shimniok 0:a6a169de725f 209 fputc(',',logp);
shimniok 1:cb84b477886c 210 printFloat(logp, s.steerAngle, 3);
shimniok 0:a6a169de725f 211 fputc(',',logp);
shimniok 0:a6a169de725f 212 fputc('\n',logp);
shimniok 0:a6a169de725f 213
shimniok 0:a6a169de725f 214 t2 = logtimer.read_us();
shimniok 0:a6a169de725f 215 fprintf(stdout, "%d\n", t2-t1);
shimniok 0:a6a169de725f 216
shimniok 0:a6a169de725f 217 return;
shimniok 0:a6a169de725f 218 }
shimniok 0:a6a169de725f 219
shimniok 0:a6a169de725f 220
shimniok 0:a6a169de725f 221 FILE *openlog(char *prefix)
shimniok 0:a6a169de725f 222 {
shimniok 0:a6a169de725f 223 FILE *fp = 0;
shimniok 0:a6a169de725f 224 char myname[64];
shimniok 0:a6a169de725f 225
shimniok 0:a6a169de725f 226 pc.printf("Opening file...\n");
shimniok 0:a6a169de725f 227
shimniok 0:a6a169de725f 228 while (fp == 0) {
shimniok 0:a6a169de725f 229 if ((fp = fopen("/log/test.txt", "w")) == 0) {
shimniok 0:a6a169de725f 230 pc.printf("Waiting for filesystem to come online...");
shimniok 0:a6a169de725f 231 wait(0.200);
shimniok 0:a6a169de725f 232 lcd.pos(0,1);
shimniok 0:a6a169de725f 233 lcd.printf("%-16s", "Waiting for fs");
shimniok 0:a6a169de725f 234 }
shimniok 0:a6a169de725f 235 }
shimniok 0:a6a169de725f 236 fclose(fp);
shimniok 0:a6a169de725f 237
shimniok 0:a6a169de725f 238 for (int i = 0; i < 1000; i++) {
shimniok 0:a6a169de725f 239 sprintf(myname, "/log/%s%03d.csv", prefix, i);
shimniok 0:a6a169de725f 240 if ((fp = fopen(myname, "r")) == 0) {
shimniok 0:a6a169de725f 241 break;
shimniok 0:a6a169de725f 242 } else {
shimniok 0:a6a169de725f 243 fclose(fp);
shimniok 0:a6a169de725f 244 }
shimniok 0:a6a169de725f 245 }
shimniok 0:a6a169de725f 246 fp = fopen(myname, "w");
shimniok 0:a6a169de725f 247 if (fp == 0) {
shimniok 0:a6a169de725f 248 pc.printf("file write failed: %s\n", myname);
shimniok 0:a6a169de725f 249 } else {
shimniok 0:a6a169de725f 250
shimniok 0:a6a169de725f 251 // TODO -- set error message, get rid of writing to terminal
shimniok 0:a6a169de725f 252
shimniok 0:a6a169de725f 253 //status = true;
shimniok 0:a6a169de725f 254 pc.printf("opened %s for writing\n", myname);
shimniok 0:a6a169de725f 255 lcd.pos(0,1);
shimniok 0:a6a169de725f 256 lcd.printf("%-16s", myname);
shimniok 0:a6a169de725f 257 }
shimniok 0:a6a169de725f 258
shimniok 0:a6a169de725f 259 return fp;
shimniok 0:a6a169de725f 260 }
shimniok 0:a6a169de725f 261
shimniok 0:a6a169de725f 262
shimniok 0:a6a169de725f 263 // Find the next unused filename of the form logger##.csv where # is 0-9
shimniok 0:a6a169de725f 264 //
shimniok 0:a6a169de725f 265 bool initLogfile()
shimniok 0:a6a169de725f 266 {
shimniok 0:a6a169de725f 267 bool status = false;
shimniok 0:a6a169de725f 268
shimniok 0:a6a169de725f 269 logp = openlog("log");
shimniok 0:a6a169de725f 270
shimniok 0:a6a169de725f 271 if (logp != 0) {
shimniok 0:a6a169de725f 272 status = true;
shimniok 0:a6a169de725f 273 //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:a6a169de725f 274 }
shimniok 0:a6a169de725f 275
shimniok 0:a6a169de725f 276 return status;
shimniok 0:a6a169de725f 277 }
shimniok 0:a6a169de725f 278
shimniok 0:a6a169de725f 279 void closeLogfile(void)
shimniok 0:a6a169de725f 280 {
shimniok 0:a6a169de725f 281 if (logp) fclose(logp);
shimniok 0:a6a169de725f 282 }