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@2:fbc6e3cf3ed8, 2013-06-06 (annotated)

Committer:
shimniok
Date:
Thu Jun 06 13:40:23 2013 +0000
Revision:
2:fbc6e3cf3ed8
Parent:
1:cb84b477886c
Child:
3:42f3821c4e54
Sort-of working version, still some errors with estimation. Added clamp() function.

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