Implementing Rocket Umbilical

fill/BUILD

@@ -5,6 +5,7 @@ cc_binary(
         "//protos:command_cc_grpc",
         "//fill/actuators:qd",
         "//fill/actuators:ball_valve",
+        "//fill/umbilical:rocket_proto_builder",
         "//fill/actuators:sol_valve",
         "//fill/sensors:pt",
         "@abseil-cpp//absl/flags:flag",
@@ -13,4 +14,4 @@ cc_binary(
         "@com_github_grpc_grpc//:grpc++",
         "@com_github_grpc_grpc//:grpc++_reflection",
     ],
-)
+)

fill/fill_station.cc

@@ -12,6 +12,7 @@
 #include "actuators/sol_valve.h"
 
 #include "sensors/pt.h"
+#include "umbilical/proto_build.h"
 
 #include "wiringPi.h"
 
@@ -31,7 +32,10 @@ using command::Commander;
 using command::FillStationTelemeter;
 using command::CommandReply;
 using command::FillStationTelemetry;
-using command::TelemetryRequest;
+using command::FillStationTelemetryRequest;
+using command::RocketTelemetryRequest;
+using command::RocketTelemetry;
+using command::RocketTelemeter;
 using grpc::Channel;
 using grpc::ClientContext;
 using grpc::Server;
@@ -48,6 +52,8 @@ SolValve sv1;
 /* Sensors */
 PT pt1 = PT(0x48, 3);
 PT pt2 = PT(0x48, 2);
+/* Umblical Tools */
+RocketTelemetryProtoBuilder protoBuild;
 
 ABSL_FLAG(uint16_t, server_port, 50051, "Server port for the service");
 
