Implementing Rocket Commanding

cornellrocketryteam · Nov 17, 2024 · d22554b · d22554b
1 parent 5288e38
commit d22554b
Show file tree

Hide file tree

Showing 5 changed files with 93 additions and 24 deletions.
diff --git a/fill/fill_station.cc b/fill/fill_station.cc
@@ -107,15 +107,9 @@ class CommanderServiceImpl final : public Commander::Service
                 sv1.close();
             }
         }
-        if (request->sv2_close()){
-            // send TCP command to rocket_controller 
-        }
-        if (request->mav_open()){
-            // send TCP command to rocket_controller 
-        }
-        if (request->fire()){
-            // send TCP command to rocket_controller 
-        }
+
+        protoBuild.sendCommand(request);
+
         return Status::OK;
     }
 };
@@ -147,6 +141,7 @@ class RocketTelemeterServiceImpl final : public RocketTelemeter::Service
         while (true) {
             auto now = std::chrono::high_resolution_clock::now();
             RocketTelemetry t = protoBuild.buildProto();
+            printf("getting here");
             if (!writer->Write(t)) {
                 // Broken stream
                 return Status::CANCELLED; 

diff --git a/fill/umbilical/proto_build.cc b/fill/umbilical/proto_build.cc
@@ -1,11 +1,33 @@
 #include "proto_build.h"
+#include <iostream>
 
-RocketTelemetryProtoBuilder::RocketTelemetryProtoBuilder(): serial_data(usb_port, std::ios::binary){}
+RocketTelemetryProtoBuilder::RocketTelemetryProtoBuilder(): serial_data(usb_port, std::ios::in | std::ios::out | std::ios::binary){}
 
 RocketTelemetryProtoBuilder::~RocketTelemetryProtoBuilder(){
     serial_data.close();
 }
 
+void RocketTelemetryProtoBuilder::sendCommand(const Command* com) {
+    if (com->sv2_open()){
+        serial_data << (uint8_t)OPEN_SV;
+    }
+    if (com->sv2_close()) {
+        serial_data << (uint8_t)CLOSE_SV; 
+    }
+    if (com->mav_open()){
+        serial_data << (uint8_t)OPEN_MAV; 
+    }
+    if (com->mav_close()){
+        serial_data << (uint8_t)CLOSE_MAV;
+    }
+    if (com->launch()){
+        serial_data << (uint8_t)LAUNCH; 
+    }
+    if (com->clear_sd()){
+        serial_data << (uint8_t)CLEAR_SD;
+    }
+}
+
 RocketTelemetry RocketTelemetryProtoBuilder::buildProto(){
     RocketTelemetry rocketTelemetry; 
 
@@ -27,17 +49,54 @@ RocketTelemetry RocketTelemetryProtoBuilder::buildProto(){
         float pt4; 
         float temp;
 
-        serial_data.read(reinterpret_cast<char*>(&metadata), sizeof(metadata));
-        serial_data.read(reinterpret_cast<char*>(&ms_since_boot), sizeof(ms_since_boot));
-        serial_data.read(reinterpret_cast<char*>(&events_val), sizeof(events_val));
+        /*
+        OLD VERSION THAT DID NOT ACCOUNT FOR \n AFTER EVERY PACKET
+        */
+
+        // serial_data.read(reinterpret_cast<char*>(&metadata), sizeof(metadata));
+        // serial_data.read(reinterpret_cast<char*>(&ms_since_boot), sizeof(ms_since_boot));
+        // serial_data.read(reinterpret_cast<char*>(&events_val), sizeof(events_val));
+
+        // serial_data.read(reinterpret_cast<char*>(&radio_state), sizeof(radio_state));
+        // serial_data.read(reinterpret_cast<char*>(&transmit_state), sizeof(transmit_state));
+
+        // serial_data.read(reinterpret_cast<char*>(&voltage), sizeof(voltage));
+        // serial_data.read(reinterpret_cast<char*>(&pt3), sizeof(pt3));
+        // serial_data.read(reinterpret_cast<char*>(&pt4), sizeof(pt4));
+        // serial_data.read(reinterpret_cast<char*>(&temp), sizeof(temp));
+
+        char packet[28]; 
+        serial_data.getline(packet, UMB_PACKET_SIZE);
+
+        memcpy(&metadata, packet, sizeof(metadata));
+        memcpy(&ms_since_boot, packet + 2, sizeof(ms_since_boot));
+        memcpy(&events_val, packet + 6, sizeof(events_val));
+
+        memcpy(&radio_state, packet + 10, sizeof(radio_state));
+        memcpy(&transmit_state, packet + 11, sizeof(transmit_state));
+
+        memcpy(&voltage, packet + 12, sizeof(voltage));
+        memcpy(&pt3, packet + 16, sizeof(pt3));
+        memcpy(&pt4, packet + 20, sizeof(pt4));
+        memcpy(&temp, packet + 24, sizeof(temp));
+
+        /* 
+        VERBOSE FOR TESTING 
+        */
+        printf("Metadata value: %d\n", metadata); 
+        printf("Ms-since-boot: %d\n", ms_since_boot);
+        printf("events: %llu\n", events_val);
 
-        serial_data.read(reinterpret_cast<char*>(&radio_state), sizeof(radio_state));
-        serial_data.read(reinterpret_cast<char*>(&transmit_state), sizeof(transmit_state));
+        printf("Radio_state: %d\n", radio_state); 
+        printf("Transmit_state: %d\n", transmit_state);;
 
-        serial_data.read(reinterpret_cast<char*>(&voltage), sizeof(voltage));
-        serial_data.read(reinterpret_cast<char*>(&pt3), sizeof(pt3));
-        serial_data.read(reinterpret_cast<char*>(&pt4), sizeof(pt4));
-        serial_data.read(reinterpret_cast<char*>(&temp), sizeof(temp));
+        printf("Voltage: %f\n", voltage);
+        printf("Pt3: %f\n", pt3);
+        printf("Pt4: %f\n", pt4);
+        printf("Temp: %f\n", temp);
+        /* 
+        END VERBOSE 
+        */
 
         rocketMetadata->set_alt_armed(static_cast<bool>(metadata & 0x1)); 
         rocketMetadata->set_alt_valid(static_cast<bool>((metadata & 0x2) >> 1)); 

diff --git a/fill/umbilical/proto_build.h b/fill/umbilical/proto_build.h
@@ -1,6 +1,15 @@
 #ifndef PROTO_BUILD_H
 #define PROTO_BUILD_H
 
+#define LAUNCH 0
+#define OPEN_MAV 1
+#define CLOSE_MAV 2
+#define OPEN_SV 3
+#define CLOSE_SV 4
+#define CLEAR_SD 5
+
+#define UMB_PACKET_SIZE 28
+
 #include <fstream>
 #include "protos/command.grpc.pb.h"
 #include <iostream>
@@ -10,17 +19,20 @@ using command::RocketMetadata;
 using command::Events;
 using command::FlightMode; 
 using command::RocketUmbTelemetry;
+using command::Command; 
 
 class RocketTelemetryProtoBuilder {
     private:
         const char* usb_port = "/dev/ttyACM0";
 
-        std::ifstream serial_data;
+        std::fstream serial_data;
     public: 
         // Establish serial connection 
         RocketTelemetryProtoBuilder();
         ~RocketTelemetryProtoBuilder();
 
+        void sendCommand(const Command* com); 
+
         RocketTelemetry buildProto(); 
 };
 

diff --git a/go-proxies/proxy_build.sh b/go-proxies/proxy_build.sh
@@ -18,4 +18,4 @@ pushd ..
 docker build -t ghcr.io/cornellrocketryteam/fill-telem-proxy:latest -f go-proxies/fill-telem-proxy/Dockerfile . &
 docker build -t ghcr.io/cornellrocketryteam/rocket-telem-proxy:latest -f go-proxies/rocket-telem-proxy/Dockerfile . &
 docker build -t ghcr.io/cornellrocketryteam/websocket-proxy:latest -f go-proxies/websocket-proxy/Dockerfile .
-popd
+popd
diff --git a/protos/command.proto b/protos/command.proto
@@ -35,9 +35,12 @@ message Command {
     optional bool bv1_off = 3;
     optional bool qd_retract = 4;
     optional bool ignite = 5;
-    optional bool sv2_close = 6;
-    optional bool mav_open = 7;
-    optional bool fire = 8;
+    optional bool sv2_open = 6;
+    optional bool sv2_close = 7;
+    optional bool mav_open = 8;
+    optional bool mav_close = 9;
+    optional bool launch = 10;
+    optional bool clear_sd = 11;
 }
 
 // The response message containing an ack