From 8846d0770bc6c04e985560502f87ee895945e671 Mon Sep 17 00:00:00 2001 From: bwzsilicon Date: Sat, 16 Nov 2024 21:41:45 -0500 Subject: [PATCH] Implementing Rocket Commanding --- fill/fill_station.cc | 14 ++---- fill/umbilical/proto_build.cc | 87 +++++++++++++++++++++++++++++------ fill/umbilical/proto_build.h | 14 +++++- go-proxies/proxy_build.sh | 2 +- protos/command.proto | 9 ++-- 5 files changed, 98 insertions(+), 28 deletions(-) diff --git a/fill/fill_station.cc b/fill/fill_station.cc index 4b1e8ad..7516bd8 100644 --- 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; } }; @@ -144,9 +138,11 @@ class RocketTelemeterServiceImpl final : public RocketTelemeter::Service Status StreamTelemetry(ServerContext *context, const RocketTelemetryRequest *request, ServerWriter *writer) override { + std::cout << "Received first rocket telemetry request\n"; while (true) { auto now = std::chrono::high_resolution_clock::now(); RocketTelemetry t = protoBuild.buildProto(); + std::cout << "About to write protobuf to stream\n"; if (!writer->Write(t)) { // Broken stream return Status::CANCELLED; diff --git a/fill/umbilical/proto_build.cc b/fill/umbilical/proto_build.cc index da6baae..16643d2 100644 --- a/fill/umbilical/proto_build.cc +++ b/fill/umbilical/proto_build.cc @@ -1,19 +1,41 @@ #include "proto_build.h" +#include -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; - - if (!serial_data){ + std::cout << "About to read file" << std::endl; + if (serial_data.is_open()){ + std::cout << "File is open" << std::endl; 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 +49,54 @@ RocketTelemetry RocketTelemetryProtoBuilder::buildProto(){ float pt4; float temp; - serial_data.read(reinterpret_cast(&metadata), sizeof(metadata)); - serial_data.read(reinterpret_cast(&ms_since_boot), sizeof(ms_since_boot)); - serial_data.read(reinterpret_cast(&events_val), sizeof(events_val)); + /* + OLD VERSION THAT DID NOT ACCOUNT FOR \n AFTER EVERY PACKET + */ + + // serial_data.read(reinterpret_cast(&metadata), sizeof(metadata)); + // serial_data.read(reinterpret_cast(&ms_since_boot), sizeof(ms_since_boot)); + // serial_data.read(reinterpret_cast(&events_val), sizeof(events_val)); + + // serial_data.read(reinterpret_cast(&radio_state), sizeof(radio_state)); + // serial_data.read(reinterpret_cast(&transmit_state), sizeof(transmit_state)); + + // serial_data.read(reinterpret_cast(&voltage), sizeof(voltage)); + // serial_data.read(reinterpret_cast(&pt3), sizeof(pt3)); + // serial_data.read(reinterpret_cast(&pt4), sizeof(pt4)); + // serial_data.read(reinterpret_cast(&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(&radio_state), sizeof(radio_state)); - serial_data.read(reinterpret_cast(&transmit_state), sizeof(transmit_state)); + printf("Radio_state: %d\n", radio_state); + printf("Transmit_state: %d\n", transmit_state);; - serial_data.read(reinterpret_cast(&voltage), sizeof(voltage)); - serial_data.read(reinterpret_cast(&pt3), sizeof(pt3)); - serial_data.read(reinterpret_cast(&pt4), sizeof(pt4)); - serial_data.read(reinterpret_cast(&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(metadata & 0x1)); rocketMetadata->set_alt_valid(static_cast((metadata & 0x2) >> 1)); @@ -109,7 +168,7 @@ RocketTelemetry RocketTelemetryProtoBuilder::buildProto(){ rocketUmbTelemetry->set_rtd_temp(temp); } else { - printf("Serial port is not open."); + std::cout << "Serial port is not open.\n"; } return rocketTelemetry; } \ No newline at end of file diff --git a/fill/umbilical/proto_build.h b/fill/umbilical/proto_build.h index 23637f1..c3a460a 100644 --- 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 #include "protos/command.grpc.pb.h" #include @@ -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 index d82ce1e..94f0a2d 100755 --- 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 \ No newline at end of file diff --git a/protos/command.proto b/protos/command.proto index decd16b..dfe9919 100644 --- 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