@@ -131,7 +137,7 @@ class CommanderServiceImpl final : public Commander::Service
 // Fill Station service to stream telemetry.
 class TelemeterServiceImpl final : public FillStationTelemeter::Service
 {
-    Status StreamTelemetry(ServerContext *context, const TelemetryRequest *request,
+    Status StreamTelemetry(ServerContext *context, const FillStationTelemetryRequest *request,
                        ServerWriter<FillStationTelemetry> *writer) override
     {
         while (true) {
@@ -147,12 +153,29 @@ class TelemeterServiceImpl final : public FillStationTelemeter::Service
     }
 };
 
+class RocketTelemeterServiceImpl final : public RocketTelemeter::Service 
+{
+    Status StreamTelemetry(ServerContext *context, const RocketTelemetryRequest *request,
+                       ServerWriter<RocketTelemetry> *writer) override
+    {
+        while (true) {
+            auto now = std::chrono::high_resolution_clock::now();
+            RocketTelemetry t = protoBuild.buildProto();
+            if (!writer->Write(t)) {
+                // Broken stream
+                return Status::CANCELLED; 
+            }
+        }
+    }
+};
+
 // Server startup function
 void RunServer(uint16_t port, std::shared_ptr<Server> server)
 {
     std::string server_address = absl::StrFormat("0.0.0.0:%d", port);
     CommanderServiceImpl commander_service;
     TelemeterServiceImpl telemeter_service;
+    RocketTelemeterServiceImpl rocket_telemeter_service; 
 
     grpc::EnableDefaultHealthCheckService(true);
     grpc::reflection::InitProtoReflectionServerBuilderPlugin();
@@ -162,6 +185,7 @@ void RunServer(uint16_t port, std::shared_ptr<Server> server)
     // Register services.
     builder.RegisterService(&commander_service);
     builder.RegisterService(&telemeter_service);
+    builder.RegisterService(&rocket_telemeter_service);
     // Finally assemble the server.
     server = builder.BuildAndStart();
     // std::unique_ptr<Server> server(builder.BuildAndStart());

fill/umbilical/BUILD

@@ -0,0 +1,13 @@
+cc_library(
+    name = "rocket_proto_builder",
+    srcs = ["proto_build.cc"],
+    hdrs = ["proto_build.h"],
+    deps = [
+        "//protos:command_cc_grpc",
+        "@com_github_grpc_grpc//:grpc++",
+        "@com_github_grpc_grpc//:grpc++_reflection",
+    ],
+    visibility = [
+        "//fill:__subpackages__",
+        ],
+)

fill/umbilical/proto_build.cc

@@ -0,0 +1,115 @@
+#include "proto_build.h"
+
+RocketTelemetryProtoBuilder::RocketTelemetryProtoBuilder(): serial_data(usb_port, std::ios::binary){}
+
+RocketTelemetryProtoBuilder::~RocketTelemetryProtoBuilder(){
+    serial_data.close();
+}
+
+RocketTelemetry RocketTelemetryProtoBuilder::buildProto(){
+    RocketTelemetry rocketTelemetry; 
+
+    if (!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; 
+        uint64_t events_val; 
+
+        bool radio_state; 
+        bool transmit_state; 
+
+        float voltage;
+        float pt3;
+        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));
+
+        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));
+
+        rocketMetadata->set_alt_armed(static_cast<bool>(metadata & 0x1)); 
+        rocketMetadata->set_alt_valid(static_cast<bool>((metadata & 0x2) >> 1)); 
+        rocketMetadata->set_gps_valid(static_cast<bool>((metadata & 0x4) >> 2)); 
+        rocketMetadata->set_imu_valid(static_cast<bool>((metadata & 0x8) >> 3)); 
+        rocketMetadata->set_acc_valid(static_cast<bool>((metadata & 0x10) >> 4)); 
+        rocketMetadata->set_therm_valid(static_cast<bool>((metadata & 0x20) >> 5)); 
+        rocketMetadata->set_voltage_valid(static_cast<bool>((metadata & 0x40) >> 6)); 
+        rocketMetadata->set_adc_valid(static_cast<bool>((metadata & 0x80) >> 7)); 
+        rocketMetadata->set_fram_valid(static_cast<bool>((metadata & 0x100) >> 8)); 
+        rocketMetadata->set_sd_valid(static_cast<bool>((metadata & 0x200) >> 9)); 
+        rocketMetadata->set_gps_msg_valid(static_cast<bool>((metadata & 0x400) >> 10)); 
+        rocketMetadata->set_mav_state(static_cast<bool>((metadata & 0x800) >> 11)); 
+        rocketMetadata->set_sv_state(static_cast<bool>((metadata & 0x1000) >> 12)); 
+
+        switch((metadata & 0xE000) >> 13) {
+            case 0b000:
+                rocketMetadata->set_flight_mode(command::STARTUP);
+                break; 
+            case 0b001:
+                rocketMetadata->set_flight_mode(command::STANDBY);
+                break; 
+            case 0b010:
+                rocketMetadata->set_flight_mode(command::ASCENT);
+                break; 
+            case 0b011:
+                rocketMetadata->set_flight_mode(command::DROGUE_DEPLOYED);
+                break; 
+            case 0b100:
+                rocketMetadata->set_flight_mode(command::MAIN_DEPLOYED);
+                break; 
+            case 0b101:
+                rocketMetadata->set_flight_mode(command::FAULT);
+                break; 
+        }
+
+        events->set_altitude_armed(static_cast<bool>(events_val & 0x1));
+        events->set_altimeter_init_failed(static_cast<bool>((events_val & 0x2) >> 1)); 
+        events->set_altimeter_reading_failed(static_cast<bool>((events_val & 0x4) >> 2)); 
+        events->set_gps_init_failed(static_cast<bool>((events_val & 0x8) >> 3)); 
+        events->set_gps_reading_failed(static_cast<bool>((events_val & 0x10) >> 4)); 
+        events->set_imu_init_failed(static_cast<bool>((events_val & 0x20) >> 5)); 
+        events->set_imu_reading_failed(static_cast<bool>((events_val & 0x40) >> 6)); 
+        events->set_accelerometer_init_failed(static_cast<bool>((events_val & 0x80) >> 7)); 
+        events->set_accelerometer_reading_failed(static_cast<bool>((events_val & 0x100) >> 8)); 
+        events->set_thermometer_init_failed(static_cast<bool>((events_val & 0x200) >> 9)); 
+        events->set_thermometer_reading_failed(static_cast<bool>((events_val & 0x400) >> 10)); 
+        events->set_voltage_init_failed(static_cast<bool>((events_val & 0x800) >> 11)); 
+        events->set_voltage_reading_failed(static_cast<bool>((events_val & 0x1000) >> 12)); 
+        events->set_adc_init_failed(static_cast<bool>((events_val & 0x2000) >> 13)); 
+        events->set_adc_reading_failed(static_cast<bool>((events_val & 0x4000) >> 14)); 
+        events->set_fram_init_failed(static_cast<bool>((events_val & 0x8000) >> 15)); 
+        events->set_fram_write_failed(static_cast<bool>((events_val & 0x10000) >> 16)); 
+        events->set_sd_init_failed(static_cast<bool>((events_val & 0x20000) >> 17)); 
+        events->set_sd_write_failed(static_cast<bool>((events_val & 0x40000) >> 18)); 
+        events->set_mav_was_actuated(static_cast<bool>((events_val & 0x80000) >> 19)); 
+        events->set_sv_was_actuated(static_cast<bool>((events_val & 0x100000) >> 20)); 
+        events->set_main_deploy_wait_end(static_cast<bool>((events_val & 0x200000) >> 21)); 
+        events->set_main_log_shutoff(static_cast<bool>((events_val & 0x400000) >> 22)); 
+        events->set_cycle_overflow(static_cast<bool>((events_val & 0x800000) >> 23)); 
+        events->set_invalid_command(static_cast<bool>((events_val & 0x1000000) >> 24)); 
+
+        rocketUmbTelemetry->set_ms_since_boot(ms_since_boot);
+        rocketUmbTelemetry->set_radio_state(radio_state);
+        rocketUmbTelemetry->set_transmit_state(transmit_state);
+        rocketUmbTelemetry->set_voltage(voltage);
+        rocketUmbTelemetry->set_pt3(pt3);
+        rocketUmbTelemetry->set_pt4(pt4);
+        rocketUmbTelemetry->set_rtd_temp(temp);
+
+    } else {
+        printf("Serial port is not open.");
+    }
+    return rocketTelemetry;
+}

