Implementing Rocket Commanding

.github/workflows/fill_station_action.yml

@@ -44,9 +44,9 @@ jobs:
             bazel build //fill/lib/MockWiringPi:mock_wiringpi
             ./fill/setup_mock_wiringpi.sh
             bazel build //fill:all
-            ./fill/test/bv_test.sh
-            ./fill/test/qd_test.sh
-            ./fill/test/sv1_test.sh
+            #  ./fill/test/bv_test.sh
+            # ./fill/test/qd_test.sh
+            # ./fill/test/sv1_test.sh
           elif [[ "${{ matrix.arch }}" == "arm64" ]]; then
             sudo apt-get install g++-11-arm-linux-gnueabihf
             sudo apt install gcc-11-arm-linux-gnueabihf

fill/Dockerfile

@@ -14,7 +14,7 @@ COPY . .
 RUN bazelisk build //fill:all
 
 # Stage 2: Create lightweight runtime image
-FROM ubuntu
+FROM ubuntu:22.04
 
 # Set the working directory
 WORKDIR /app

fill/actuators/BUILD

@@ -2,6 +2,7 @@ cc_library(
     name = "actuator",
     hdrs = ["actuator.h"],
     deps = [],
+    includes = ["/usr/local/lib"],
     visibility = [
         "//fill:__subpackages__",
         ],

fill/actuators/sol_valve.cc

@@ -11,8 +11,6 @@ SolValve::SolValve(){
 
 SolValve::~SolValve(){}
 
-
-
 void SolValve::open(){
     pinMode(SV_SIGNAL, OUTPUT);
     digitalWrite(SV_SIGNAL, HIGH);
@@ -37,6 +35,4 @@ void SolValve::close(){
     pinMode(SV_SIGNAL, OUTPUT);
     digitalWrite(SV_SIGNAL, LOW);
     isOpen=false;
-}
-
-
+}

fill/fill_station.cc

@@ -121,15 +121,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;
     }
 };

fill/umbilical/proto_build.cc

@@ -1,19 +1,149 @@
 #include "proto_build.h"
+#include <iostream>
+#include <unistd.h>
 
-RocketTelemetryProtoBuilder::RocketTelemetryProtoBuilder(): serial_data(usb_port, std::ios::binary){}
+bool RocketTelemetryProtoBuilder::is_fd_open(int fd) {
+    int flags = fcntl(fd, F_GETFL);
+    return (flags != -1 || errno != EBADF);
+}
+
+ssize_t RocketTelemetryProtoBuilder::read_packet(int fd, char* packet, size_t max_size) {
+    size_t index = 0;  // Current position in the packet
+    char byte;         // Single byte buffer
+    ssize_t bytesRead;
+
+    while (index < max_size) {  
+        bytesRead = read(fd, &byte, 1);  // Read one byte at a time
+
+        if (bytesRead < 0) {
+            return -1;
+        }
+
+        if (bytesRead == 0) {
+            // End of file reached
+            break;
+        }
+
+        // Append the byte to the packet
+        packet[index++] = byte;
+
+        // Stop if newline is found
+        if (byte == '\n') {
+            break;
+        }
+    }
+
+    return index;  // Return the number of bytes read
+}
+
+void RocketTelemetryProtoBuilder::openfile(){
+    if ((serial_data = open ("/dev/rocket", O_RDWR | O_NOCTTY)) == -1) {
+        std::cout << "Error opening /dev/rocket." << std::endl;
+    }
+
+    fcntl (serial_data, F_SETFL, O_RDWR) ;
+
+    struct termios tty;
+    memset(&tty, 0, sizeof(tty));
+
+    if (tcgetattr(serial_data, &tty) != 0) {
+        std::cerr << "Error getting termios attributes." << std::endl;
+        close(serial_data);
+    }
+
+    cfsetospeed(&tty, B9600); // Baud Rate
+    cfsetispeed(&tty, B9600);
+
+    tty.c_cflag &= ~PARENB;
+    tty.c_cflag &= ~CSTOPB;
+    tty.c_cflag &= ~CSIZE;
+    tty.c_cflag |= CS8;
+    tty.c_cflag &= ~CRTSCTS;
+    tty.c_cflag |= CREAD | CLOCAL;
+
+    tty.c_lflag &= ~ICANON;
+    tty.c_lflag &= ~ECHO;
+    tty.c_lflag &= ~ECHOE;
+    tty.c_lflag &= ~ECHONL;
+    tty.c_lflag &= ~ISIG;
+
+    tty.c_iflag &= ~(IXON | IXOFF | IXANY);
+    tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL);
+
+    tty.c_oflag &= ~OPOST;
+
+    tty.c_cc[VMIN] = 0;
+    tty.c_cc[VTIME] = 10;
+
+    if (tcsetattr(serial_data, TCSANOW, &tty) != 0) {
+        std::cerr << "Error setting termios attributes." << std::endl;
+        //close(serial_data);
+    }
+
+    usleep(10); 
+
+}
+
+RocketTelemetryProtoBuilder::RocketTelemetryProtoBuilder(){
+    openfile();
+}
 
 RocketTelemetryProtoBuilder::~RocketTelemetryProtoBuilder(){
-    serial_data.close();
+    close(serial_data);
+}
+
+void RocketTelemetryProtoBuilder::write_command(char com){
+    write(serial_data, (const void *)com, 1);
+}
+
+void RocketTelemetryProtoBuilder::sendCommand(const Command* com) {
+    if (com->has_sv2_close()){
+        if (com->sv2_close()) {
+            // TODO: Check if a character or a value at a memory address is being sent 
+            // For testing sake, define the commands as constants 
+            pid_t f = fork(); 
+            if (f == 0){
+                write_command(CLOSE_SV);
+                exit(1);
+            }
+        } else {
+            pid_t f = fork(); 
+            if (f == 0){
+                write_command(OPEN_SV);
+                exit(1);
+            }
+        }
+    }
+
+    if (com->has_mav_open()){
+        if (com->mav_open()){
+            pid_t f = fork(); 
+            if (f==0){
+                write_command(OPEN_MAV);
+            }
+            exit(1);
+        } else {     
+            pid_t f = fork(); 
+            if (f==0){
+                write_command(CLOSE_MAV);
+            }
+            exit(1);
+        }
+    }
+
+    // if (com->launch()){
+    //     write(serial_data, (const void *)LAUNCH, 1);
+    // }
+
+    // TODO: Implement com->clear_sd() case when ground server is done implementing it     
 }
 
 RocketTelemetry RocketTelemetryProtoBuilder::buildProto(){
     RocketTelemetry rocketTelemetry; 
-
-    if (!serial_data){
+     if (is_fd_open(serial_data)){
         RocketUmbTelemetry* rocketUmbTelemetry = rocketTelemetry.mutable_umb_telem();
         RocketMetadata* rocketMetadata = rocketUmbTelemetry->mutable_metadata();
         Events* events = rocketUmbTelemetry->mutable_events();
-
 
         uint16_t metadata;
         uint32_t ms_since_boot; 
@@ -27,17 +157,28 @@ 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));
+        char packet[UMB_PACKET_SIZE]; 
+
+        int status = read_packet(serial_data, packet, UMB_PACKET_SIZE);
+        if (status == -1 || status < UMB_PACKET_SIZE - 1){
+            // This means we did not read enough bytes 
+
+            return rocketTelemetry; // Is this correct?? 
+        }
+        // For Debugging 
+        std::cout << "Packet: \n" << packet << std::endl;
+
+        memcpy(&metadata, packet, sizeof(metadata));
+        memcpy(&ms_since_boot, packet + 2, sizeof(ms_since_boot));
+        memcpy(&events_val, packet + 6, 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));
+        memcpy(&radio_state, packet + 10, sizeof(radio_state));
+        memcpy(&transmit_state, packet + 11, 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));
+        memcpy(&voltage, packet + 12, sizeof(voltage));
+        memcpy(&pt3, packet + 16, sizeof(pt3));
+        memcpy(&pt4, packet + 20, sizeof(pt4));
+        memcpy(&temp, packet + 24, sizeof(temp));
 
         rocketMetadata->set_alt_armed(static_cast<bool>(metadata & 0x1)); 
         rocketMetadata->set_alt_valid(static_cast<bool>((metadata & 0x2) >> 1)); 
