diff --git a/TARS/src/mcu_main/telemetry.cpp b/TARS/src/mcu_main/telemetry.cpp index b606011a..3ece10a6 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..73f7b659 100644 --- a/TARS/src/mcu_main/telemetry.h +++ b/TARS/src/mcu_main/telemetry.h @@ -37,6 +37,7 @@ struct TelemetryDataLite { }; struct TelemetryPacket { + unsigned short idx = 0; TelemetryDataLite datapoints[4]; float gps_lat; float gps_long; @@ -68,6 +69,25 @@ struct TelemetryPacket { int16_t barometer_temp; //[-128, 128] }; +struct CompactTelemetryPacket { + unsigned short 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 +137,6 @@ class Telemetry { command_handler_struct freq_status = {}; TelemetryPacket makePacket(const sensorDataStruct_t& data_struct); + + CompactTelemetryPacket makeCompactPacket(const sensorDataStruct_t& data_struct); }; diff --git a/ground/src/feather/main.cpp b/ground/src/feather/main.cpp index c1983ee6..144028da 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,67 @@ 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); + + // 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); + 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("}}"); }