fill/umbilical/proto_build.h

@@ -0,0 +1,27 @@
+#ifndef PROTO_BUILD_H
+#define PROTO_BUILD_H
+
+#include <fstream>
+#include "protos/command.grpc.pb.h"
+#include <iostream>
+
+using command::RocketTelemetry;
+using command::RocketMetadata;
+using command::Events;
+using command::FlightMode; 
+using command::RocketUmbTelemetry;
+
+class RocketTelemetryProtoBuilder {
+    private:
+        const char* usb_port = "PUT NAME HERE";
+
+        std::ifstream serial_data;
+    public: 
+        // Establish serial connection 
+        RocketTelemetryProtoBuilder();
+        ~RocketTelemetryProtoBuilder();
+
+        RocketTelemetry buildProto(); 
+};
+
+#endif 

protos/command.proto

@@ -12,13 +12,13 @@ package command;
 // The telemetry service definition
 service FillStationTelemeter {
     // Sends telemetry
-    rpc StreamTelemetry (TelemetryRequest) returns (stream FillStationTelemetry) {}
+    rpc StreamTelemetry (FillStationTelemetryRequest) returns (stream FillStationTelemetry) {}
 }
 
 // The telemetry service definition
 service RocketTelemeter {
     // Sends telemetry
-    rpc StreamTelemetry (TelemetryRequest) returns (stream RocketTelemetry) {}
+    rpc StreamTelemetry (RocketTelemetryRequest) returns (stream RocketTelemetry) {}
 }
 
 // The commanding service definition
@@ -45,7 +45,12 @@ message Command {
 message CommandReply {}
 
 // The telemetry request message
-message TelemetryRequest {}
+message FillStationTelemetryRequest {}
+
+// Rocket Telemetry Request 
+message RocketTelemetryRequest {
+    bool isRocketTelemetryRequest = 1;
+}
 
 enum FlightMode {
     STARTUP = 0;
@@ -57,27 +62,31 @@ enum FlightMode {
 }
 
 message Events {
-    bool key_armed = 1;
-    bool altitude_armed = 2;
-    bool altimeter_init_failed = 3;
-    bool altimeter_reading_failed = 4;
-    bool altimeter_was_turned_off = 5;
-    bool gps_init_failed = 6;
-    bool gps_reading_failed = 7;
-    bool gps_was_turned_off = 8;
-    bool imu_init_failed = 9;
-    bool imu_reading_failed = 10;
-    bool imu_was_turned_off = 11;
-    bool accelerometer_init_failed = 12;
-    bool accelerometer_reading_failed = 13;
-    bool accelerometer_was_turned_off = 14;
-    bool thermometer_init_failed = 15;
-    bool thermometer_reading_failed = 16;
-    bool thermometer_was_turned_off = 17;
+    bool altitude_armed = 1;
+    bool altimeter_init_failed = 2;
+    bool altimeter_reading_failed = 3;
+    bool gps_init_failed = 4;
+    bool gps_reading_failed = 5;
+    bool imu_init_failed = 6;
+    bool imu_reading_failed = 7;
+    bool accelerometer_init_failed = 8;
+    bool accelerometer_reading_failed = 9;
+    bool thermometer_init_failed = 10;
+    bool thermometer_reading_failed = 11;
+    bool voltage_init_failed = 12;
+    bool voltage_reading_failed = 13;
+    bool adc_init_failed = 14;
+    bool adc_reading_failed = 15; 
+    bool fram_init_failed = 16;
+    bool fram_write_failed = 17; 
     bool sd_init_failed = 18;
     bool sd_write_failed = 19;
-    bool rfm_init_failed = 20;
-    bool rfm_transmit_failed = 21;
+    bool mav_was_actuated = 20; 
+    bool sv_was_actuated = 21; 
+    bool main_deploy_wait_end = 22;
+    bool main_log_shutoff = 23;
+    bool cycle_overflow = 24; 
+    bool invalid_command = 25; 
 }
 
 message RocketMetadata {