Made changes to the state machine, its been a late night, most data live

Ezra Boley · Ezra Boley · commit c6afc0775ff8 · 2019-07-16T04:54:39.000-05:00
diff --git a/Makefile b/Makefile
@@ -26,7 +26,7 @@ endif
 LV_USER   := "debian"
 HV_USER	  := "ezra"
 LV_IP  := "192.168.1.126"
-HV_IP  := "192.168.1.146"
+HV_IP  := "192.168.1.147"
 
 ifdef BB
 BEAGLE := ${BBCC}
diff --git a/embedded/app/main/badgerloop_HV.cpp b/embedded/app/main/badgerloop_HV.cpp
@@ -40,7 +40,7 @@ int init() {
 	SetupHVTelemRecv();	
 	
     /* Start 'black box' data saving */
-    SetupDataDump();
+/*    SetupDataDump();*/
 	
     return 0;	
 }
@@ -50,7 +50,6 @@ void shutdown() {
     data->flags->shutdown = true;
     return;
 }
-
 int main() {
 	/* Create the big data structures to house pod data */
 	
@@ -68,7 +67,6 @@ int main() {
             exit(0);
 	    }
 	    runStateMachine();
-
         if (data->flags->shouldBrake)
             signalLV((char *)"brake");
         usleep(10000);
diff --git a/embedded/app/main/badgerloop_LV.cpp b/embedded/app/main/badgerloop_LV.cpp
@@ -9,6 +9,7 @@
 extern "C"
 {
     #include "lv_iox.h"
+    #include "nav.h"
     #include "braking.h"
     #include "proc_iox.h"
 	#include "imu.h"
@@ -20,20 +21,23 @@ int init() {
     /* Init Data */
     initData();
 
+    initProcIox(true);
+    initLVIox(true);
 	/* Init all peripherals */
 	SetupIMU();
-    initLVIox(true);
-    initProcIox(true);
-
-	/* Init telemetry services */
-	SetupLVTelemetry((char *) DASHBOARD_IP, DASHBOARD_PORT);
+    initRetros();
+    initPressureMonitor();
+    initNav();
+    /* Init telemetry services */
+	SetupLVTelemetry((char *) DASHBOARD_IP, LV_TELEM_PORT);
 	SetupLVTCPServer();
 
     return 0;
 }
 
 int main() {
 
+
 	if (init() == 1) {
 		printf("Error in initialization! Exiting...\r\n");
 		exit(1);
diff --git a/embedded/app/src/state_machine.c b/embedded/app/src/state_machine.c
@@ -277,6 +277,7 @@ void runStateMachine(void) {
     }
     /* execute the state and check if we should be transitioning */
 	stateTransition_t *transition = stateMachine.currState->action();
+    printf("curr: %s\n", stateMachine.currState->name);
     if (transition != NULL) {
         if (transition->action() == 0) 
             stateMachine.currState = transition->target;
diff --git a/embedded/app/src/states.c b/embedded/app/src/states.c
@@ -53,25 +53,25 @@ stateTransition_t * idleAction() {
     
 
     // CHECK PRESSURE
-    if(!checkPrerunPressures()){
-        return stateMachine.currState->fault;
-    }
+/*    if(!checkPrerunPressures()){*/
+/*        return stateMachine.currState->fault;*/
+/*    }*/
     
     // CHECK STOPPED (MOTION)
-    if(!checkStopped()){
-        return stateMachine.currState->fault;
-    }
+/*    if(!checkStopped()){*/
+/*        return stateMachine.currState->fault;*/
+/*    }*/
     
     // TODO check LV Power
     // TODO check LV Temp
     
-    if(!checkPrerunBattery()){
-        return stateMachine.currState->fault;
-    }
+/*    if(!checkPrerunBattery()){*/
+/*        return stateMachine.currState->fault;*/
+/*    }*/
     
-    if(!checkPrerunRMS()){
-        return stateMachine.currState->fault;
-    }
+/*    if(!checkPrerunRMS()){*/
+/*        return stateMachine.currState->fault;*/
+/*    }*/
     
     // TRANSITION CRITERIA
     if(data->flags->readyPump){
diff --git a/embedded/data/include/data.h b/embedded/data/include/data.h
@@ -147,6 +147,7 @@ typedef struct bms_t {
     uint16_t relayStatus;
     uint8_t highTemp;
     uint8_t lowTemp;
+    uint8_t avgTemp;
     float cellMaxVoltage;
     float cellMinVoltage;
     uint16_t cellAvgVoltage;
diff --git a/embedded/examples/brakingTest.cpp b/embedded/examples/brakingTest.cpp
@@ -14,9 +14,12 @@ extern "C" {
 
 void showBrakingInfo() {
     int i = 0;
-    showPressures();
-    for (i = 0; i < NUM_LIM_SWITCHES; i++)
-        printf("Limit switch %d: %d\n", i, limSwitchGet(i));
+    pressure_t *p = data->pressure; 
+    //showPressures();
+    fprintf(stderr,"%f,%f,%f,%f,%f,%f\n", p->primTank, p->primLine, p->primAct,
+            p->secTank, p->secLine, p->secAct);
+    printf("%f,%f,%f,%f,%f,%f\n", p->primTank, p->primLine, p->primAct,
+            p->secTank, p->secLine, p->secAct);
 }
 
 
@@ -49,7 +52,8 @@ int main(int argc, char *argv[]) {
             }
         }
     }
-
+    fprintf(stderr, "primTank,primLine,primAct,secTank,secLine,secAct\n");
+    printf("primTank,primLine,primAct,secTank,secLine,secAct\n");
     FOREVER {
         showBrakingInfo();
         usleep(100000);
diff --git a/embedded/peripherals/include/bms.h b/embedded/peripherals/include/bms.h
@@ -6,5 +6,5 @@
 int bmsClearFaults();
 int bmsParseMsg(uint32_t id, uint8_t *msg);
 void bmsDump();
-
+void dumpCells();
 #endif
diff --git a/embedded/peripherals/src/batt.c b/embedded/peripherals/src/batt.c
@@ -2,8 +2,11 @@
 #include "NCD9830DBR2G.h"
 
 #define SCALE(x) ((x) * 2.96636085627)
-// - 2.5 / 2.5 * 50)
 
-/*double getLVBattVoltage() {*/
-/*    readPressureSensor(ADC_0)*/
-/*}*/
+double getLVBattVoltage() {
+    return 0;
+}
+
+double getLVCurrent() {
+    return 0;
+}
diff --git a/embedded/peripherals/src/bms.c b/embedded/peripherals/src/bms.c
@@ -4,8 +4,9 @@
 #include "bms.h"
 #include "can.h"
 #include "data.h"
-#define DEBUG_BMS
+
 extern data_t *data;
+float cells[72] = {0};
 
 int bmsClearFaults(void){
 
@@ -45,7 +46,7 @@ int bmsParseMsg(uint32_t id, uint8_t *msg) {
 			bms->packVoltage /= 10;
 			bms->Soc = msg[4]/2;
 			bms->relayStatus = msg[6] | msg[5] << 8;
-            bms->cellMaxVoltage = (msg[5] << 8) /10000.0;
+            bms->cellMaxVoltage = ((msg[5] << 8)| msg[6]) /10000.0;
 #ifdef DEBUG_BMS
 			printf("V: %f\r\n", bms->packVoltage);
 			printf("A: %f\r\n", bms->packCurrent);
@@ -56,15 +57,15 @@ int bmsParseMsg(uint32_t id, uint8_t *msg) {
 		case 0x6B1:
 			bms->packDCL = msg[1] | msg[0] << 8;
 			bms->highTemp = msg[4];
-			bms->cellMinVoltage = (msg[7] | (msg[6] << 8)) / 10000.0;
+/*			bms->cellMinVoltage = (msg[7] | (msg[6] << 8)) / 10000.0;*/
 #ifdef DEBUG_BMS
             printf("DCL: %d\r\n", bms->packDCL);
 			printf("High T: %d\r\n", bms->highTemp);
 			printf("Low T: %d\r\n", bms->lowTemp);
 #endif
 			break;
 		case 0x653:
-			printf("ID: 0x%3lx\r\n", (long unsigned int) id);
+/*			printf("ID: 0x%3lx\r\n", (long unsigned int) id);*/
 			bms->relayStatus = msg[1] | msg[0] << 8;
 			bms->relayStatus = msg[0];
 			bms->inputVoltage = msg[2] | (msg[3] << 8);
@@ -121,9 +122,9 @@ int bmsParseMsg(uint32_t id, uint8_t *msg) {
             break;
 		case 0x150:
 
-			bms->packCurrent = msg[0] | (msg[1] << 8);
+/*			bms->avgTemp = msg[0];*/
 			bms->packCurrent /= 10;
-            bms->cellMinVoltage = ((msg[3] << 8) | msg[4]) / 10000.0;
+/*            bms->cellMinVoltage = ((msg[3] << 8) | msg[4]) / 10000.0;*/
 /*			bms->packVoltage = msg[2] | (msg[3] << 8);*/
 /*			bms->packVoltage /= 10;*/
 			bms->packAh = msg[4] | (msg[5] << 8);
@@ -136,12 +137,23 @@ int bmsParseMsg(uint32_t id, uint8_t *msg) {
 			printf("High Temp  %d\r\n", bms->highTemp);
             printf("Low Temp  %d\r\n", bms->lowTemp);
 #endif
+            break;
+        case 0x6b2:
+            data->bms->cellMinVoltage = ((msg[0] << 8) | msg[1]) / 10000.0;
+/*            data->bms->cellMaxVoltage = ((msg[2] << 8) | msg[3]) / 10000;*/
+            bms->avgTemp = msg[2];
             break;
 		case 0x80:
 			break;
-		default:
+        case 0x36:
+            if (msg[0] >= 0 && msg[0] < 72)
+                cells[msg[0]] = (msg[2] | (msg[1] << 8)) / 10000.0;
+             
+        
+        default:
 			return 0;
-	}
+        
+    }
 	return 1;
 }
 
@@ -162,9 +174,21 @@ void bmsDump () {
     printf("\tRelay Status      = %u\n", bms->relayStatus);
     printf("\tHigh Temp         = %u\n", bms->highTemp);
     printf("\tLow Temp          = %u\n", bms->lowTemp);
+    printf("\tAvg Temp          = %d\n", bms->avgTemp);
     printf("\tCell Max Voltage  = %f\n", bms->cellMaxVoltage);
     printf("\tCell Min Voltage  = %f\n", bms->cellMinVoltage);
     printf("\tMax Cells         = %u\n", bms->maxCells);
     printf("\tNumber of Cells   = %u\n", bms->numCells);
     printf("---END BMS---\n");
 }
+float *getCellArray();
+float *getCellArray() {
+    return cells;
+}
+
+void dumpCells() {
+    printf("BATTERY CELLS: \n");
+    for (int i = 0; i < 72; i++) {
+        printf("CELL: %d : %f V\n", i , cells[i]);
+    }
+}
diff --git a/embedded/peripherals/src/braking.c b/embedded/peripherals/src/braking.c
@@ -5,7 +5,7 @@
 #include <lv_iox.h>
 #include <pthread.h>
 
-#define VOLTAGE_2000_SCALING(x) ( ((((x / 256.0) * 5.0) - 0.5) / 4.0) * 2000.0)
+#define VOLTAGE_2000_SCALING(x) ((((((x/256.0)*5.0)-0.5)/4.0)*2000.0))
 #define CURRENT_500_SCALING(x)  ( ((((x / 256.0) * 5.0) - 0.6) / 2.4) * 500.0)
 #define CURRENT_50_SCALING(x)   ( ((((x / 256.0) * 5.0) - 0.6) / 2.4) * 50.0)
 
@@ -44,11 +44,11 @@ void *pressureMonitor() {
 
     uint64_t i = 0;
     while(1) {
-        primTankRing[i % RING_SIZE] = readPrimaryTank();
+        primTankRing[i % RING_SIZE] = readPrimaryTank() + 10.76;
         primLineRing[i % RING_SIZE] = readPrimaryLine();
         primActRing[i % RING_SIZE]  = readPrimaryActuator();
 
-        secTankRing[i % RING_SIZE]  = readSecTank();
+        secTankRing[i % RING_SIZE]  = readSecTank() + 11.83;
         secLineRing[i % RING_SIZE]  = readSecLine();
         secActRing[i % RING_SIZE]   = readSecActuator();
 
diff --git a/embedded/peripherals/src/can_devices.c b/embedded/peripherals/src/can_devices.c
@@ -25,13 +25,14 @@ void rx_recv(struct can_frame *can_mesg){
 		//	printf("ID: %#X || ", (unsigned int) can_mesg->can_id);
 		//	printf("Data: [%#X.%#X.%#X.%#X.%#X.%#X.%#X.%#X]\n\r", can_mesg->data[0], can_mesg->data[1], can_mesg->data[2], can_mesg->data[3], can_mesg->data[4], can_mesg->data[5], can_mesg->data[6], can_mesg->data[7]);
 			bool validRMSMesg = false;
-/*			if(rms_parser(can_mesg->can_id, can_mesg->data, NO_FILTER)){*/
+			if(!rms_parser(can_mesg->can_id, can_mesg->data, NO_FILTER)){
 		//		printf("RMS Data parsed successfully\n");
-/*				validRMSMesg = true;*/
-/*			}*/
+				validRMSMesg = true;
+			}
 			if(!validRMSMesg && bmsParseMsg(can_mesg->can_id, can_mesg->data)){
-				printf("BMS Data parsed successfully\n");
-				bmsDump();
+/*				printf("BMS Data parsed successfully\n");*/
+/*				//dumpCells();*/
+/*                bmsDump();*/
 		}
 		NEW_CAN_MESSAGE = false;
 	}
diff --git a/embedded/peripherals/src/rms.c b/embedded/peripherals/src/rms.c
@@ -133,8 +133,9 @@ int rms_parser(uint32_t id, uint8_t *rmsData, uint32_t filter){
     if (filter != 0 && filter != id) {
         return 1;
     }
+    uint16_t val;
+    uint16_t val2;
 	switch(id){
-        case (0xc2):
            case (0xa0):
 			data->rms->igbtTemp = (rmsData[0] | (rmsData[1] <<8)) / 10; //Deg C
 			data->rms->gateDriverBoardTemp = (rmsData[6] | (rmsData[7] << 8)) / 10; //Deg C
@@ -150,7 +151,8 @@ int rms_parser(uint32_t id, uint8_t *rmsData, uint32_t filter){
 #endif
             break;
 		case (0xa2):
-			data->rms->motorTemp = (rmsData[4] | (rmsData[5] << 8)) / 10; //Deg C
+            val = (rmsData[4] | rmsData[5] << 8) / 10;
+			data->rms->motorTemp = val > 300 ? data->rms->motorTemp : val; //Deg C
 #ifdef DEBUG_RMS
             printf("Motor Temp: %d\r\n", data->rms->motorTemp);
 #endif
@@ -160,25 +162,31 @@ int rms_parser(uint32_t id, uint8_t *rmsData, uint32_t filter){
 		case (0xa4):
 			break;
 		case (0xa5):
-			data->rms->motorSpeed = (rmsData[2] | (rmsData[3] << 8)); // RPM
+            val = (rmsData[2] | (rmsData[3] << 8));
+			data->rms->motorSpeed = val < -10000 || val > 10000 ? data->rms->motorSpeed: val; // RPM
 			data->rms->electricalFreq = (rmsData[4] | (rmsData[5] << 8 )) / 10; //electrical frequency Hz
 #ifdef DEBUG_RMS
             printf("Motor Speed: %d\r\n", data->rms->motorSpeed);
 			printf("Elect. Freq: %d\r\n", data->rms->electricalFreq);
 #endif
             break;
 		case (0xa6):
-			data->rms->phaseACurrent = (rmsData[0] | (rmsData[1] << 8)) / 10; // Phase A current
-			data->rms->dcBusCurrent = (rmsData[6] | (rmsData[7] << 8)) / 10; //DC Bus current
+            val = (rmsData[0] | (rmsData[1] << 8)) / 10;
+            val2 = (rmsData[6] | (rmsData[7] << 8)) / 10;
+			data->rms->phaseACurrent = val > 1000 ? data->rms->phaseACurrent : val; // Phase A current
+			data->rms->dcBusCurrent = val2 < 0 ? data->rms->dcBusCurrent : val2; //DC Bus current
 #ifdef DEBUG_RMS
             printf("Phase A Current: %d\r\n", data->rms->phaseACurrent);
 			printf("DC Bus Current: %d\r\n", data->rms->dcBusCurrent);
             printf("Phase B Current: %d\r\n", data->rms->phaseBCurrent); //FIXME This isnt actually being read in?
 #endif
             break;
 		case (0xa7):
-			data->rms->dcBusVoltage = (rmsData[0] | (rmsData[1] << 8))/10; //DC Bus voltage
-			data->rms->outputVoltageLn = (rmsData[2] | (rmsData[3] << 8)) / 10; //Voltage line to netural 
+
+            val =  (rmsData[0] | (rmsData[1] << 8))/10;
+            val2 = (rmsData[2] | (rmsData[3] << 8)) / 10;
+			data->rms->dcBusVoltage = val > 1000 ? data->rms->dcBusVoltage : val;//DC Bus voltage
+			data->rms->outputVoltageLn = val2; //Voltage line to netural 
 #ifdef DEBUG_RMS
             printf("DC Bus Voltage: %d\r\n", data->rms->dcBusVoltage);
 			printf("Output Voltage line: %d\r\n", data->rms->outputVoltageLn);
@@ -210,8 +218,10 @@ int rms_parser(uint32_t id, uint8_t *rmsData, uint32_t filter){
 #endif
             break;
 		case (0xac):
-			data->rms->commandedTorque = (rmsData[0] | (rmsData[1] << 8)) /10;
-			data->rms->actualTorque = (rmsData[2] | (rmsData[3] << 8)) / 10;
+            val = (rmsData[0] | (rmsData[1] << 8)) /10;
+            val2 = (rmsData[2] | (rmsData[3] << 8)) / 10;
+			data->rms->commandedTorque = val > 200 ? data->rms->commandedTorque : val; 
+			data->rms->actualTorque = val2 > 200 ? data->rms->actualTorque : val2;
 #ifdef DEBUG_RMS
             printf("Commanded Torque: %d\r\n", data->rms->commandedTorque);
 			printf("Actual Torque: %d\r\n", data->rms->actualTorque);
diff --git a/middleware/include/connStat.h b/middleware/include/connStat.h
diff --git a/middleware/src/HVTelemetry_Loop.cpp b/middleware/src/HVTelemetry_Loop.cpp

Original file line number	Diff line number	Diff line change
`@@ -277,6 +277,7 @@ void runStateMachine(void) {`
`277`	`277`	`}`
`278`	`278`	`/* execute the state and check if we should be transitioning */`
`279`	`279`	`stateTransition_t *transition = stateMachine.currState->action();`
	`280`	`+ printf("curr: %s\n", stateMachine.currState->name);`
`280`	`281`	`if (transition != NULL) {`
`281`	`282`	`if (transition->action() == 0)`
`282`	`283`	`stateMachine.currState = transition->target;`
Original file line number	Diff line number	Diff line change
`@@ -14,9 +14,12 @@ extern "C" {`
`14`	`14`
`15`	`15`	`void showBrakingInfo() {`
`16`	`16`	`int i = 0;`
`17`		`- showPressures();`
`18`		`- for (i = 0; i < NUM_LIM_SWITCHES; i++)`
`19`		`- printf("Limit switch %d: %d\n", i, limSwitchGet(i));`
	`17`	`+ pressure_t *p = data->pressure;`
	`18`	`+ //showPressures();`
	`19`	`+ fprintf(stderr,"%f,%f,%f,%f,%f,%f\n", p->primTank, p->primLine, p->primAct,`
	`20`	`+ p->secTank, p->secLine, p->secAct);`
	`21`	`+ printf("%f,%f,%f,%f,%f,%f\n", p->primTank, p->primLine, p->primAct,`
	`22`	`+ p->secTank, p->secLine, p->secAct);`
`20`	`23`	`}`
`21`	`24`
`22`	`25`
`@@ -49,7 +52,8 @@ int main(int argc, char *argv[]) {`
`49`	`52`	`}`
`50`	`53`	`}`
`51`	`54`	`}`
`52`		`-`
	`55`	`+ fprintf(stderr, "primTank,primLine,primAct,secTank,secLine,secAct\n");`
	`56`	`+ printf("primTank,primLine,primAct,secTank,secLine,secAct\n");`
`53`	`57`	`FOREVER {`
`54`	`58`	`showBrakingInfo();`
`55`	`59`	`usleep(100000);`