diff --git a/fill/BUILD b/fill/BUILD index f148578c..16b07527 100644 --- a/fill/BUILD +++ b/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", ], -) +) \ No newline at end of file diff --git a/fill/fill_station.cc b/fill/fill_station.cc index de111062..8ba0c825 100644 --- a/fill/fill_station.cc +++ b/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 *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 *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) { 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) // 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(builder.BuildAndStart()); diff --git a/fill/umbilical/BUILD b/fill/umbilical/BUILD new file mode 100644 index 00000000..264c657b --- /dev/null +++ b/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__", + ], +) \ No newline at end of file diff --git a/fill/umbilical/proto_build.cc b/fill/umbilical/proto_build.cc new file mode 100644 index 00000000..da6baae6 --- /dev/null +++ b/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(&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)); + + rocketMetadata->set_alt_armed(static_cast(metadata & 0x1)); + rocketMetadata->set_alt_valid(static_cast((metadata & 0x2) >> 1)); + rocketMetadata->set_gps_valid(static_cast((metadata & 0x4) >> 2)); + rocketMetadata->set_imu_valid(static_cast((metadata & 0x8) >> 3)); + rocketMetadata->set_acc_valid(static_cast((metadata & 0x10) >> 4)); + rocketMetadata->set_therm_valid(static_cast((metadata & 0x20) >> 5)); + rocketMetadata->set_voltage_valid(static_cast((metadata & 0x40) >> 6)); + rocketMetadata->set_adc_valid(static_cast((metadata & 0x80) >> 7)); + rocketMetadata->set_fram_valid(static_cast((metadata & 0x100) >> 8)); + rocketMetadata->set_sd_valid(static_cast((metadata & 0x200) >> 9)); + rocketMetadata->set_gps_msg_valid(static_cast((metadata & 0x400) >> 10)); + rocketMetadata->set_mav_state(static_cast((metadata & 0x800) >> 11)); + rocketMetadata->set_sv_state(static_cast((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(events_val & 0x1)); + events->set_altimeter_init_failed(static_cast((events_val & 0x2) >> 1)); + events->set_altimeter_reading_failed(static_cast((events_val & 0x4) >> 2)); + events->set_gps_init_failed(static_cast((events_val & 0x8) >> 3)); + events->set_gps_reading_failed(static_cast((events_val & 0x10) >> 4)); + events->set_imu_init_failed(static_cast((events_val & 0x20) >> 5)); + events->set_imu_reading_failed(static_cast((events_val & 0x40) >> 6)); + events->set_accelerometer_init_failed(static_cast((events_val & 0x80) >> 7)); + events->set_accelerometer_reading_failed(static_cast((events_val & 0x100) >> 8)); + events->set_thermometer_init_failed(static_cast((events_val & 0x200) >> 9)); + events->set_thermometer_reading_failed(static_cast((events_val & 0x400) >> 10)); + events->set_voltage_init_failed(static_cast((events_val & 0x800) >> 11)); + events->set_voltage_reading_failed(static_cast((events_val & 0x1000) >> 12)); + events->set_adc_init_failed(static_cast((events_val & 0x2000) >> 13)); + events->set_adc_reading_failed(static_cast((events_val & 0x4000) >> 14)); + events->set_fram_init_failed(static_cast((events_val & 0x8000) >> 15)); + events->set_fram_write_failed(static_cast((events_val & 0x10000) >> 16)); + events->set_sd_init_failed(static_cast((events_val & 0x20000) >> 17)); + events->set_sd_write_failed(static_cast((events_val & 0x40000) >> 18)); + events->set_mav_was_actuated(static_cast((events_val & 0x80000) >> 19)); + events->set_sv_was_actuated(static_cast((events_val & 0x100000) >> 20)); + events->set_main_deploy_wait_end(static_cast((events_val & 0x200000) >> 21)); + events->set_main_log_shutoff(static_cast((events_val & 0x400000) >> 22)); + events->set_cycle_overflow(static_cast((events_val & 0x800000) >> 23)); + events->set_invalid_command(static_cast((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; +} \ No newline at end of file diff --git a/fill/umbilical/proto_build.h b/fill/umbilical/proto_build.h new file mode 100644 index 00000000..0faf4564 --- /dev/null +++ b/fill/umbilical/proto_build.h @@ -0,0 +1,27 @@ +#ifndef PROTO_BUILD_H +#define PROTO_BUILD_H + +#include +#include "protos/command.grpc.pb.h" +#include + +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 \ No newline at end of file diff --git a/protos/command.proto b/protos/command.proto index 05c4a2ae..decd16b9 100644 --- a/protos/command.proto +++ b/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 {