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@0:a6a169de725f, 2013-05-27 (annotated)

Committer:
shimniok
Date:
Mon May 27 13:26:03 2013 +0000
Revision:
0:a6a169de725f
Child:
1:cb84b477886c
Working version with priorities set and update time display

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 0:a6a169de725f 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 0:a6a169de725f 179
shimniok 0:a6a169de725f 180 // GPS 2
shimniok 0:a6a169de725f 181 fprintf(logp, "%.7f,%.7f,", s.gpsLatitude2, s.gpsLongitude2);
shimniok 0:a6a169de725f 182 //printFloat(logp, s.gpsLatitude2, 7);
shimniok 0:a6a169de725f 183 //fputc(',',logp);
shimniok 0:a6a169de725f 184 //printFloat(logp, s.gpsLongitude2, 7);
shimniok 0:a6a169de725f 185 //fputc(',',logp);
shimniok 0:a6a169de725f 186 printFloat(logp, s.gpsCourse_deg2, 2);
shimniok 0:a6a169de725f 187 fputc(',',logp);
shimniok 0:a6a169de725f 188 printFloat(logp, s.gpsSpeed_mps2, 2);
shimniok 0:a6a169de725f 189 fputc(',',logp);
shimniok 0:a6a169de725f 190 printFloat(logp, s.gpsHDOP2, 1);
shimniok 0:a6a169de725f 191 fputc(',',logp);
shimniok 0:a6a169de725f 192 printInt(logp, s.gpsSats2);
shimniok 0:a6a169de725f 193 fputc(',',logp);
shimniok 0:a6a169de725f 194
shimniok 0:a6a169de725f 195
shimniok 0:a6a169de725f 196 printFloat(logp, s.lrEncDistance, 7);
shimniok 0:a6a169de725f 197 fputc(',',logp);
shimniok 0:a6a169de725f 198 printFloat(logp, s.rrEncDistance, 7);
shimniok 0:a6a169de725f 199 fputc(',',logp);
shimniok 0:a6a169de725f 200 printFloat(logp, s.lrEncSpeed, 2);
shimniok 0:a6a169de725f 201 fputc(',',logp);
shimniok 0:a6a169de725f 202 printFloat(logp, s.rrEncSpeed, 2);
shimniok 0:a6a169de725f 203 fputc(',',logp);
shimniok 0:a6a169de725f 204 printFloat(logp, s.encHeading, 2);
shimniok 0:a6a169de725f 205 fputc(',',logp);
shimniok 0:a6a169de725f 206 printFloat(logp, s.estHeading, 2);
shimniok 0:a6a169de725f 207 fputc(',',logp);
shimniok 0:a6a169de725f 208 printFloat(logp, s.estLatitude, 7);
shimniok 0:a6a169de725f 209 fputc(',',logp);
shimniok 0:a6a169de725f 210 printFloat(logp, s.estLongitude, 7);
shimniok 0:a6a169de725f 211 fputc(',',logp);
shimniok 0:a6a169de725f 212 printFloat(logp, s.estX, 4);
shimniok 0:a6a169de725f 213 fputc(',',logp);
shimniok 0:a6a169de725f 214 printFloat(logp, s.estY, 4);
shimniok 0:a6a169de725f 215 fputc(',',logp);
shimniok 0:a6a169de725f 216 printInt(logp, s.nextWaypoint);
shimniok 0:a6a169de725f 217 fputc(',',logp);
shimniok 0:a6a169de725f 218 printFloat(logp, s.bearing, 2);
shimniok 0:a6a169de725f 219 fputc(',',logp);
shimniok 0:a6a169de725f 220 printFloat(logp, s.distance, 3);
shimniok 0:a6a169de725f 221 fputc(',',logp);
shimniok 0:a6a169de725f 222 /*
shimniok 0:a6a169de725f 223 printFloat(logp, s.gbias, 3);
shimniok 0:a6a169de725f 224 fputc(',',logp);
shimniok 0:a6a169de725f 225 printFloat(logp, s.errAngle, 5);
shimniok 0:a6a169de725f 226 fputc(',',logp);
shimniok 0:a6a169de725f 227 printFloat(logp, s.leftRanger, 3);
shimniok 0:a6a169de725f 228 fputc(',',logp);
shimniok 0:a6a169de725f 229 printFloat(logp, s.rightRanger, 3);
shimniok 0:a6a169de725f 230 fputc(',',logp);
shimniok 0:a6a169de725f 231 printFloat(logp, s.centerRanger, 3);
shimniok 0:a6a169de725f 232 fputc(',',logp);
shimniok 0:a6a169de725f 233 printFloat(logp, s.crossTrackErr, 3);
shimniok 0:a6a169de725f 234 */
shimniok 0:a6a169de725f 235 fputc('\n',logp);
shimniok 0:a6a169de725f 236
shimniok 0:a6a169de725f 237 t2 = logtimer.read_us();
shimniok 0:a6a169de725f 238 fprintf(stdout, "%d\n", t2-t1);
shimniok 0:a6a169de725f 239
shimniok 0:a6a169de725f 240 return;
shimniok 0:a6a169de725f 241 }
shimniok 0:a6a169de725f 242
shimniok 0:a6a169de725f 243
shimniok 0:a6a169de725f 244 FILE *openlog(char *prefix)
shimniok 0:a6a169de725f 245 {
shimniok 0:a6a169de725f 246 FILE *fp = 0;
shimniok 0:a6a169de725f 247 char myname[64];
shimniok 0:a6a169de725f 248
shimniok 0:a6a169de725f 249 pc.printf("Opening file...\n");
shimniok 0:a6a169de725f 250
shimniok 0:a6a169de725f 251 while (fp == 0) {
shimniok 0:a6a169de725f 252 if ((fp = fopen("/log/test.txt", "w")) == 0) {
shimniok 0:a6a169de725f 253 pc.printf("Waiting for filesystem to come online...");
shimniok 0:a6a169de725f 254 wait(0.200);
shimniok 0:a6a169de725f 255 lcd.pos(0,1);
shimniok 0:a6a169de725f 256 lcd.printf("%-16s", "Waiting for fs");
shimniok 0:a6a169de725f 257 }
shimniok 0:a6a169de725f 258 }
shimniok 0:a6a169de725f 259 fclose(fp);
shimniok 0:a6a169de725f 260
shimniok 0:a6a169de725f 261 for (int i = 0; i < 1000; i++) {
shimniok 0:a6a169de725f 262 sprintf(myname, "/log/%s%03d.csv", prefix, i);
shimniok 0:a6a169de725f 263 if ((fp = fopen(myname, "r")) == 0) {
shimniok 0:a6a169de725f 264 break;
shimniok 0:a6a169de725f 265 } else {
shimniok 0:a6a169de725f 266 fclose(fp);
shimniok 0:a6a169de725f 267 }
shimniok 0:a6a169de725f 268 }
shimniok 0:a6a169de725f 269 fp = fopen(myname, "w");
shimniok 0:a6a169de725f 270 if (fp == 0) {
shimniok 0:a6a169de725f 271 pc.printf("file write failed: %s\n", myname);
shimniok 0:a6a169de725f 272 } else {
shimniok 0:a6a169de725f 273
shimniok 0:a6a169de725f 274 // TODO -- set error message, get rid of writing to terminal
shimniok 0:a6a169de725f 275
shimniok 0:a6a169de725f 276 //status = true;
shimniok 0:a6a169de725f 277 pc.printf("opened %s for writing\n", myname);
shimniok 0:a6a169de725f 278 lcd.pos(0,1);
shimniok 0:a6a169de725f 279 lcd.printf("%-16s", myname);
shimniok 0:a6a169de725f 280 }
shimniok 0:a6a169de725f 281
shimniok 0:a6a169de725f 282 return fp;
shimniok 0:a6a169de725f 283 }
shimniok 0:a6a169de725f 284
shimniok 0:a6a169de725f 285
shimniok 0:a6a169de725f 286 // Find the next unused filename of the form logger##.csv where # is 0-9
shimniok 0:a6a169de725f 287 //
shimniok 0:a6a169de725f 288 bool initLogfile()
shimniok 0:a6a169de725f 289 {
shimniok 0:a6a169de725f 290 bool status = false;
shimniok 0:a6a169de725f 291
shimniok 0:a6a169de725f 292 logp = openlog("log");
shimniok 0:a6a169de725f 293
shimniok 0:a6a169de725f 294 if (logp != 0) {
shimniok 0:a6a169de725f 295 status = true;
shimniok 0:a6a169de725f 296 //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 297 }
shimniok 0:a6a169de725f 298
shimniok 0:a6a169de725f 299 return status;
shimniok 0:a6a169de725f 300 }
shimniok 0:a6a169de725f 301
shimniok 0:a6a169de725f 302 void closeLogfile(void)
shimniok 0:a6a169de725f 303 {
shimniok 0:a6a169de725f 304 if (logp) fclose(logp);
shimniok 0:a6a169de725f 305 }