Added pressure data to the telem packet

Ezra Boley · Ezra Boley · commit 6a0bd5059ed6 · 2019-07-15T13:43:16.000-05:00
diff --git a/Makefile b/Makefile
@@ -23,9 +23,10 @@ ifdef LOCAL
 NOI2C := NOI2C
 endif
 
-USER   := "debian"
-LV_IP  := "192.168.7.2"
-HV_IP  := "192.168.7.3"
+LV_USER   := "debian"
+HV_USER	  := "ezra"
+LV_IP  := "192.168.1.106"
+HV_IP  := "192.168.1.146"
 
 ifdef BB
 BEAGLE := ${BBCC}
@@ -71,8 +72,8 @@ examples: $(EXAMPLES)
 utils: $(UTILS)
 
 copy:
-	-scp -q -o ConnectTimeout=2 -r $(OUTPUT_DIR) $(USER)@$(LV_IP):~/bin &
-	-scp -q -o ConnectTimeout=2 -r $(OUTPUT_DIR) $(USER)@$(HV_IP):~/bin &
+	-scp -q -o ConnectTimeout=2 -r $(OUTPUT_DIR) $(LV_USER)@$(LV_IP):~/bin &
+	-scp -q -o ConnectTimeout=2 -r $(OUTPUT_DIR) $(HV_USER)@$(HV_IP):~/bin &
 
 $(OUTPUT_DIR)/%: $(GEN_OBJ) $(OBJ_DIR)/%.o
 	$(GPP) $(LDFLAGS) $^ $(LDLIBS) -o $@
diff --git a/embedded/app/include/state_machine.h b/embedded/app/include/state_machine.h
@@ -19,7 +19,7 @@
 #define PROPULSION_NAME         "propulsion"
 #define BRAKING_NAME            "braking"
 #define STOPPED_NAME            "stopped"
-#define CRAWL_NAME              "crawl"
+#define CRAWL_NAME              "servicePropulsion"
 #define POST_RUN_NAME           "postRun"
 #define SAFE_TO_APPROACH_NAME   "safeToApproach"
 
