add sd card trip logging and fix disarming when boards turned off

This commit is contained in:
interfisch 2023-06-18 02:12:10 +02:00
parent 6c69e4c895
commit 4b5797d2f4
6 changed files with 179 additions and 28 deletions

View File

@ -103,6 +103,12 @@ float max_filtered_currentAll;
float min_filtered_currentAll; float min_filtered_currentAll;
float max_meanSpeed; float max_meanSpeed;
float minSpeedms; //speed in m/s of slowest wheel
double overallTrip; //m. trip with read distance from sd card
double trip; //m. trip distance since boot
double currentConsumed;
double overallCurrentConsumed;
float lastTripVoltage=0;
int16_t cmd_send=0; int16_t cmd_send=0;
@ -119,6 +125,7 @@ bool button_start_state=false;
#define DEBOUNCE_TIME 50 #define DEBOUNCE_TIME 50
bool armed = false; //cmd output values forced to 0 if false bool armed = false; //cmd output values forced to 0 if false
bool statswritten = true;

View File

@ -104,7 +104,9 @@ void display_drivingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) {
display.setTextColor(SSD1306_WHITE); // Draw white text display.setTextColor(SSD1306_WHITE); // Draw white text
display.setCursor(0,SCREEN_HEIGHT-(SCREEN_HEIGHT-18)/2 - 3); // Start at top-left corner display.setCursor(0,SCREEN_HEIGHT-(SCREEN_HEIGHT-18)/2 - 3); // Start at top-left corner
float _speeddisplay=(escFront.getMeanSpeed()+escRear.getMeanSpeed())/2.0*3.6; //float _speeddisplay=(escFront.getMeanSpeed()+escRear.getMeanSpeed())/2.0*3.6;
float _speeddisplay=minSpeedms*3.6;
//_speeddisplay=(millis()/1000)%21; //debugging //_speeddisplay=(millis()/1000)%21; //debugging
char buf[8]; char buf[8];
dtostrf(_speeddisplay,1,1,buf); dtostrf(_speeddisplay,1,1,buf);
@ -172,6 +174,14 @@ void display_debugDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) {
} }
void display_standingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) { void display_standingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) {
double _displaytrip=trip;
double _displaycurrent=currentConsumed;
bool _displayOverall= ((millis()/3000)%2==0);
if (_displayOverall) { //alternate between this trip and overall trip
_displaytrip=overallTrip;
_displaycurrent=overallCurrentConsumed;
}
char buf[8]; char buf[8];
display.setFont(); display.setFont();
display.setCursor(0,0); display.setCursor(0,0);
@ -190,19 +200,23 @@ void display_standingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) {
display.println(); display.println();
display.print(F("Trip:")); display.print(F("Trip:"));
dtostrf(min(escFront.getTrip(),escRear.getTrip()),1,0,buf); dtostrf(_displaytrip,1,0,buf);
display.print((String)buf); display.print((String)buf);
display.print(" m "); display.print(" m ");
dtostrf(escFront.getCurrentConsumed()+escRear.getCurrentConsumed(),1,2,buf); dtostrf(_displaycurrent,1,2,buf);
display.print((String)buf); display.print((String)buf);
display.print(" Ah"); display.print(" Ah");
display.println(); display.println();
display.print(F("Eff: ")); display.print(F("Eff: "));
dtostrf( min(escFront.getTrip(),escRear.getTrip())/1000/(escFront.getCurrentConsumed()+escRear.getCurrentConsumed()) ,1,2,buf); dtostrf( _displaytrip/1000/_displaycurrent ,1,2,buf);
display.print((String)buf); display.print((String)buf);
display.print(" km/Ah"); display.print(" km/Ah");
if (_displayOverall){
display.print(" sum");
}
display.println(); display.println();

View File

@ -20,27 +20,27 @@ bool initLogging() {
// see if the card is present and can be initialized: // see if the card is present and can be initialized:
if (!SD.begin(SDCHIPSELECT)) { if (!SD.begin(SDCHIPSELECT)) {
Serial.println("Card failed, or not present"); Serial.println("Card failed, or not present");
display.print(F("SD Init Fail!")); display.display(); display.println(F("SD Init Fail!")); display.display();
datalogging=false; //disable logging datalogging=false; //disable logging
delay(1000); delay(1000);
return false; return false;
}else{ }else{
Serial.println("Card initialized."); Serial.println("Card initialized.");
display.print(F("LOG=")); display.display(); display.println(F("LOG=")); display.display();
if (datalogging){ if (datalogging){
int filenumber=0; int filenumber=0;
char buffer[6]; char buffer[6];
sprintf(buffer, "%04d", filenumber); sprintf(buffer, "%04d", filenumber);
datalogging_filename="LOG_"+String(buffer)+".TXT"; datalogging_filename="LOG_"+String(buffer)+".TXT";
while(SD.exists(datalogging_filename) && filenumber<10000) { while(SD.exists(datalogging_filename) && filenumber<10000) {
Serial.print(datalogging_filename); Serial.println(" exists"); Serial.print(datalogging_filename); Serial.println(" exists");
filenumber++; filenumber++;
sprintf(buffer, "%04d", filenumber); sprintf(buffer, "%04d", filenumber);
datalogging_filename="LOG_"+String(buffer)+".TXT"; datalogging_filename="LOG_"+String(buffer)+".TXT";
} }
Serial.print(datalogging_filename); Serial.println(" is free"); Serial.print(datalogging_filename); Serial.println(" is free");
display.print(datalogging_filename); display.display(); display.print(datalogging_filename); display.display();
} }
} }
return true; return true;
@ -65,7 +65,7 @@ void loggingLoop(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm
dataFile.print("current_FrontL,current_FrontR,current_RearL,current_RearR,"); dataFile.print("current_FrontL,current_FrontR,current_RearL,current_RearR,");
dataFile.print("rpm_FrontL,rpm_FrontR,rpm_RearL,rpm_RearR,"); dataFile.print("rpm_FrontL,rpm_FrontR,rpm_RearL,rpm_RearR,");
dataFile.print("temp_Front,temp_Rear,vbat_Front,vbat_Rear,"); dataFile.print("temp_Front,temp_Rear,vbat_Front,vbat_Rear,");
dataFile.print("currentAll,throttle,brake,speed,trip_Front,trip_Rear,currentConsumed_Front,currentConsumed_Rear,"); dataFile.print("currentAll,throttle,brake,speed,trip_Front,trip_Rear,trip,currentConsumed_Front,currentConsumed_Rear,currentConsumed,");
dataFile.println("temp_ESCFront,temp_ESCRear,temp_Air,looptime_duration"); dataFile.println("temp_ESCFront,temp_ESCRear,temp_Air,looptime_duration");
dataFile.print("#TIMESTAMP:"); dataFile.println(now()); dataFile.print("#TIMESTAMP:"); dataFile.println(now());
logging_headerWritten=true; logging_headerWritten=true;
@ -93,12 +93,15 @@ void loggingLoop(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm
dataFile.print((escFront.getMeanSpeed()+escRear.getMeanSpeed())/2.0); dataFile.print(","); dataFile.print((escFront.getMeanSpeed()+escRear.getMeanSpeed())/2.0); dataFile.print(",");
dataFile.print(escFront.getTrip()); dataFile.print(","); dataFile.print(escFront.getTrip()); dataFile.print(",");
dataFile.print(escRear.getTrip()); dataFile.print(","); dataFile.print(escRear.getTrip()); dataFile.print(",");
dataFile.print(trip); dataFile.print(",");
dataFile.print(escFront.getCurrentConsumed(),3); dataFile.print(","); dataFile.print(escFront.getCurrentConsumed(),3); dataFile.print(",");
dataFile.print(escRear.getCurrentConsumed(),3); dataFile.print(","); dataFile.print(escRear.getCurrentConsumed(),3); dataFile.print(",");
dataFile.print(currentConsumed,3); dataFile.print(",");
dataFile.print(temp_ESCFront,2); dataFile.print(","); dataFile.print(temp_ESCFront,2); dataFile.print(",");
dataFile.print(temp_ESCRear,2); dataFile.print(","); dataFile.print(temp_ESCRear,2); dataFile.print(",");
dataFile.print(temp_Air,2); dataFile.print(","); dataFile.print(temp_Air,2); dataFile.print(",");
dataFile.print(looptime_duration,0); dataFile.print(","); dataFile.print(looptime_duration); dataFile.print(",");
looptime_duration=0; //reset
dataFile.println(""); dataFile.println("");
dataFile.close(); dataFile.close();
@ -127,4 +130,90 @@ String getLogFilename() {
return datalogging_filename; return datalogging_filename;
} }
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{
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)=="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);
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) {
File myFile = SD.open("stats.txt", FILE_WRITE);
if (myFile) {
myFile.print("trp=");
myFile.println(overallTrip);
myFile.print("occ=");
myFile.println(overallCurrentConsumed);
myFile.print("vlt=");
float _voltage=(escFront.getFeedback_batVoltage()+escRear.getFeedback_batVoltage())/2.0;
myFile.println(_voltage,2);
myFile.close();
}
}
}
void resetTrip() {
if (datalogging) {
File myFile = SD.open("stats.txt", FILE_WRITE);
if (myFile) {
myFile.print("trp=");
myFile.println(0);
myFile.print("occ=");
myFile.println(0);
myFile.print("vlt=");
myFile.println(0);
myFile.flush();
delay(10);
myFile.close();
}
}
}
#endif #endif

View File

@ -71,9 +71,7 @@ void temperatureLoop(unsigned long loopmillis) {
flag_requestTemperatures=false; flag_requestTemperatures=false;
} }
if (!flag_requestTemperatures) { if (!flag_requestTemperatures) {
unsigned long _start=millis();
sensors.requestTemperatures(); //this takes ~34ms sensors.requestTemperatures(); //this takes ~34ms
Serial.print("DS Request took ms:"); Serial.println(millis()-_start);
flag_requestTemperatures=true; flag_requestTemperatures=true;
} }
if (sensors.isConversionComplete()) { if (sensors.isConversionComplete()) {

@ -1 +1 @@
Subproject commit 8d180debf7633d3a8ff86a588c00b199c0876225 Subproject commit 150ce61f9f8d8c013f2fca424ec2976c3582d403

View File

@ -1,3 +1,9 @@
//TODO:
/*
- reset Trip to 0 by button press or something. function: resetTrip()
*/
#include <Arduino.h> #include <Arduino.h>
@ -107,6 +113,8 @@ void setup()
initResult=initLogging(); initResult=initLogging();
led_simpeProgress(2,initResult); led_simpeProgress(2,initResult);
escFront.init(); escFront.init();
led_simpeProgress(3,true); led_simpeProgress(3,true);
@ -146,8 +154,17 @@ void setup()
led_simpeProgress(7,true); led_simpeProgress(7,true);
} }
if (datalogging) { //sd init was successful
initResult=loadTripSD();
}else{
initResult=false;
}
led_simpeProgress(8,initResult);
initTemperature(); initTemperature();
led_simpeProgress(8,true); led_simpeProgress(9,true);
@ -254,6 +271,22 @@ void loop() {
escFront.update(loopmillis); escFront.update(loopmillis);
escRear.update(loopmillis); escRear.update(loopmillis);
static unsigned long last_statsupdate=0;
#define STATSUPDATEINTERVAL 100
if (loopmillis-last_statsupdate>STATSUPDATEINTERVAL) {
minSpeedms=min(escFront.getWheelspeed_L(),min(escFront.getWheelspeed_R(),min(escRear.getWheelspeed_L(),escRear.getWheelspeed_R()))); //take speed of slowest wheel
float _tripincrease=abs(minSpeedms) * ((loopmillis-last_statsupdate)/1000.0);
trip+=_tripincrease;
overallTrip+=_tripincrease;
float _currentIncrease=(escFront.getFiltered_curL()+escFront.getFiltered_curR()+escRear.getFiltered_curL()+escRear.getFiltered_curR())* ((loopmillis-last_statsupdate)/1000.0)/3600.0; //amp hours
currentConsumed += _currentIncrease;
overallCurrentConsumed += _currentIncrease;
last_statsupdate=loopmillis;
}
/* TODO: remove this if, because everything contained in esc.update() /* TODO: remove this if, because everything contained in esc.update()
if (loopmillis - last_send > SENDPERIOD) { //Calculate motor stuff and send to motor controllers if (loopmillis - last_send > SENDPERIOD) { //Calculate motor stuff and send to motor controllers
last_send=loopmillis; last_send=loopmillis;
@ -273,6 +306,13 @@ void loop() {
//If needed write log to serial port //If needed write log to serial port
//checkLog(); //TODO remove //checkLog(); //TODO remove
loggingLoop(loopmillis,escFront,escRear); loggingLoop(loopmillis,escFront,escRear);
if (!armed && !statswritten) { //write stats only once when disarmed
statswritten=true;
writeTrip(loopmillis,escFront,escRear);
}
if (statswritten && armed) {
statswritten=false;
}
leds(); leds();
led_update(loopmillis,escFront,escRear); //ws2812 led ring led_update(loopmillis,escFront,escRear); //ws2812 led ring
@ -296,9 +336,11 @@ void loop() {
boolean fanstatus=digitalRead(PIN_FAN); boolean fanstatus=digitalRead(PIN_FAN);
//float temp=max(escFront.getFeedback_boardTemp(),escRear.getFeedback_boardTemp()); //float temp=max(escFront.getFeedback_boardTemp(),escRear.getFeedback_boardTemp());
float temp=max(temp_ESCFront,temp_ESCRear); float temp=max(temp_ESCFront,temp_ESCRear);
if (!escFront.getControllerConnected() || !escRear.getControllerConnected()) { //boards are not powered on if (temp_ESCFront==DEVICE_DISCONNECTED_C || temp_ESCRear==DEVICE_DISCONNECTED_C ) { //temperature error
digitalWrite(PIN_FAN,HIGH); //force fan on digitalWrite(PIN_FAN,HIGH); //force fan on
}else{ //if both controllers are on, use temperature regulation
}else{ //normal temperature control
if (!fanstatus) { //fan is off if (!fanstatus) { //fan is off
if (temp>=fan_turn_on_temp){ if (temp>=fan_turn_on_temp){
digitalWrite(PIN_FAN,HIGH); digitalWrite(PIN_FAN,HIGH);
@ -309,6 +351,7 @@ void loop() {
} }
} }
} }
} }
@ -475,10 +518,11 @@ void failChecks() {
error_ads_max_read_interval=true; error_ads_max_read_interval=true;
writeLogComment(loopmillis, "Error ADS Max read interval"); writeLogComment(loopmillis, "Error ADS Max read interval");
} }
//Serial.print("Error ADS Max read interval=");Serial.println(loopmillis-last_adsread); //Serial.print("Error ADS Max read interval="); Serial.println(loopmillis-last_adsread);
} }
if (!controllers_connected || error_brake_outofrange || error_throttle_outofrange || error_ads_max_read_interval) { //any errors? if (!controllers_connected || error_brake_outofrange || error_throttle_outofrange || error_ads_max_read_interval) { //any errors?
armed=false; //disarm
throttle_pos=0; throttle_pos=0;
brake_pos=0; brake_pos=0;
} }
@ -533,7 +577,6 @@ void calculateSetSpeed(unsigned long timediff){
escRear.setSpeed(cmd_send_toMotor,cmd_send_toMotor); escRear.setSpeed(cmd_send_toMotor,cmd_send_toMotor);
log_update=true; log_update=true;
} }
/* /*