Diff: logging/logging.cpp

Revision:

23:a34af501ea89

Parent:

15:01fb4916a5cd

--- a/logging/logging.cpp	Thu Nov 29 19:45:49 2018 +0000
+++ b/logging/logging.cpp	Fri Nov 30 15:41:05 2018 +0000
@@ -1,273 +0,0 @@
-#include "SystemState.h"
-#include "globals.h"
-#include "logging.h"
-#include "SDFileSystem.h"
-#include "SerialGraphicLCD.h"
-
-// TODO 2 set up logging out of low priority interrupt handler
-
-//SDFileSystem sd(p5, p6, p7, p8, "log"); // mosi, miso, sclk, cs
-static FILE *logp;
-
-Timer logtimer;
-//extern int bufCount;
-
-/*
-void logData( const SystemState s ) {
-    unsigned char buf[512]; // for now we really only need ~256 bytes but in case I add more to state...
-    unsigned char *state = (unsigned char *) &s;
-    //unsigned int t1, t2, t3;
-    //logtimer.start();
-    //logtimer.reset();
-    if (logp) {
-        //t1 = logtimer.read_us();
-        encode(state, sizeof(s), buf, 0); // infinite line size
-        //t2 = logtimer.read_us();
-        fputs((char *) buf, logp);
-        fputs("\n", logp);
-        bufCount--;
-        fprintf(stdout, "bufCount: %d\n", bufCount);
-        //t3 = logtimer.read_us();
-        //fprintf(stdout, "%d %d\n", t3-t2, t2-t1);
-    }
-}
-*/
-
-// from Arduino source
-size_t printNumber(FILE *f, unsigned long n)
-{
-    char buf[8 * sizeof(long) + 1]; // Assumes 8-bit chars plus zero byte.
-    char *str = &buf[sizeof(buf) - 1];
-
-    *str = '\0';
-
-    do {
-        unsigned long m = n;
-        n /= 10;
-        char c = m - 10 * n;
-        *--str = c + '0';
-    } while(n);
-
-    return fputs(str, f);
-}
-
-// from Arduino source
-size_t printInt(FILE *f, long n)
-{
-    int t = 0;
-    if (n < 0) {
-        t = fputc('-', f);
-        n = -n;
-    }
-    return printNumber(f, n) + t;
-}
-
-// from Arduino source
-size_t printFloat(FILE *f, double number, uint8_t digits)
-{
-    size_t n=0;
-
-    if (isnan(number)) return fputs("nan", f);
-    if (isinf(number)) return fputs("inf", f);
-    if (number > 4294967040.0) return fputs("ovf", f);  // constant determined empirically
-    if (number <-4294967040.0) return fputs("ovf", f);  // constant determined empirically
-
-    // Handle negative numbers
-    if (number < 0.0) {
-        n += fputc('-', f);
-        number = -number;
-    }
-
-    // Round correctly so that print(1.999, 2) prints as "2.00"
-    double rounding = 0.5;
-    for (uint8_t i=0; i < digits; ++i)
-        rounding /= 10.0;
-
-    number += rounding;
-
-    // Extract the integer part of the number and print it
-    unsigned long int_part = (unsigned long)number;
-    double remainder = number - (double)int_part;
-    n += printInt(f, int_part);
-
-    // Print the decimal point, but only if there are digits beyond
-    if (digits > 0) {
-        n += fputc('.', f);
-    }
-
-    // Extract digits from the remainder one at a time
-    while (digits-- > 0) {
-        remainder *= 10.0;
-        int toPrint = int(remainder);
-        n += fputc(toPrint+'0', f);
-        remainder -= toPrint;
-    }
-
-    return n;
-}
-
-
-// If I use arduino style print routines, logging takes ~1000 / ~8000 usec
-// the big sprintf takes ~ 700-750 usec all by itself
-void logData( SystemState *s )
-{
-    //char buf[256];
-    //unsigned int t1, t2;
-    //logtimer.start();
-    //logtimer.reset();
-    //t1 = logtimer.read_us();
-
-    if (s) {
-        printInt(logp, s->millis);
-        fputc(',',logp);
-        printFloat(logp, s->current, 2);
-        fputc(',',logp);
-        printFloat(logp, s->voltage, 2);
-        fputc(',',logp);
-        for (int q=0; q < 3; q++) {
-            printFloat(logp, s->gyro[q], 6);
-            fputc(',',logp);
-        }
-        printInt(logp, s->gTemp);
-        fputc(',',logp);
-        for (int q=0; q < 3; q++) {
-            printInt(logp, s->a[q]);
-            fputc(',',logp);
-        }
-        /*
-        for (int q=0; q < 3; q++) {
-            printInt(logp, s->m[q]);
-            fputc(',',logp);
-        }
-        */
-        printFloat(logp, s->gHeading, 2);
-        fputc(',',logp);
-
-        // GPS 1
-        fprintf(logp, "%.7f,%.7f,", s->gpsLatitude, s->gpsLongitude);
-        //printFloat(logp, s->gpsLatitude, 7);
-        //fputc(',',logp);
-        //printFloat(logp, s->gpsLongitude, 7);
-        //fputc(',',logp);
-        printFloat(logp, s->gpsCourse_deg, 2);
-        fputc(',',logp);
-        printFloat(logp, s->gpsSpeed_mps, 2);
-        fputc(',',logp);
-        printFloat(logp, s->gpsHDOP, 1);
-        fputc(',',logp);
-        printInt(logp, s->gpsSats);
-        fputc(',',logp);
-        // Encoders
-        printFloat(logp, s->lrEncDistance, 7);
-        fputc(',',logp);
-        printFloat(logp, s->rrEncDistance, 7);
-        fputc(',',logp);
-        printFloat(logp, s->lrEncSpeed, 2);
-        fputc(',',logp);
-        printFloat(logp, s->rrEncSpeed, 2);
-        fputc(',',logp);
-        printFloat(logp, s->encHeading, 2);
-        fputc(',',logp);
-        // Estimates
-        printFloat(logp, s->estHeading, 2);
-        fputc(',',logp);
-        printFloat(logp, s->estLagHeading, 2);
-        fputc(',',logp);
-        printFloat(logp, s->estLatitude,  7);
-        fputc(',',logp);
-        printFloat(logp, s->estLongitude, 7);
-        fputc(',',logp);
-        printFloat(logp, s->estX, 4);
-        fputc(',',logp);
-        printFloat(logp, s->estY, 4);
-        fputc(',',logp);
-        // Nav
-        printInt(logp, s->nextWaypoint);
-        fputc(',',logp);
-        printFloat(logp, s->bearing, 2);
-        fputc(',',logp);
-        printFloat(logp, s->distance, 3);
-        fputc(',',logp);
-        printFloat(logp, s->steerAngle, 3);
-        fputc(',',logp);
-        printFloat(logp, s->errHeading, 3);
-        fputc(',',logp);
-        printFloat(logp, s->LABrg, 2);
-        fputc(',',logp);
-        printFloat(logp, s->LAx, 4);
-        fputc(',',logp);
-        printFloat(logp, s->LAy, 4);
-        fputc('\n',logp);
-        fflush(logp);
-
-        //t2 = logtimer.read_us();
-        //fprintf(stdout, "%d\n", t2-t1);
-    }
-
-    return;
-}
-
-
-FILE *openlog(const char *prefix)
-{
-    FILE *fp = 0;
-    char myname[64];
-
-    pc.printf("Opening file...\n");
-
-    while (fp == 0) {
-        sprintf(myname, "%s/test.txt", LOGDIR);
-        if ((fp = fopen(myname, "w")) == 0) {
-            pc.printf("Waiting for filesystem to come online...");
-            wait(0.200);
-            lcd.pos(0,1);
-            lcd.printf("%-16s", "Waiting for fs");
-        }
-    }
-    fclose(fp);
-
-    for (int i = 0; i < 1000; i++) {
-        sprintf(myname, "%s/%s%03d.csv", LOGDIR, prefix, i);
-        if ((fp = fopen(myname, "r")) == 0) {
-            break;
-        } else {
-            fclose(fp);
-        }
-    }
-    fp = fopen(myname, "w");
-    if (fp == 0) {
-        pc.printf("file write failed: %s\n", myname);
-    } else {
-
-        // TODO 3 set error message, get rid of writing to terminal
-
-        //status = true;
-        pc.printf("opened %s for writing\n", myname);
-        lcd.pos(0,1);
-        lcd.printf("%-16s", myname);
-    }
-
-    return fp;
-}
-
-
-// Find the next unused filename of the form logger##.csv where # is 0-9
-//
-bool initLogfile()
-{
-    bool status = false;
-
-    logp = openlog("log");
-
-    if (logp != 0) {
-        status = true;
-        //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");
-    }
-
-    return status;
-}
-
-void closeLogfile(void)
-{
-    if (logp) fclose(logp);
-}