545 lines
18 KiB
C
545 lines
18 KiB
C
#ifndef _LOGGING_H_
|
|
#define _LOGGING_H_
|
|
|
|
// SD Card Logging
|
|
|
|
#include <SPI.h>
|
|
#include <SD.h> //Format sd cart with FAT or FAT16. FAT32 for >1GB Cards on Teensy4.1
|
|
|
|
//#define SDCHIPSELECT 14
|
|
boolean sdcard_available=false;
|
|
boolean datalogging=true;
|
|
String datalogging_filename="UNKNOWN.txt";
|
|
|
|
uint16_t chunksize=128; //for bulk data transmission
|
|
|
|
#define LOGGINGINTERVAL 100 //logging interval when driving
|
|
#define LOGGINGSTANDSTILLTIME 30000 //after which time of standstill logging interval should change
|
|
#define LOGGINGINTERVALSTANDSTILL 500 //logging interval when standing still
|
|
#define LOGGINGINTERVALDISARMED 1000 //logging interval when disarmed
|
|
|
|
bool serialCommandEcho_Enabled=true;
|
|
|
|
bool initLogging();
|
|
void loggingLoop(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm& escRear);
|
|
|
|
|
|
void writeLogComment(unsigned long time, String msg);
|
|
void printFileListing(bool pretty);
|
|
void printDirectory(File dir, int numTabs,String parent,bool printSize,bool printFullDirectoryNames);
|
|
void printFile(String filename);
|
|
void printFilesize(String filename);
|
|
void transferFile(String filename);
|
|
void removeFile(String filename);
|
|
void serialCommandLoop(unsigned long loopmillis, ESCSerialComm& escFront, ESCSerialComm& escRear);
|
|
float getBatteryVoltage(ESCSerialComm& escFront, ESCSerialComm& escRear);
|
|
|
|
|
|
bool initLogging() {
|
|
Serial.print("Datalogging is ");
|
|
if (datalogging) {
|
|
Serial.println("enabled");
|
|
}else{
|
|
Serial.println("disabled");
|
|
}
|
|
Serial.print("Initializing SD card...");
|
|
// see if the card is present and can be initialized:
|
|
if (!SD.begin(BUILTIN_SDCARD)) {
|
|
Serial.println("Card failed, or not present");
|
|
display.println(F("SD Init Fail!")); display.display();
|
|
sdcard_available=false; //disable logging
|
|
datalogging=false;
|
|
delay(1000);
|
|
return false;
|
|
}else{
|
|
sdcard_available=true;
|
|
Serial.println("Card initialized.");
|
|
display.print(F("LOG=")); display.display();
|
|
|
|
int filenumber=0;
|
|
char buffer[6];
|
|
sprintf(buffer, "%04d", filenumber);
|
|
datalogging_filename="LOG_"+String(buffer)+".TXT";
|
|
while(SD.exists(datalogging_filename.c_str()) && filenumber<10000) {
|
|
//Serial.print(datalogging_filename); Serial.println(" exists");
|
|
filenumber++;
|
|
sprintf(buffer, "%04d", filenumber);
|
|
datalogging_filename="LOG_"+String(buffer)+".TXT";
|
|
|
|
}
|
|
Serial.print(datalogging_filename); Serial.println(" is free");
|
|
display.print(datalogging_filename);
|
|
if (!datalogging) { //datalogging is disabled during boot
|
|
display.print(" OFF");
|
|
}
|
|
display.println(); display.display();
|
|
|
|
}
|
|
return true;
|
|
}
|
|
|
|
|
|
void loggingLoop(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm& escRear) {
|
|
|
|
static unsigned long last_datalogging_write=loopmillis; //initialize with current time to have first log written after one interval
|
|
static boolean logging_headerWritten=false;
|
|
|
|
unsigned long logginginterval=LOGGINGINTERVAL;
|
|
if (armed) { //Change Logginginterval when less is happening
|
|
if (loopmillis-last_notidle>LOGGINGSTANDSTILLTIME) {
|
|
logginginterval=LOGGINGINTERVALSTANDSTILL;
|
|
}
|
|
}else{ //disamred
|
|
logginginterval=LOGGINGINTERVALDISARMED;
|
|
}
|
|
|
|
|
|
if (loopmillis-last_datalogging_write>logginginterval)
|
|
{
|
|
last_datalogging_write=loopmillis;
|
|
|
|
if (datalogging && sdcard_available) {
|
|
|
|
File dataFile = SD.open(datalogging_filename.c_str(), FILE_WRITE);
|
|
|
|
if (dataFile) { // if the file is available, write to it
|
|
if (!logging_headerWritten) {
|
|
dataFile.print("time,cmd_FrontL,cmd_FrontR,cmd_RearL,cmd_RearR,");
|
|
dataFile.print("current_FrontL,current_FrontR,current_RearL,current_RearR,");
|
|
dataFile.print("rpm_FrontL,rpm_FrontR,rpm_RearL,rpm_RearR,");
|
|
dataFile.print("temp_ESCFront,temp_ESCRear,vbat_Front,vbat_Rear,");
|
|
dataFile.print("currentAll,throttle,brake,speed,trip_Front,trip_Rear,trip,currentConsumed_Front,currentConsumed_Rear,currentConsumed,watthoursConsumed,");
|
|
dataFile.println("temp_Front,temp_Rear,temp_Air,looptime_duration_min,looptime_duration_max");
|
|
dataFile.print("#TIMESTAMP:"); dataFile.println(now());
|
|
logging_headerWritten=true;
|
|
}
|
|
dataFile.print(loopmillis/1000.0,3); dataFile.print(",");
|
|
dataFile.print(escFront.getCmdL()); dataFile.print(",");
|
|
dataFile.print(escFront.getCmdR()); dataFile.print(",");
|
|
dataFile.print(escRear.getCmdL()); dataFile.print(",");
|
|
dataFile.print(escRear.getCmdR()); dataFile.print(",");
|
|
dataFile.print(escFront.getFiltered_curL(),3); dataFile.print(",");
|
|
dataFile.print(escFront.getFiltered_curR(),3); dataFile.print(",");
|
|
dataFile.print(escRear.getFiltered_curL(),3); dataFile.print(",");
|
|
dataFile.print(escRear.getFiltered_curR(),3); dataFile.print(",");
|
|
dataFile.print(escFront.getFeedback_speedL_meas()); dataFile.print(","); //+ //Todo: check if speed for R wheels needs to be negated
|
|
dataFile.print(escFront.getFeedback_speedR_meas()); dataFile.print(","); //-
|
|
dataFile.print(escRear.getFeedback_speedL_meas()); dataFile.print(","); //+
|
|
dataFile.print(escRear.getFeedback_speedR_meas()); dataFile.print(","); //-
|
|
dataFile.print(escFront.getFeedback_boardTemp()); dataFile.print(",");
|
|
dataFile.print(escRear.getFeedback_boardTemp()); dataFile.print(",");
|
|
dataFile.print(escFront.getFeedback_batVoltage()); dataFile.print(",");
|
|
dataFile.print(escRear.getFeedback_batVoltage()); dataFile.print(",");
|
|
dataFile.print(filtered_currentAll,3); dataFile.print(",");
|
|
dataFile.print(throttle_pos); dataFile.print(",");
|
|
dataFile.print(brake_pos); dataFile.print(",");
|
|
dataFile.print((escFront.getMeanSpeed()+escRear.getMeanSpeed())/2.0); dataFile.print(",");
|
|
dataFile.print(escFront.getTrip()); dataFile.print(",");
|
|
dataFile.print(escRear.getTrip()); dataFile.print(",");
|
|
dataFile.print(trip); dataFile.print(",");
|
|
dataFile.print(escFront.getCurrentConsumed(),3); dataFile.print(",");
|
|
dataFile.print(escRear.getCurrentConsumed(),3); dataFile.print(",");
|
|
dataFile.print(currentConsumed,3); dataFile.print(",");
|
|
dataFile.print(watthoursConsumed,3); dataFile.print(",");
|
|
dataFile.print(temp_Front,2); dataFile.print(",");
|
|
dataFile.print(temp_Rear,2); dataFile.print(",");
|
|
dataFile.print(temp_Air,2); dataFile.print(",");
|
|
dataFile.print(looptime_duration_min); dataFile.print(",");
|
|
dataFile.print(looptime_duration_max); //dataFile.print(",");
|
|
looptime_duration_max=0; //reset
|
|
looptime_duration_min=1000; //reset
|
|
|
|
|
|
|
|
dataFile.println("");
|
|
dataFile.close();
|
|
}else{ //dataFile not available
|
|
error_sdfile_unavailable=true;
|
|
}
|
|
}
|
|
|
|
looptime_duration_max=0; //reset
|
|
looptime_duration_min=1000; //reset
|
|
|
|
}
|
|
}
|
|
|
|
void writeLogComment(unsigned long time, String msg) {
|
|
//SerialRef.print("#"); SerialRef.print(time/1000.0,3); SerialRef.print(","); SerialRef.print(msg); SerialRef.println();
|
|
if (datalogging && sdcard_available) {
|
|
File dataFile = SD.open(datalogging_filename.c_str(), FILE_WRITE);
|
|
|
|
if (dataFile) { // if the file is available, write to it
|
|
dataFile.print("#");
|
|
dataFile.print(time/1000.0,3);
|
|
dataFile.print(",");
|
|
dataFile.print(msg);
|
|
dataFile.println();
|
|
dataFile.close();
|
|
}
|
|
}
|
|
}
|
|
|
|
String getLogFilename() {
|
|
return datalogging_filename;
|
|
}
|
|
|
|
bool getDatalogging() {
|
|
return datalogging;
|
|
}
|
|
|
|
bool loadTripSD()
|
|
{
|
|
//stats.txt containt lines with driven distance or consume current.
|
|
//the lowest line is the latest data
|
|
//data is usually written rather slowly
|
|
|
|
File myFile = SD.open("stats.txt", FILE_WRITE);
|
|
if (myFile) {
|
|
myFile.print("ini=");
|
|
myFile.println(now());
|
|
myFile.flush();
|
|
delay(100);
|
|
myFile.close();
|
|
Serial.println("TripSD: time written");
|
|
}else{
|
|
Serial.println("TripSD: could not open stats.txt");
|
|
return false;
|
|
}
|
|
|
|
|
|
myFile = SD.open("stats.txt");
|
|
if (myFile) {
|
|
while (myFile.available()) {
|
|
String line=myFile.readStringUntil('\n');
|
|
Serial.print("TripSD Read Line:");
|
|
Serial.println(line);
|
|
if (line.substring(0,4)=="trp="){
|
|
overallTrip=(line.substring(4)).toFloat();
|
|
}else if (line.substring(0,4)=="occ="){
|
|
overallCurrentConsumed=(line.substring(4)).toFloat();
|
|
}else if (line.substring(0,4)=="owh="){
|
|
overallWatthoursConsumed=(line.substring(4)).toFloat();
|
|
}else if (line.substring(0,4)=="vlt="){
|
|
lastTripVoltage=(line.substring(4)).toFloat();
|
|
}
|
|
}
|
|
Serial.print("TripSD trip:");
|
|
Serial.println(overallTrip);
|
|
display.print(F("TripSD=")); display.println(overallTrip);
|
|
Serial.print("TripSD current:");
|
|
Serial.println(overallCurrentConsumed);
|
|
Serial.print("TripSD watthours:");
|
|
Serial.println(overallWatthoursConsumed);
|
|
display.print(F("Cur. SD=")); display.println(overallCurrentConsumed); display.display();
|
|
Serial.print("TripSD voltage:");
|
|
Serial.println(lastTripVoltage);
|
|
display.print(F("Volt. SD=")); display.println(lastTripVoltage); display.display();
|
|
|
|
myFile.flush();
|
|
delay(10);
|
|
myFile.close();
|
|
}else{
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void writeTrip(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm& escRear) {
|
|
if (datalogging && sdcard_available) {
|
|
File myFile = SD.open("stats.txt", FILE_WRITE);
|
|
if (myFile) {
|
|
myFile.print("trp=");
|
|
myFile.println(overallTrip);
|
|
myFile.print("occ=");
|
|
myFile.println(overallCurrentConsumed);
|
|
myFile.print("owh=");
|
|
myFile.println(overallWatthoursConsumed);
|
|
myFile.print("vlt=");
|
|
float _voltage=getBatteryVoltage(escFront, escRear);
|
|
myFile.println(_voltage,2);
|
|
myFile.close();
|
|
}
|
|
}
|
|
}
|
|
|
|
float getBatteryVoltage(ESCSerialComm& escFront, ESCSerialComm& escRear) {
|
|
return (escFront.getFeedback_batVoltage()+escRear.getFeedback_batVoltage())/2.0;
|
|
}
|
|
|
|
void resetTrip() {
|
|
if (datalogging && sdcard_available) {
|
|
File myFile = SD.open("stats.txt", FILE_WRITE);
|
|
if (myFile) {
|
|
myFile.print("trp=");
|
|
myFile.println(0);
|
|
myFile.print("occ=");
|
|
myFile.println(0);
|
|
myFile.print("owh=");
|
|
myFile.println(0);
|
|
myFile.print("vlt=");
|
|
myFile.println(0);
|
|
myFile.flush();
|
|
delay(10);
|
|
myFile.close();
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
void serialCommandLoop(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm& escRear) {
|
|
#define MAX_MESSAGE_LENGTH 80
|
|
|
|
uint8_t timeoutcounter=MAX_MESSAGE_LENGTH;
|
|
|
|
while (Serial.available() > 0 && timeoutcounter>0)
|
|
{
|
|
timeoutcounter--;
|
|
|
|
static char message[MAX_MESSAGE_LENGTH];
|
|
static unsigned int message_pos = 0;
|
|
|
|
char inByte = Serial.read();
|
|
if (inByte == '\r') {
|
|
break; //ignore CR
|
|
}
|
|
|
|
|
|
if ( inByte != '\n' && (message_pos < MAX_MESSAGE_LENGTH - 1) )
|
|
{
|
|
|
|
message[message_pos] = inByte;
|
|
message_pos++;
|
|
}
|
|
|
|
else
|
|
{
|
|
message[message_pos] = '\0';
|
|
|
|
|
|
String smessage=String(message);
|
|
|
|
if(smessage.equals("echo off")) {
|
|
serialCommandEcho_Enabled=false;
|
|
}else if(smessage.equals("echo on")) {
|
|
serialCommandEcho_Enabled=true;
|
|
}
|
|
|
|
if (serialCommandEcho_Enabled) { //Echo Command
|
|
Serial.print("$"); Serial.println(message);
|
|
}
|
|
|
|
//Handle Commands
|
|
if(smessage.equals("test")) {
|
|
Serial.println("OK");
|
|
}else if(smessage.equals("ls")) {
|
|
printFileListing(false);
|
|
}else if(smessage.equals("ls -a")) {
|
|
printFileListing(true);
|
|
}else if(smessage.startsWith("cat")) {
|
|
String _filename=smessage.substring(4);
|
|
if (_filename.length()<3){ //if no filename given, use current log file
|
|
_filename=getLogFilename();
|
|
}
|
|
printFile(_filename);
|
|
}else if(smessage.startsWith("get")) {
|
|
String _filename=smessage.substring(4);
|
|
transferFile(_filename);
|
|
}else if(smessage.startsWith("sizeof")) {
|
|
String _filename=smessage.substring(7);
|
|
printFilesize(_filename);
|
|
}else if(smessage.startsWith("chunksize")) { //change chunksize for get command
|
|
String _chunksizestring=smessage.substring(10);
|
|
long _chunksize=_chunksizestring.toInt();
|
|
if (_chunksize>0 && _chunksize<LONG_MAX) { //value ok
|
|
chunksize=_chunksize;
|
|
}else{
|
|
Serial.println("Chunksize value out of range");
|
|
}
|
|
}else if(smessage.startsWith("rm")) {
|
|
removeFile(smessage.substring(3));
|
|
}else if(smessage.equals("log off")) {
|
|
writeLogComment(loopmillis, "Datalogging disabled by serial command");
|
|
datalogging=false;
|
|
Serial.print("Log disabled: "); Serial.println(datalogging_filename);
|
|
}else if(smessage.equals("log on")) {
|
|
datalogging=true;
|
|
writeLogComment(loopmillis, "Datalogging enabled by serial command");
|
|
Serial.print("Log enabled: "); Serial.println(datalogging_filename);
|
|
}else if(smessage.equals("show stats")) {
|
|
Serial.print("overallTrip\t"); Serial.println(overallTrip);
|
|
Serial.print("overallCurrentConsumed\t"); Serial.println(overallCurrentConsumed);
|
|
Serial.print("overallWatthoursConsumed\t"); Serial.println(overallWatthoursConsumed);
|
|
Serial.print("voltage\t"); Serial.println(getBatteryVoltage(escFront,escRear));
|
|
}else if(smessage.equals("show sensors")) {
|
|
Serial.print("loopmillis\t"); Serial.println(loopmillis);
|
|
Serial.print("temp_ESCFront\t"); Serial.println(escFront.getFeedback_boardTemp());
|
|
Serial.print("temp_ESCRear\t"); Serial.println(escRear.getFeedback_boardTemp());
|
|
Serial.print("vbat_Front\t"); Serial.println(escFront.getFeedback_batVoltage());
|
|
Serial.print("vbat_Rear\t"); Serial.println(escRear.getFeedback_batVoltage());
|
|
Serial.print("throttle\t"); Serial.println(throttle_pos);
|
|
Serial.print("brake\t"); Serial.println(brake_pos);
|
|
Serial.print("trip_Front\t"); Serial.println(escFront.getTrip());
|
|
Serial.print("trip_Rear\t"); Serial.println(escRear.getTrip());
|
|
Serial.print("trip\t"); Serial.println(trip);
|
|
Serial.print("currentConsumed_Front\t"); Serial.println(escFront.getCurrentConsumed(),3);
|
|
Serial.print("currentConsumed_Rear\t"); Serial.println(escRear.getCurrentConsumed(),3);
|
|
Serial.print("currentConsumed\t"); Serial.println(currentConsumed,3);
|
|
Serial.print("watthoursConsumed\t"); Serial.println(watthoursConsumed,3);
|
|
Serial.print("temp_Front\t"); Serial.println(temp_Front,3);
|
|
Serial.print("temp_Rear\t"); Serial.println(temp_Rear,3);
|
|
Serial.print("temp_Air\t"); Serial.println(temp_Air,3);
|
|
Serial.print("looptime_duration_min\t"); Serial.println(looptime_duration_min);
|
|
Serial.print("looptime_duration_max\t"); Serial.println(looptime_duration_max);
|
|
}
|
|
|
|
message_pos = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
void printFileListing(bool pretty) {
|
|
File root;
|
|
root = SD.open("/");
|
|
|
|
if (pretty) {
|
|
printDirectory(root, 0,"",true,false); //tabbed with file sizes
|
|
}else{
|
|
printDirectory(root, 0,"",false,true); //full paths without size
|
|
}
|
|
}
|
|
|
|
|
|
void printDirectory(File dir, int numTabs,String parent,bool printSize,bool printFullDirectoryNames) {
|
|
|
|
|
|
while (true) {
|
|
|
|
File entry = dir.openNextFile();
|
|
if (! entry) {
|
|
// no more files
|
|
break;
|
|
}
|
|
if (!printFullDirectoryNames) {
|
|
for (uint8_t i = 0; i < numTabs; i++) {
|
|
Serial.print('\t');
|
|
}
|
|
|
|
}
|
|
|
|
if (!entry.isDirectory() || !printFullDirectoryNames) {
|
|
Serial.print(parent+entry.name());
|
|
}
|
|
|
|
if (entry.isDirectory()) {
|
|
if (!printFullDirectoryNames) {
|
|
Serial.println("/");
|
|
}
|
|
printDirectory(entry, numTabs + 1,parent+entry.name()+"/",printSize,printFullDirectoryNames);
|
|
} else {
|
|
if (printSize) {
|
|
// files have sizes, directories do not
|
|
Serial.print("\t\t");
|
|
Serial.print(entry.size(), DEC);
|
|
}
|
|
Serial.println();
|
|
}
|
|
entry.close();
|
|
}
|
|
}
|
|
|
|
void printFilesize(String filename) {
|
|
File dataFile = SD.open(filename.c_str(), FILE_READ);
|
|
if (dataFile) {
|
|
Serial.println(dataFile.size(), DEC);
|
|
dataFile.close();
|
|
}else{
|
|
Serial.println('0');
|
|
}
|
|
|
|
}
|
|
|
|
|
|
void printFile(String filename) {
|
|
|
|
File dataFile = SD.open(filename.c_str(), FILE_READ);
|
|
|
|
|
|
|
|
// if the file is available, write to it:
|
|
if (dataFile) {
|
|
while (dataFile.available()) {
|
|
Serial.write(dataFile.read());
|
|
}
|
|
dataFile.close();
|
|
}
|
|
// if the file isn't open, pop up an error:
|
|
else {
|
|
Serial.print("error opening "); Serial.println(filename);
|
|
}
|
|
|
|
}
|
|
|
|
|
|
void transferFile(String filename) {
|
|
//similar to printFile, but with checksum and chunk based transfer
|
|
|
|
|
|
|
|
|
|
File dataFile = SD.open(filename.c_str(), FILE_READ);
|
|
|
|
bool transmiterror=false;
|
|
|
|
// if the file is available, write to it:
|
|
if (dataFile) {
|
|
while (dataFile.available() && !transmiterror) {
|
|
byte checksum=0;
|
|
for (uint16_t i=0;i<chunksize;i++) {
|
|
if (dataFile.available()) {
|
|
byte b = dataFile.read();
|
|
Serial.write(b);
|
|
checksum+=b;
|
|
}
|
|
}
|
|
int inByte=-1;
|
|
while (inByte==-1) {
|
|
if (Serial.available() > 0)
|
|
{
|
|
inByte = Serial.read();
|
|
|
|
if ((byte)inByte!=checksum){
|
|
transmiterror=true; //exit
|
|
Serial.println();
|
|
Serial.println("TRANSMISSION ERROR WRONG CHECKSUM");
|
|
}
|
|
}
|
|
}
|
|
|
|
}
|
|
dataFile.close();
|
|
}
|
|
// if the file isn't open, pop up an error:
|
|
else {
|
|
Serial.print("error opening "); Serial.println(filename);
|
|
}
|
|
|
|
}
|
|
|
|
|
|
void removeFile(String filename) {
|
|
|
|
SD.remove(filename.c_str());
|
|
if (SD.exists(filename.c_str())) {
|
|
Serial.println("File still exists");
|
|
} else {
|
|
Serial.println("OK");
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
#endif |