From 2349e8028d4e4a30a55609612067bf982a6121dd Mon Sep 17 00:00:00 2001 From: KaustubhKhulbe Date: Wed, 6 Sep 2023 15:33:31 -0500 Subject: [PATCH 1/4] initial variable buffer software --- TARS/src/mcu_main/telemetry.cpp | 33 +++++++++++++++++++++++++++++++-- TARS/src/mcu_main/telemetry.h | 23 +++++++++++++++++++++++ 2 files changed, 54 insertions(+), 2 deletions(-) diff --git a/TARS/src/mcu_main/telemetry.cpp b/TARS/src/mcu_main/telemetry.cpp index b606011a..b49f0643 100644 --- a/TARS/src/mcu_main/telemetry.cpp +++ b/TARS/src/mcu_main/telemetry.cpp @@ -159,9 +159,26 @@ void Telemetry::transmit() { digitalWrite(LED_BLUE, blue_state); blue_state = !blue_state; - TelemetryPacket packet = makePacket(dataLogger.read()); + Packets packet; + + auto rocket_state = dataLogger.read().rocketState_data.rocketStates[0]; + if (rocket_state == FSM_State::STATE_INIT || rocket_state == FSM_State::STATE_IDLE) { + packet.compact_packet = makeCompactPacket(dataLogger.read()); + packet.type = Packets::COMPACT; + } else { + packet.default_packet = makePacket(dataLogger.read()); + packet.type = Packets::DEFAULT; + } + #ifndef ENABLE_SILSIM_MODE - rf95.send((uint8_t *)&packet, sizeof(packet)); + switch (packet.type) { + case (Packets::DEFAULT): + rf95.send((uint8_t *)&packet, sizeof(packet.default_packet)); + break; + case (Packets::COMPACT): + rf95.send((uint8_t *)&packet, sizeof(packet.compact_packet)); + break; + } chThdSleepMilliseconds(170); @@ -392,6 +409,18 @@ TelemetryPacket Telemetry::makePacket(const sensorDataStruct_t &data_struct) { return packet; } +CompactTelemetryPacket Telemetry::makeCompactPacket(const sensorDataStruct_t &data_struct) { + CompactTelemetryPacket packet{}; + packet.gps_lat = data_struct.gps_data.latitude; + packet.gps_long = data_struct.gps_data.longitude; + packet.gps_alt = data_struct.gps_data.altitude; + + packet.voltage_battery = inv_convert_range(data_struct.voltage_data.v_battery, 16); + packet.barometer_temp = inv_convert_range(data_struct.barometer_data.temperature, 256); + + return packet; +} + void Telemetry::bufferData() { #ifdef ENABLE_TELEMETRY #ifndef TLM_DEBUG diff --git a/TARS/src/mcu_main/telemetry.h b/TARS/src/mcu_main/telemetry.h index 6a0d1d74..b3b4a575 100644 --- a/TARS/src/mcu_main/telemetry.h +++ b/TARS/src/mcu_main/telemetry.h @@ -37,6 +37,7 @@ struct TelemetryDataLite { }; struct TelemetryPacket { + unsigned int idx = 0; TelemetryDataLite datapoints[4]; float gps_lat; float gps_long; @@ -68,6 +69,26 @@ struct TelemetryPacket { int16_t barometer_temp; //[-128, 128] }; +struct CompactTelemetryPacket { + unsigned int idx = 1; + float gps_lat; + float gps_long; + float gps_alt; + uint8_t voltage_battery; //[0, 16] + int16_t barometer_temp; //[-128, 128] +}; + +struct Packets { + enum { + COMPACT, + DEFAULT, + } type; + + TelemetryPacket default_packet; + CompactTelemetryPacket compact_packet; + +}; + // Commands transmitted from ground station to rocket enum CommandType { SET_FREQ, SET_CALLSIGN, ABORT, TEST_FLAPS, EMPTY }; @@ -117,4 +138,6 @@ class Telemetry { command_handler_struct freq_status = {}; TelemetryPacket makePacket(const sensorDataStruct_t& data_struct); + + CompactTelemetryPacket makeCompactPacket(const sensorDataStruct_t& data_struct); }; From 30efa489ec7b681eccf66ef0524193317c99c9e4 Mon Sep 17 00:00:00 2001 From: KaustubhKhulbe Date: Wed, 6 Sep 2023 16:59:30 -0500 Subject: [PATCH 2/4] formatting and change idx to short --- TARS/src/mcu_main/telemetry.cpp | 2 +- TARS/src/mcu_main/telemetry.h | 5 ++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/TARS/src/mcu_main/telemetry.cpp b/TARS/src/mcu_main/telemetry.cpp index b49f0643..3ece10a6 100644 --- a/TARS/src/mcu_main/telemetry.cpp +++ b/TARS/src/mcu_main/telemetry.cpp @@ -165,7 +165,7 @@ void Telemetry::transmit() { if (rocket_state == FSM_State::STATE_INIT || rocket_state == FSM_State::STATE_IDLE) { packet.compact_packet = makeCompactPacket(dataLogger.read()); packet.type = Packets::COMPACT; - } else { + } else { packet.default_packet = makePacket(dataLogger.read()); packet.type = Packets::DEFAULT; } diff --git a/TARS/src/mcu_main/telemetry.h b/TARS/src/mcu_main/telemetry.h index b3b4a575..73f7b659 100644 --- a/TARS/src/mcu_main/telemetry.h +++ b/TARS/src/mcu_main/telemetry.h @@ -37,7 +37,7 @@ struct TelemetryDataLite { }; struct TelemetryPacket { - unsigned int idx = 0; + unsigned short idx = 0; TelemetryDataLite datapoints[4]; float gps_lat; float gps_long; @@ -70,7 +70,7 @@ struct TelemetryPacket { }; struct CompactTelemetryPacket { - unsigned int idx = 1; + unsigned short idx = 1; float gps_lat; float gps_long; float gps_alt; @@ -86,7 +86,6 @@ struct Packets { TelemetryPacket default_packet; CompactTelemetryPacket compact_packet; - }; // Commands transmitted from ground station to rocket From b54b4e6d473b9efe741fa0237a6689d6c91f328f Mon Sep 17 00:00:00 2001 From: KaustubhKhulbe Date: Wed, 6 Sep 2023 18:31:13 -0500 Subject: [PATCH 3/4] add ground parsing --- ground/src/feather/main.cpp | 91 ++++++++++++++++++++++++------------- 1 file changed, 60 insertions(+), 31 deletions(-) diff --git a/ground/src/feather/main.cpp b/ground/src/feather/main.cpp index c1983ee6..fab1102f 100644 --- a/ground/src/feather/main.cpp +++ b/ground/src/feather/main.cpp @@ -76,6 +76,7 @@ struct TelemetryDataLite { struct TelemetryPacket { TelemetryDataLite datapoints[4]; + unsigned int idx; float gps_lat; float gps_long; float gps_alt; @@ -138,6 +139,8 @@ struct FullTelemetryData { float barometer_temp; //[-128, 128] float freq; int64_t print_time; + + unsigned int idx; }; enum class CommandType { SET_FREQ, SET_CALLSIGN, ABORT, TEST_FLAP, EMPTY }; @@ -182,14 +185,20 @@ void printFloat(float f, int precision = 5) { } } -void EnqueuePacket(const TelemetryPacket& packet, float frequency) { - if (packet.datapoint_count == 0) return; +void SetCompactData(const TelemetryPacket& packet, FullTelemetryData& item) { + item.barometer_temp = convert_range(packet.barometer_temp, 256); + item.gps_alt = packet.gps_alt; + item.gps_lat = packet.gps_lat; + item.gps_long = packet.gps_long; + item.voltage_battery = convert_range(packet.voltage_battery, 16); + print_queue.emplace(item); +} +void SetData(const TelemetryPacket& packet, FullTelemetryData& item, float frequency) { int64_t start_timestamp = packet.datapoints[0].timestamp; int64_t start_printing = millis(); for (int i = 0; i < packet.datapoint_count && i < 4; i++) { - FullTelemetryData item; TelemetryDataLite data = packet.datapoints[i]; item.barometer_pressure = convert_range(data.barometer_pressure, 4096); item.barometer_temp = convert_range(packet.barometer_temp, 256); @@ -223,6 +232,18 @@ void EnqueuePacket(const TelemetryPacket& packet, float frequency) { } } +void EnqueuePacket(const TelemetryPacket& packet, float frequency) { + if (packet.datapoint_count == 0) return; + + FullTelemetryData item; + item.idx = packet.idx; + if (packet.idx == 1) { // compact + SetCompactData(packet, item); + } else { + SetData(packet, item, frequency); + } +} + void printJSONField(const char* name, float val, bool comma = true) { Serial.print('\"'); Serial.print(name); @@ -250,34 +271,42 @@ void printJSONField(const char* name, const char* val, bool comma = true) { void printPacketJson(FullTelemetryData const& packet) { Serial.print(R"({"type": "data", "value": {)"); - printJSONField("response_ID", packet.response_ID); - printJSONField("gps_lat", packet.gps_lat); - printJSONField("gps_long", packet.gps_long); - printJSONField("gps_alt", packet.gps_alt); - printJSONField("KX_IMU_ax", packet.highG_ax); - printJSONField("KX_IMU_ay", packet.highG_ay); - printJSONField("KX_IMU_az", packet.highG_az); - printJSONField("IMU_gx", packet.gyro_x); - printJSONField("IMU_gy", packet.gyro_y); - printJSONField("IMU_gz", packet.gyro_z); - printJSONField("IMU_mx", packet.mag_x); - printJSONField("IMU_my", packet.mag_y); - printJSONField("IMU_mz", packet.mag_z); - printJSONField("FSM_state", packet.FSM_State); - printJSONField("sign", "NOSIGN"); - printJSONField("RSSI", rf95.lastRssi()); - printJSONField("Voltage", packet.voltage_battery); - printJSONField("frequency", packet.freq); - printJSONField("flap_extension", packet.flap_extension); - printJSONField("STE_ALT", packet.gnc_state_x); - printJSONField("STE_VEL", packet.gnc_state_vx); - printJSONField("STE_ACC", packet.gnc_state_ax); - printJSONField("STE_APO", packet.gnc_state_apo); - printJSONField("BNO_YAW", packet.bno_yaw); - printJSONField("BNO_PITCH", packet.bno_pitch); - printJSONField("BNO_ROLL", packet.bno_roll); - printJSONField("TEMP", packet.barometer_temp); - printJSONField("pressure", packet.barometer_pressure, false); + if (packet.idx == 1) { // compact + printJSONField("gps_lat", packet.gps_lat); + printJSONField("gps_long", packet.gps_long); + printJSONField("gps_alt", packet.gps_alt); + printJSONField("Voltage", packet.voltage_battery); + printJSONField("TEMP", packet.barometer_temp); + } else { + printJSONField("response_ID", packet.response_ID); + printJSONField("gps_lat", packet.gps_lat); + printJSONField("gps_long", packet.gps_long); + printJSONField("gps_alt", packet.gps_alt); + printJSONField("KX_IMU_ax", packet.highG_ax); + printJSONField("KX_IMU_ay", packet.highG_ay); + printJSONField("KX_IMU_az", packet.highG_az); + printJSONField("IMU_gx", packet.gyro_x); + printJSONField("IMU_gy", packet.gyro_y); + printJSONField("IMU_gz", packet.gyro_z); + printJSONField("IMU_mx", packet.mag_x); + printJSONField("IMU_my", packet.mag_y); + printJSONField("IMU_mz", packet.mag_z); + printJSONField("FSM_state", packet.FSM_State); + printJSONField("sign", "NOSIGN"); + printJSONField("RSSI", rf95.lastRssi()); + printJSONField("Voltage", packet.voltage_battery); + printJSONField("frequency", packet.freq); + printJSONField("flap_extension", packet.flap_extension); + printJSONField("STE_ALT", packet.gnc_state_x); + printJSONField("STE_VEL", packet.gnc_state_vx); + printJSONField("STE_ACC", packet.gnc_state_ax); + printJSONField("STE_APO", packet.gnc_state_apo); + printJSONField("BNO_YAW", packet.bno_yaw); + printJSONField("BNO_PITCH", packet.bno_pitch); + printJSONField("BNO_ROLL", packet.bno_roll); + printJSONField("TEMP", packet.barometer_temp); + printJSONField("pressure", packet.barometer_pressure, false); + } Serial.println("}}"); } From 1b1fb0cdbd65d780bf66464a0310b6d8b575f122 Mon Sep 17 00:00:00 2001 From: KaustubhKhulbe Date: Tue, 12 Sep 2023 20:22:49 -0500 Subject: [PATCH 4/4] print zeroes --- ground/src/feather/main.cpp | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/ground/src/feather/main.cpp b/ground/src/feather/main.cpp index fab1102f..144028da 100644 --- a/ground/src/feather/main.cpp +++ b/ground/src/feather/main.cpp @@ -277,6 +277,31 @@ void printPacketJson(FullTelemetryData const& packet) { printJSONField("gps_alt", packet.gps_alt); printJSONField("Voltage", packet.voltage_battery); printJSONField("TEMP", packet.barometer_temp); + + // TODO(replace with memset) + printJSONField("response_ID", 0); + printJSONField("KX_IMU_ax", 0); + printJSONField("KX_IMU_ay", 0); + printJSONField("KX_IMU_az", 0); + printJSONField("IMU_gx", 0); + printJSONField("IMU_gy", 0); + printJSONField("IMU_gz", 0); + printJSONField("IMU_mx", 0); + printJSONField("IMU_my", 0); + printJSONField("IMU_mz", 0); + printJSONField("FSM_state", 0); + printJSONField("sign", "NOSIGN"); + printJSONField("RSSI", rf95.lastRssi()); + printJSONField("frequency", 0); + printJSONField("flap_extension", 0); + printJSONField("STE_ALT", 0); + printJSONField("STE_VEL", 0); + printJSONField("STE_ACC", 0); + printJSONField("STE_APO", 0); + printJSONField("BNO_YAW", 0); + printJSONField("BNO_PITCH", 0); + printJSONField("BNO_ROLL", 0); + printJSONField("pressure", 0, false); } else { printJSONField("response_ID", packet.response_ID); printJSONField("gps_lat", packet.gps_lat);