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@15:01fb4916a5cd, 2018-11-29 (annotated)

Committer:
shimniok
Date:
Thu Nov 29 17:11:34 2018 +0000
Revision:
15:01fb4916a5cd
Parent:
3:42f3821c4e54
Child:
23:a34af501ea89
temp fix for setSteering() compile err in shell.cpp; updated logging class

Who changed what in which revision?

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