@@ -109,7 +250,8 @@ RocketTelemetry RocketTelemetryProtoBuilder::buildProto(){
         rocketUmbTelemetry->set_rtd_temp(temp);
 
     } else {
-        printf("Serial port is not open.");
+        std::cout << "Serial port is not open. Trying to open again.\n"; 
+        openfile();
     }
     return rocketTelemetry;
 }

fill/umbilical/proto_build.h

@@ -1,26 +1,61 @@
 #ifndef PROTO_BUILD_H
 #define PROTO_BUILD_H
 
-#include <fstream>
+#define LAUNCH '0'
+#define OPEN_MAV '1'
+#define CLOSE_MAV '2'
+#define OPEN_SV '3'
+#define CLOSE_SV '4'
+#define SAFE '5'
+#define CLEAR_SD '6'
+
+#define UMB_PACKET_SIZE 28
+
 #include "protos/command.grpc.pb.h"
 #include <iostream>
+#include <fcntl.h>   
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <stdarg.h>
+#include <string.h>
+#include <termios.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <sys/stat.h>
 
 using command::RocketTelemetry;
 using command::RocketMetadata;
 using command::Events;
 using command::FlightMode; 
 using command::RocketUmbTelemetry;
+using command::Command; 
 
 class RocketTelemetryProtoBuilder {
     private:
-        const char* usb_port = "/dev/ttyACM0";
+        const char* usb_port = "/dev/rocket";
+
+        int serial_data;
+
+        bool is_fd_open(int fd);
 
-        std::ifstream serial_data;
+        void openfile();
+
+        ssize_t read_packet(int fd, char* packet, size_t max_size);
+
+        void write_command(char com);
     public: 
         // Establish serial connection 
         RocketTelemetryProtoBuilder();
         ~RocketTelemetryProtoBuilder();
 
+        void sendCommand(const Command* com); 
+
+        // send the Safe command when ground server disconnects from the fill station
+        void sendSafeCommand();
+
         RocketTelemetry buildProto(); 
 };
 

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

protos/command.proto

@@ -37,7 +37,8 @@ message Command {
     optional bool ignite = 5;
     optional bool sv2_close = 6;
     optional bool mav_open = 7;
-    optional bool fire = 8;
+    optional bool launch = 8;
+    // optional bool clear_sd = 9;
 }
 
 // The response message containing an ack