diff --git a/embedded/app/src/motor.c b/embedded/app/src/motor.c
@@ -146,7 +146,6 @@ static void *motorHeartbeatLoop(void *unusedParam) {
                 fprintf(stderr, "Failed to send heartbeat\n");
             }
         } else {
-			printf("IDLE\n");
             if (rmsIdleHb() != 0) {
                 fprintf(stderr, "Failed to send idle heartbeat\n");
             }
diff --git a/embedded/app/src/nav.c b/embedded/app/src/nav.c
@@ -171,7 +171,6 @@ void navLoop(void *unused) {
         }
     
         if ((getuSTimestamp() - data->timers->startTime) > TOTAL_RUN_TIME) {
-            printf("Hit time limit\n");
             data->flags->shouldStop = true;
         }
 
diff --git a/embedded/examples/bmsDisplay.cpp b/embedded/examples/bmsDisplay.cpp
@@ -0,0 +1,19 @@
+#include <stdio.h>
+#include <connStat.h>
+#include <pthread.h>
+#include "HVTelemetry_Loop.h"
+
+extern "C" {
+    #include "data.h"
+    #include "imu.h"
+    #include "can_devices.h"
+    #include "bms.h"
+    extern pthread_t CANThread;
+}
+
+int main() {
+    initData();
+    SetupCANDevices();
+    SetupHVTelemetry((char *) DASHBOARD_IP, DASHBOARD_PORT);
+    pthread_join(CANThread, NULL);
+}
diff --git a/embedded/examples/brakingTest.cpp b/embedded/examples/brakingTest.cpp
@@ -1,11 +1,14 @@
 #include <stdio.h>
 #include <unistd.h>
+#include "LVTelemetry_Loop.h"
 #include <string.h>
+#include "connStat.h"
+extern "C" {
 #include "braking.h"
 #include "lv_iox.h"
 #include "data.h"
-
-#define NUM_LIM_SWITCHES 4  /* In test */
+}
+#define NUM_LIM_SWITCHES 2  /* In test */
 #define FOREVER while(1)
 
 void showBrakingInfo() {
@@ -24,7 +27,7 @@ int main(int argc, char *argv[]) {
     initData();
     initPressureMonitor();
     initLVIox(isHard);
-
+    SetupLVTelemetry((char *) DASHBOARD_IP, DASHBOARD_PORT);
     if (argc > 1) {
         if (strcmp(argv[1], "-p") == 0) {
             if (argc > 2) {
diff --git a/embedded/examples/navTest.cpp b/embedded/examples/navTest.cpp
@@ -22,9 +22,9 @@ int main(int argc, char *argv[])
   initRetros();
   initNav();
   
+  SetupLVTelemetry((char *)"192.168.1.120", 33333);
   joinRetroThreads();
 
 
-  SetupLVTelemetry((char *)"127.0.0.1", 33333);
   while(1);
 }
diff --git a/embedded/peripherals/src/batt.c b/embedded/peripherals/src/batt.c
@@ -2,7 +2,7 @@
 #include "NCD9830DBR2G.h"
 
 #define SCALE(x) ((x) * 2.96636085627)
-
+// - 2.5 / 2.5 * 50)
 
 /*double getLVBattVoltage() {*/
 /*    readPressureSensor(ADC_0)*/
diff --git a/embedded/peripherals/src/can_devices.c b/embedded/peripherals/src/can_devices.c
@@ -30,8 +30,8 @@ void rx_recv(struct can_frame *can_mesg){
 				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");
+				bmsDump();
 		}
 		NEW_CAN_MESSAGE = false;
 	}
diff --git a/embedded/peripherals/src/imu.c b/embedded/peripherals/src/imu.c
@@ -75,12 +75,6 @@ void *IMULoop(void *arg){
 		write_byte_i2c(i2c, DATA_REG);
 		unsigned char dataBuffer[messageSize];
 		read_i2c(i2c, dataBuffer, messageSize);
-        printf("message size\n");
-        printf("message:\n");
-        for (i = 0; i < messageSize; i++) {
-            printf("%#x ", dataBuffer[i]);
-        }
-        printf("\n");
         i = 0;
 		while(i < messageSize){
 			//Check delta velocity
@@ -121,7 +115,7 @@ void *IMULoop(void *arg){
                 data->velZ = data->velZ + (data->accelZ * 0.01);
                 
                 
-                data->posX += (data->velX * 0.01);
+/*                data->posX += (data->velX * 0.01);*/
                 
                 
                 sem_post(&data->mutex);
diff --git a/embedded/peripherals/src/retro.c b/embedded/peripherals/src/retro.c
@@ -23,8 +23,8 @@
 #define BUF_LEN 256
 #define SAFETY_CONSTANT 2
 #define SEC_TO_USEC     1000000
-#define VOTE_BUFFER     200000
-#define VOTE_RESET_TIME 3000000  /* Hard coded for max speed, approx algo should be (DIST_BTWN_STRIPS / (2 * SPEED_FTPS)) */
+#define VOTE_BUFFER     500000
+#define VOTE_RESET_TIME 30000  /* Hard coded for max speed, approx algo should be (DIST_BTWN_STRIPS / (2 * SPEED_FTPS)) */
 #define CONST_TERM      SAFETY_CONSTANT * SEC_TO_USEC
 
 static pthread_t retroThreads[3];
@@ -67,7 +67,7 @@ int initRetros() {
 ***/
 int joinRetroThreads() {
 	int i = 0;
-	shouldQuit = true;
+/*	shouldQuit = true;*/
 	for (i = 0; i < NUM_RETROS; i++) {
 		pthread_join(retroThreads[i], NULL);
 	}
@@ -105,7 +105,7 @@ static int voteOnCandidate(int retroNum) {
 static int onTapeStrip(int retroNum) {
     DBG_RETRO_PRINTF("Tape strip detected on retro %d\n", retroNum);
 	uint64_t currTime = getuSTimestamp();
-    uint64_t delay = getDelay();
+    uint64_t delay = 1000;//getDelay();
     DBG_RETRO_PRINTF("Current delay: %llu\n", delay);
 
 
diff --git a/middleware/include/connStat.h b/middleware/include/connStat.h
@@ -1,7 +1,7 @@
 #ifndef __CONNSTAT_H__
 #define __CONNSTAT_H__
 
-#define DASHBOARD_IP "192.168.1.145"
+#define DASHBOARD_IP "192.168.1.120"
 #define DASHBOARD_PORT 33333
 
 #define LV_SERVER_IP "192.168.1.146"
diff --git a/middleware/src/LVTelemetry_Loop.cpp b/middleware/src/LVTelemetry_Loop.cpp
@@ -10,10 +10,12 @@
 #include "document.h"
 #include "writer.h"
 #include "connStat.h"
+#include "data.h"
 
 /* ADD SENSOR INCLUDES HERE */
 extern "C" 
 {
+    #include "lv_iox.h"
 	#include "imu.h"
 }
 
@@ -76,20 +78,20 @@ void *LVTelemetryLoop(void *arg)
 			
 			// POSITION
 			Value pos;
-			pos.SetNull();
+			pos.SetFloat(data->motion->pos);
 			
 			// RETRO
 			Value retro;
-			retro.SetNull();
+			retro.SetInt(data->motion->retroCount);
 			
 			// VELOCITY - Change "X" to "Y" if need be
 			Value vel;
-			// vel.SetFloat(getDeltaVX());
+			vel.SetFloat(data->motion->vel);
 	
 			
 			// ACCELERATION - Change "X" to "Y" if need be
 			Value accel;
-			// accel.SetFloat(getAccelX());
+			accel.SetFloat(getAccelX());
 				
 			// HIGH TEMP
 			Value tempH;
@@ -101,12 +103,28 @@ void *LVTelemetryLoop(void *arg)
 						
 			// PRESSURE VESSEL PRESSURE
 			Value pressureV;
-			pressureV.SetNull();
-			
+			pressureV.SetDouble(data->pressure->pv);
+			
+            Value primTank, primLine, primAct;
+            primTank.SetFloat(data->pressure->primTank);
+            primLine.SetFloat(data->pressure->primLine);
+            primAct.SetFloat(data->pressure->primAct);
+            
+            Value secTank, secLine, secAct;
+            secTank.SetFloat(data->pressure->secTank);
+            secLine.SetFloat(data->pressure->secLine);
+            secAct.SetFloat(data->pressure->secAct);
+ 
 			// CURRENT PRESSURE
 			Value currP;
 			currP.SetNull();
 			
+            Value primBrake, secBrake;
+            primBrake.SetBool(limSwitchGet(PRIM_LIM_SWITCH));
+            secBrake.SetBool(limSwitchGet(SEC_LIM_SWITCH));
+
+            Value imuPos;
+            imuPos.SetFloat(getPosX());
 			
 			/* INSERT VALUES INTO JSON DOCUMENTS */
 			
@@ -119,7 +137,8 @@ void *LVTelemetryLoop(void *arg)
 			motionDoc.AddMember("retro", retro, motionDoc.GetAllocator());
 			motionDoc.AddMember("velocity", vel, motionDoc.GetAllocator());
 			motionDoc.AddMember("acceleration", accel, motionDoc.GetAllocator());
-			
+			motionDoc.AddMember("distanceSinceLastRetro", imuPos, motionDoc.GetAllocator());
+
 			Document batteryDoc;
 			batteryDoc.SetObject();
 			batteryDoc.AddMember("highTemp", tempH, batteryDoc.GetAllocator());
@@ -129,7 +148,15 @@ void *LVTelemetryLoop(void *arg)
 			brakingDoc.SetObject();
 			brakingDoc.AddMember("pressureVesselPressure", pressureV, brakingDoc.GetAllocator());
 			brakingDoc.AddMember("currentPressure", currP, brakingDoc.GetAllocator());
-			
+			brakingDoc.AddMember("primBrake", primBrake, brakingDoc.GetAllocator());
+            brakingDoc.AddMember("secBrake", secBrake, brakingDoc.GetAllocator());
+            brakingDoc.AddMember("primaryTank", primTank, brakingDoc.GetAllocator()); 
+            brakingDoc.AddMember("primaryLine", primLine, brakingDoc.GetAllocator()); 
+            brakingDoc.AddMember("primaryActuation", primAct, brakingDoc.GetAllocator()); 
+            brakingDoc.AddMember("secondaryTank", secTank, brakingDoc.GetAllocator()); 
+            brakingDoc.AddMember("secondaryLine", secLine, brakingDoc.GetAllocator()); 
+            brakingDoc.AddMember("secondaryActuation", secAct, brakingDoc.GetAllocator()); 
+
 			/* ADD DOCUMENTS TO MAIN JSON DOCUMENT */
 			
 			document.AddMember("time", age, document.GetAllocator());

Original file line number	Diff line number	Diff line change
`@@ -146,7 +146,6 @@ static void motorHeartbeatLoop(void unusedParam) {`
`146`	`146`	`fprintf(stderr, "Failed to send heartbeat\n");`
`147`	`147`	`}`
`148`	`148`	`} else {`
`149`		`- printf("IDLE\n");`
`150`	`149`	`if (rmsIdleHb() != 0) {`
`151`	`150`	`fprintf(stderr, "Failed to send idle heartbeat\n");`
`152`	`151`	`}`
Original file line number	Diff line number	Diff line change
`@@ -171,7 +171,6 @@ void navLoop(void *unused) {`
`171`	`171`	`}`
`172`	`172`
`173`	`173`	`if ((getuSTimestamp() - data->timers->startTime) > TOTAL_RUN_TIME) {`
`174`		`- printf("Hit time limit\n");`
`175`	`174`	`data->flags->shouldStop = true;`
`176`	`175`	`}`
`177`	`176`
Original file line number	Diff line number	Diff line change
`@@ -22,9 +22,9 @@ int main(int argc, char *argv[])`
`22`	`22`	`initRetros();`
`23`	`23`	`initNav();`
`24`	`24`
	`25`	`+ SetupLVTelemetry((char *)"192.168.1.120", 33333);`
`25`	`26`	`joinRetroThreads();`
`26`	`27`
`27`	`28`
`28`		`- SetupLVTelemetry((char *)"127.0.0.1", 33333);`
`29`	`29`	`while(1);`
`30`	`30`	`}`
Original file line number	Diff line number	Diff line change
`@@ -30,8 +30,8 @@ void rx_recv(struct can_frame *can_mesg){`
`30`	`30`	`validRMSMesg = true;`
`31`	`31`	`}`
`32`	`32`	`if(!validRMSMesg && bmsParseMsg(can_mesg->can_id, can_mesg->data)){`
`33`		`- // printf("BMS Data parsed successfully\n");`
`34`		`- // bmsDump();`
	`33`	`+ printf("BMS Data parsed successfully\n");`
	`34`	`+ bmsDump();`
`35`	`35`	`}`
`36`	`36`	`NEW_CAN_MESSAGE = false;`
`37`	`37`	`}`