From 6b5168e6ff84dc64476a188dc12a0e18ad9e3433 Mon Sep 17 00:00:00 2001 From: Nicholas Phillips <31660017+NPIPHI@users.noreply.github.com> Date: Tue, 23 Apr 2024 18:55:12 -0500 Subject: [PATCH] New telemetry packet (#45) * packet definition * progress * packet packing + new frequencies * Update telemetry_packet.h * Update fsm_states.h --- MIDAS/src/finite-state-machines/fsm_states.h | 1 + MIDAS/src/hardware/telemetry_backend.cpp | 9 +- MIDAS/src/rocket_state.h | 1 - MIDAS/src/systems.cpp | 14 -- MIDAS/src/telemetry.cpp | 152 ++++--------------- MIDAS/src/telemetry.h | 19 --- MIDAS/src/telemetry_packet.h | 60 ++------ 7 files changed, 45 insertions(+), 211 deletions(-) diff --git a/MIDAS/src/finite-state-machines/fsm_states.h b/MIDAS/src/finite-state-machines/fsm_states.h index ad0f7fa2..d00fb797 100644 --- a/MIDAS/src/finite-state-machines/fsm_states.h +++ b/MIDAS/src/finite-state-machines/fsm_states.h @@ -19,4 +19,5 @@ enum FSMState { // #else STATE_FIRST_SEPARATION, // #endif + FSM_STATE_COUNT, //used to get the total number of fsm states so we can assert that the fsm will fit in 4 bits }; diff --git a/MIDAS/src/hardware/telemetry_backend.cpp b/MIDAS/src/hardware/telemetry_backend.cpp index 9f82c525..28738b32 100644 --- a/MIDAS/src/hardware/telemetry_backend.cpp +++ b/MIDAS/src/hardware/telemetry_backend.cpp @@ -24,9 +24,9 @@ // Change to 434.0 or other frequency, must match RX's freq! #ifdef IS_BOOSTER -#define RF95_FREQ 433.0 +#define RF95_FREQ 425.15 #else -#define RF95_FREQ 434.0 +#define RF95_FREQ 426.15 #endif TelemetryBackend::TelemetryBackend() : rf95(RFM96_CS, RFM96_INT) { @@ -53,7 +53,10 @@ ErrorCode TelemetryBackend::init() { if (!rf95.setFrequency(RF95_FREQ)) { return ErrorCode::RadioSetFrequencyFailed; } - + rf95.setSignalBandwidth(125000); + rf95.setCodingRate4(8); + rf95.setSpreadingFactor(10); + rf95.setPayloadCRC(true); /* * The default transmitter power is 13dBm, using PA_BOOST. * If you are using RFM95/96/97/98 modules which uses the PA_BOOST diff --git a/MIDAS/src/rocket_state.h b/MIDAS/src/rocket_state.h index 2ffdf771..472b7b33 100644 --- a/MIDAS/src/rocket_state.h +++ b/MIDAS/src/rocket_state.h @@ -111,6 +111,5 @@ struct RocketData { SensorData orientation; SensorData voltage; - Latency telem_latency; Latency log_latency; }; diff --git a/MIDAS/src/systems.cpp b/MIDAS/src/systems.cpp index b8ce5cb8..257e53cd 100644 --- a/MIDAS/src/systems.cpp +++ b/MIDAS/src/systems.cpp @@ -176,25 +176,12 @@ DECLARE_THREAD(kalman, RocketSystems* arg) { } } - -/** - * See \ref data_logger_thread - */ -DECLARE_THREAD(telemetry_buffering, RocketSystems* arg) { - while (true) { - arg->tlm.bufferData(arg->rocket_data); - THREAD_SLEEP(260/4); - } -} - - /** * See \ref data_logger_thread */ DECLARE_THREAD(telemetry, RocketSystems* arg) { while (true) { arg->tlm.transmit(arg->rocket_data, arg->led); - arg->rocket_data.telem_latency.tick(); THREAD_SLEEP(1); } @@ -257,7 +244,6 @@ ErrorCode init_systems(RocketSystems& systems) { START_THREAD(fsm, SENSOR_CORE, config, 8); START_THREAD(buzzer, SENSOR_CORE, config, 6); START_THREAD(telemetry, SENSOR_CORE, config, 15); - START_THREAD(telemetry_buffering, SENSOR_CORE, config, 14); config->buzzer.play_tune(free_bird, FREE_BIRD_LENGTH); diff --git a/MIDAS/src/telemetry.cpp b/MIDAS/src/telemetry.cpp index b1b5c7b4..7ffe8a78 100644 --- a/MIDAS/src/telemetry.cpp +++ b/MIDAS/src/telemetry.cpp @@ -20,150 +20,56 @@ T inv_convert_range(float val, float range) { return std::max(std::min((float)std::numeric_limits::max(), converted), (float)std::numeric_limits::min()); } +std::tuple pack_highg_tilt(HighGData const& highg, uint8_t tilt) { + uint16_t ax = (uint16_t)inv_convert_range(highg.ax, 32); + uint16_t ay = (uint16_t)inv_convert_range(highg.ay, 32); + uint16_t az = (uint16_t)inv_convert_range(highg.az, 32); + + uint16_t x = (ax & 0xfffc) | ((tilt >> 0) & 0x3); + uint16_t y = (ay & 0xfffc) | ((tilt >> 2) & 0x3); + uint16_t z = (az & 0xfffc) | ((tilt >> 4) & 0x3); + + return {x,y,z}; +} + Telemetry::Telemetry(TelemetryBackend&& backend) : backend(std::move(backend)) { } void Telemetry::transmit(RocketData& rocket_data, LEDController& led) { -// telemetry_command command { }; -// while (backend.read(&command)) { -// handleCommand(command); -// } - -// if (!std::isnan(set_frequency_to)) { -// backend.setFrequency(set_frequency_to); -// set_frequency_to = NAN; -// } + static_assert(sizeof(TelemetryPacket) == 20); TelemetryPacket packet = makePacket(rocket_data); led.toggle(LED::BLUE); backend.send(packet); } - -/** - * @brief This function handles commands sent from the ground station - * to TARS. The effects of this function depend on the command - * sent. - * - * @param cmd: struct containing information necessary to process - * ground station command. - * - * @return void - */ -void Telemetry::handleCommand(const telemetry_command &cmd) { - /* Check if the security code is present and matches on ground and on the rocket */ - if (cmd.verify != std::array{'A', 'Y', 'B', 'E', 'R', 'K'}) { - return; - } - /* Check if lasted command ID matched current command ID */ - if (last_command_id == cmd.cmd_id) { - return; - } - last_command_id = (int16_t)cmd.cmd_id; - - if (cmd.command == SET_FREQ) { - set_frequency_to = cmd.freq; - } - - if (cmd.command == SET_CALLSIGN) { - size_t callsign_size = sizeof(callsign); - - for (size_t i = 0; i < callsign_size; ++i) { - callsign[i] = cmd.callsign[i]; - } - Serial.println("[DEBUG]: Got callsign"); - } -} - - TelemetryPacket Telemetry::makePacket(RocketData& data) { TelemetryPacket packet { }; - - TelemetryDataLite small_packet { }; - packet.datapoint_count = 0; - for (int8_t i = 0; i < 4 && small_packet_queue.receive(&small_packet); i++) { - packet.datapoints[i] = small_packet; - packet.datapoint_count++; - } - GPS gps = data.gps.getRecentUnsync(); - Magnetometer magnetometer = data.magnetometer.getRecentUnsync(); Voltage voltage = data.voltage.getRecentUnsync(); - packet.voltage_battery = inv_convert_range(voltage.voltage, 4096); Barometer barometer = data.barometer.getRecentUnsync(); - LowGLSM lowGlsm = data.low_g_lsm.getRecentUnsync(); - - packet.gps_lat = gps.latitude; - packet.gps_long = gps.longitude; - packet.gps_alt = gps.altitude; - - packet.mag_x = inv_convert_range(magnetometer.mx, 8); - packet.mag_y = inv_convert_range(magnetometer.my, 8); - packet.mag_z = inv_convert_range(magnetometer.mz, 8); - packet.gyro_x = inv_convert_range(lowGlsm.gx, 8192); - packet.gyro_y = inv_convert_range(lowGlsm.gy, 8192); - packet.gyro_z = inv_convert_range(lowGlsm.gz, 8192); - - packet.rssi = backend.getRecentRssi(); - packet.FSM_state = (char) data.fsm_state.getRecentUnsync(); - - packet.barometer_temp = inv_convert_range(barometer.temperature, 256); - - auto pyros = data.pyro.getRecentUnsync(); - packet.pyros_bits = 0; - packet.pyros_bits |= pyros.channels[0].is_armed << 0; - packet.pyros_bits |= pyros.channels[1].is_armed << 1; - packet.pyros_bits |= pyros.channels[2].is_armed << 2; - packet.pyros_bits |= pyros.channels[3].is_armed << 3; - packet.pyros_bits |= pyros.channels[0].is_firing << 4; - packet.pyros_bits |= pyros.channels[1].is_firing << 5; - packet.pyros_bits |= pyros.channels[2].is_firing << 6; - packet.pyros_bits |= pyros.channels[3].is_firing << 7; - + FSMState fsm = data.fsm_state.getRecentUnsync(); Continuity continuity = data.continuity.getRecentUnsync(); - packet.sense_pyro = inv_convert_range(continuity.sense_pyro, 4096); - for (int i = 0; i < 4; i++) { - packet.continuity[i] = inv_convert_range(continuity.pins[i], 20); - } - - packet.telem_latency = inv_convert_range((float) data.telem_latency.getLatency(), 1024); - packet.log_latency = inv_convert_range((float) data.log_latency.getLatency(), 1024); - -#ifdef IS_BOOSTER - packet.is_booster = true; -#else - packet.is_booster = false; -#endif - - memcpy(&packet.callsign, &callsign, sizeof(callsign)); + HighGData highg = data.high_g.getRecentUnsync(); + PyroState pyro = data.pyro.getRecentUnsync(); + + packet.lat = gps.latitude; + packet.lon = gps.longitude; + packet.alt = uint16_t(gps.altitude); + packet.baro_alt = uint16_t(barometer.altitude); + auto [ax,ay,az] = pack_highg_tilt(highg, 33); + packet.highg_ax = ax; + packet.highg_ay = ay; + packet.highg_az = az; + packet.batt_volt = inv_convert_range(voltage.voltage, 16); + static_assert(FSMState::FSM_STATE_COUNT < 16); + uint8_t sat_count = gps.satellite_count < 16 ? gps.satellite_count : 15; + packet.fsm_satcount = ((uint8_t)fsm) | (sat_count << 4); return packet; } -void Telemetry::bufferData(RocketData& rocket) { - TelemetryDataLite data { }; - data.timestamp = pdTICKS_TO_MS(xTaskGetTickCount()); - data.barometer_pressure = inv_convert_range(rocket.barometer.getRecentUnsync().pressure , 4096); - - HighGData highGData = rocket.high_g.getRecentUnsync(); - data.highG_ax = inv_convert_range(highGData.ax, 256); - data.highG_ay = inv_convert_range(highGData.ay, 256); - data.highG_az = inv_convert_range(highGData.az, 256); - - LowGLSM lowGlsm = rocket.low_g_lsm.getRecentUnsync(); - data.lowg_ax = inv_convert_range(lowGlsm.ax, 8); - data.lowg_ay = inv_convert_range(lowGlsm.ay, 8); - data.lowg_az = inv_convert_range(lowGlsm.az, 8); - - Orientation orient = rocket.orientation.getRecentUnsync(); - data.bno_pitch = inv_convert_range(orient.pitch , 8); - data.bno_yaw = inv_convert_range(orient.yaw , 8); - data.bno_roll = inv_convert_range(orient.roll , 8); - - small_packet_queue.send(data); -} - ErrorCode __attribute__((warn_unused_result)) Telemetry::init() { return backend.init(); } diff --git a/MIDAS/src/telemetry.h b/MIDAS/src/telemetry.h index f67b35a5..498cefd0 100644 --- a/MIDAS/src/telemetry.h +++ b/MIDAS/src/telemetry.h @@ -20,27 +20,8 @@ class Telemetry { ErrorCode __attribute__((warn_unused_result)) init(); void transmit(RocketData& rocket_data, LEDController& led); - void bufferData(RocketData& rocket); - private: - Queue small_packet_queue; - - // Initializing command ID - int16_t last_command_id = -1; - - float set_frequency_to = NAN; - - #ifdef IS_SUSTAINER - char callsign[8] = "KD9ZMJ"; - #endif - - #ifdef IS_BOOSTER - char callsign[8] = "KD9ZPM"; - #endif - - TelemetryPacket makePacket(RocketData& data); - void handleCommand(const telemetry_command& cmd); TelemetryBackend backend; }; diff --git a/MIDAS/src/telemetry_packet.h b/MIDAS/src/telemetry_packet.h index 512bc89c..af52ac44 100644 --- a/MIDAS/src/telemetry_packet.h +++ b/MIDAS/src/telemetry_packet.h @@ -3,58 +3,16 @@ #include #include -struct TelemetryDataLite { - uint32_t timestamp; //[0, 2^32] - - uint16_t barometer_pressure; //[0, 4096] - int16_t highG_ax; //[128, -128] - int16_t highG_ay; //[128, -128] - int16_t highG_az; //[128, -128] - int16_t lowg_ax; // [-4, 4] - int16_t lowg_ay; // [-4, 4] - int16_t lowg_az; // [-4, 4] - int16_t bno_roll; //[-4,4] - int16_t bno_pitch; //[-4,4] - int16_t bno_yaw; //[-4,4] -}; - struct TelemetryPacket { - int8_t datapoint_count; //[0,4] - TelemetryDataLite datapoints[4]; - - int32_t gps_lat; - int32_t gps_long; - float gps_alt; - int16_t mag_x; // [-4, 4] - int16_t mag_y; // [-4, 4] - int16_t mag_z; // [-4, 4] - int16_t gyro_x; // [-4096, 4096] - int16_t gyro_y; // [-4096, 4096] - int16_t gyro_z; // [-4096, 4096] - int8_t rssi; // [-128, 128] - uint16_t voltage_battery; // [0, 16] - uint8_t FSM_state; // [0,256] - int16_t barometer_temp; // [-128, 128] - uint16_t sense_pyro; // [0, 16] - int8_t continuity[4]; // [-10, 10] - /** Pyros bit format: - * Lowest order bit is 0 - * 0: pyros_armed[0] - * 1: pyros_armed[1] - * 2: pyros_armed[2] - * 3: pyros_armed[3] - * 4: pyros_firing[0] - * 5: pyros_firing[1] - * 6: pyros_firing[2] - * 7: pyros_firing[3] - */ - uint8_t pyros_bits; - - uint8_t telem_latency; // [0, 1024] - uint8_t log_latency; // [0, 1024] - - bool is_booster; - char callsign[8]; + int32_t lat; + int32_t lon; + uint16_t alt; + uint16_t baro_alt; + uint16_t highg_ax; //14 bit signed ax [-16,16) 2 bit tilt angle + uint16_t highg_ay; //14 bit signed ax [-16,16) 2 bit tilt angle + uint16_t highg_az; //14 bit signed ax [-16,16) 2 bit tilt angle + uint8_t batt_volt; + uint8_t fsm_satcount; }; // Commands transmitted from ground station to rocket