From 1e536d29c1b8e8477e3d02192664ab48f60cfdd3 Mon Sep 17 00:00:00 2001 From: anthonysmyk Date: Sat, 28 Sep 2024 16:20:13 -0500 Subject: [PATCH 1/4] WIP: added support for receiving commands on launchpad --- MIDAS/src/hardware/telemetry_backend.h | 29 +++++++++++++++++++------- MIDAS/src/systems.cpp | 11 +++++++++- MIDAS/src/telemetry.cpp | 5 +++++ MIDAS/src/telemetry.h | 2 ++ MIDAS/src/telemetry_received.h | 21 +++++++++++++++++++ 5 files changed, 60 insertions(+), 8 deletions(-) create mode 100644 MIDAS/src/telemetry_received.h diff --git a/MIDAS/src/hardware/telemetry_backend.h b/MIDAS/src/hardware/telemetry_backend.h index 4ee8ea87..f179cfb4 100644 --- a/MIDAS/src/hardware/telemetry_backend.h +++ b/MIDAS/src/hardware/telemetry_backend.h @@ -19,7 +19,7 @@ class TelemetryBackend { int8_t getRecentRssi(); void setFrequency(float frequency); - + /** * @brief This function transmits data from the struct provided as * the parameter (data collected from sensor suite) to the @@ -60,18 +60,33 @@ class TelemetryBackend { * @return bool indicating a successful read and write to buffer */ template - bool read(T* write) { + bool read(T* command, int wait_milliseconds) { static_assert(sizeof(T) <= RH_RF95_MAX_MESSAGE_LEN, "The data type to receive is too large"); uint8_t len = sizeof(T); - if (rf95.available() && rf95.recv((uint8_t*) write, &len)) { - return len == sizeof(T); - } else { - return false; + + // set receive mode + rf95.setModeRx(); + + // busy wait for interrupt signalling + for(int i = 1; i < wait_milliseconds; i++){ + THREAD_SLEEP(1); + if(digitalRead(rf95._interruptPin)){ + break; + } + } + + rf95.handleInterrupt(); + if (rf95.available() && rf95.recv((uint8_t*) command, &len)) { + if (sizeof(T) == len) { + return true; + } else { + return false; + } } + return false; } private: RH_RF95 rf95; - bool led_state; }; diff --git a/MIDAS/src/systems.cpp b/MIDAS/src/systems.cpp index 36bff5b8..d989ed00 100644 --- a/MIDAS/src/systems.cpp +++ b/MIDAS/src/systems.cpp @@ -154,7 +154,16 @@ DECLARE_THREAD(telemetry, RocketSystems* arg) { while (true) { arg->tlm.transmit(arg->rocket_data, arg->led); - THREAD_SLEEP(1); + FSMState current_state = arg->rocket_data.fsm_state.getRecentUnsync(); + if (current_state == FSMState(STATE_IDLE)) { + TelemetryCommand command; + if (arg->tlm.receive(&command, 1000)) { + // handle command + Serial.println(command.callsign); + } + } else { + THREAD_SLEEP(1); + } } } diff --git a/MIDAS/src/telemetry.cpp b/MIDAS/src/telemetry.cpp index ecd4448d..31152b01 100644 --- a/MIDAS/src/telemetry.cpp +++ b/MIDAS/src/telemetry.cpp @@ -1,6 +1,7 @@ #include #include "telemetry.h" +#include "telemetry_received.h" /** @@ -59,6 +60,10 @@ void Telemetry::transmit(RocketData& rocket_data, LEDController& led) { backend.send(packet); } +bool Telemetry::receive(TelemetryCommand* command, int wait_milliseconds) { + return backend.read(command, wait_milliseconds); +} + /** * @brief creates the packet to send through the telemetry system * diff --git a/MIDAS/src/telemetry.h b/MIDAS/src/telemetry.h index 932d5424..7364e78d 100644 --- a/MIDAS/src/telemetry.h +++ b/MIDAS/src/telemetry.h @@ -4,6 +4,7 @@ #include "rocket_state.h" #include "errors.h" #include "led.h" +#include "telemetry_received.h" #if defined(SILSIM) #include "silsim/emulated_telemetry.h" @@ -25,6 +26,7 @@ class Telemetry { ErrorCode __attribute__((warn_unused_result)) init(); void transmit(RocketData& rocket_data, LEDController& led); + bool receive(TelemetryCommand *write, int wait_milliseconds); private: TelemetryPacket makePacket(RocketData& data); diff --git a/MIDAS/src/telemetry_received.h b/MIDAS/src/telemetry_received.h new file mode 100644 index 00000000..94511f36 --- /dev/null +++ b/MIDAS/src/telemetry_received.h @@ -0,0 +1,21 @@ +#pragma once +#include + +enum class CommandType { SET_FREQ, SET_CALLSIGN, ABORT, TEST_FLAP, EMPTY , RESET_KF }; +// Commands transmitted from ground station to rocket + +/** + * @struct TelemetryCommand + * + * @brief format of the packet that telemetry receives +*/ +struct TelemetryCommand { + CommandType command; + int id; + union { + char callsign[8]; + float freq; + bool do_abort; + }; + std::array verify = {{'A', 'Y', 'B', 'E', 'R', 'K'}}; +}; \ No newline at end of file From ee9b754f7f177d580fae269481fecda042ecbff3 Mon Sep 17 00:00:00 2001 From: RishiAttarde Date: Tue, 8 Oct 2024 19:46:51 -0500 Subject: [PATCH 2/4] Added KF reset to telemetry thread --- MIDAS/src/systems.cpp | 4 ++++ MIDAS/src/systems.h | 1 + MIDAS/src/telemetry_received.h | 1 + 3 files changed, 6 insertions(+) diff --git a/MIDAS/src/systems.cpp b/MIDAS/src/systems.cpp index d989ed00..82bb58d6 100644 --- a/MIDAS/src/systems.cpp +++ b/MIDAS/src/systems.cpp @@ -159,6 +159,10 @@ DECLARE_THREAD(telemetry, RocketSystems* arg) { TelemetryCommand command; if (arg->tlm.receive(&command, 1000)) { // handle command + if (command.command == CommandType::RESET_KF && command.do_KF_reset) { + yessir.initialize(); + Serial.println("Reset KF"); + } Serial.println(command.callsign); } } else { diff --git a/MIDAS/src/systems.h b/MIDAS/src/systems.h index 301add67..bbfdb112 100644 --- a/MIDAS/src/systems.h +++ b/MIDAS/src/systems.h @@ -10,6 +10,7 @@ #include "led.h" #include "telemetry.h" #include "finite-state-machines/fsm.h" +#include "telemetry_received.h" #if defined(SILSIM) #include "silsim/emulated_sensors.h" diff --git a/MIDAS/src/telemetry_received.h b/MIDAS/src/telemetry_received.h index 94511f36..755b9008 100644 --- a/MIDAS/src/telemetry_received.h +++ b/MIDAS/src/telemetry_received.h @@ -16,6 +16,7 @@ struct TelemetryCommand { char callsign[8]; float freq; bool do_abort; + bool do_KF_reset; }; std::array verify = {{'A', 'Y', 'B', 'E', 'R', 'K'}}; }; \ No newline at end of file From 460340d07ae8fe3fa7475628369d9ee4d4bd0189 Mon Sep 17 00:00:00 2001 From: rndas27 Date: Tue, 8 Oct 2024 20:03:22 -0500 Subject: [PATCH 3/4] Added Reset_KF to Ground Station --- ground/src/feather/main.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/ground/src/feather/main.cpp b/ground/src/feather/main.cpp index 04a9a430..80463ee5 100644 --- a/ground/src/feather/main.cpp +++ b/ground/src/feather/main.cpp @@ -175,7 +175,7 @@ struct FullTelemetryData { char callsign[8]; }; -enum class CommandType { SET_FREQ, SET_CALLSIGN, ABORT, TEST_FLAP, EMPTY }; +enum class CommandType { SET_FREQ, SET_CALLSIGN, ABORT, TEST_FLAP, EMPTY, RESET_KF }; // Commands transmitted from ground station to rocket struct telemetry_command { CommandType command; @@ -184,6 +184,7 @@ struct telemetry_command { char callsign[8]; float freq; bool do_abort; + bool do_KF_reset; }; std::array verify = {{'A', 'Y', 'B', 'E', 'R', 'K'}}; }; @@ -401,6 +402,13 @@ void SerialInput(const char* key, const char* value) { return; } else if (strcmp(key, "FLAP") == 0) { command.command = CommandType::TEST_FLAP; + } else if (strcmp(key, "RESET_KF") == 0) { + command.command = CommandType::RESET_KF; + command.do_KF_reset = true; + Serial.println(json_command_success); + Serial.println(R"({"type": "reset_KF_success")"); + Serial.println("}"); + return; } else { SerialError(); return; @@ -518,6 +526,9 @@ void loop() { Serial.print(R"({"type": "freq_success", "frequency":)"); Serial.print(cmd.command.freq); Serial.println("}"); + } else if (cmd.command.command == CommandType::RESET_KF) { + Serial.println(R"({"type": "reset_KF_success")"); + Serial.println("}"); } cmd_queue.pop(); } else { From 4ea8cebb1343e5a9528e93a42f3c89a9f46be043 Mon Sep 17 00:00:00 2001 From: rndas27 Date: Tue, 8 Oct 2024 21:35:11 -0500 Subject: [PATCH 4/4] Updated MIDAS RX --- MIDAS/src/hardware/telemetry_backend.h | 2 +- MIDAS/src/systems.cpp | 4 +-- ground/lib/RadioHead/RH_RF95.cpp | 2 -- ground/src/feather/main.cpp | 49 +++++++++++++------------- 4 files changed, 28 insertions(+), 29 deletions(-) diff --git a/MIDAS/src/hardware/telemetry_backend.h b/MIDAS/src/hardware/telemetry_backend.h index f179cfb4..089e7315 100644 --- a/MIDAS/src/hardware/telemetry_backend.h +++ b/MIDAS/src/hardware/telemetry_backend.h @@ -71,11 +71,11 @@ class TelemetryBackend { for(int i = 1; i < wait_milliseconds; i++){ THREAD_SLEEP(1); if(digitalRead(rf95._interruptPin)){ + rf95.handleInterrupt(); break; } } - rf95.handleInterrupt(); if (rf95.available() && rf95.recv((uint8_t*) command, &len)) { if (sizeof(T) == len) { return true; diff --git a/MIDAS/src/systems.cpp b/MIDAS/src/systems.cpp index 82bb58d6..def373e3 100644 --- a/MIDAS/src/systems.cpp +++ b/MIDAS/src/systems.cpp @@ -1,4 +1,4 @@ -#include "systems.h" + #include "systems.h" #include "hal.h" #include "gnc/yessir.h" @@ -157,7 +157,7 @@ DECLARE_THREAD(telemetry, RocketSystems* arg) { FSMState current_state = arg->rocket_data.fsm_state.getRecentUnsync(); if (current_state == FSMState(STATE_IDLE)) { TelemetryCommand command; - if (arg->tlm.receive(&command, 1000)) { + if (arg->tlm.receive(&command, 2000)) { // handle command if (command.command == CommandType::RESET_KF && command.do_KF_reset) { yessir.initialize(); diff --git a/ground/lib/RadioHead/RH_RF95.cpp b/ground/lib/RadioHead/RH_RF95.cpp index 655bbabe..4924e46e 100644 --- a/ground/lib/RadioHead/RH_RF95.cpp +++ b/ground/lib/RadioHead/RH_RF95.cpp @@ -100,8 +100,6 @@ bool RH_RF95::setupInterruptHandler() { //were using polling so no interrupts should be setup pinMode(_interruptPin, INPUT); - return true; - // For some subclasses (eg RH_ABZ) we dont want to set up interrupt int interruptNumber = NOT_AN_INTERRUPT; diff --git a/ground/src/feather/main.cpp b/ground/src/feather/main.cpp index 80463ee5..c55923b2 100644 --- a/ground/src/feather/main.cpp +++ b/ground/src/feather/main.cpp @@ -39,7 +39,7 @@ // #define LED 13 // Blinks on receipt // Change to 434.0 or other frequency, must match RX's freq! -#define RF95_FREQ 433.0 +#define RF95_FREQ 425.15 #define DEFAULT_CMD 0 #define MAX_CMD_LEN 10 @@ -408,7 +408,6 @@ void SerialInput(const char* key, const char* value) { Serial.println(json_command_success); Serial.println(R"({"type": "reset_KF_success")"); Serial.println("}"); - return; } else { SerialError(); return; @@ -421,7 +420,7 @@ void SerialInput(const char* key, const char* value) { void process_command_queue() { if (cmd_queue.empty()) return; - + Serial.println("SENSEND"); TelemetryCommandQueueElement cmd = cmd_queue.front(); rf95.send((uint8_t*)&cmd.command, sizeof(cmd.command)); rf95.waitPacketSent(); @@ -454,6 +453,11 @@ void setup() { Serial.print(RF95_FREQ); Serial.println("}"); + rf95.setSignalBandwidth(125000); + rf95.setCodingRate4(8); + rf95.setSpreadingFactor(10); + rf95.setPayloadCRC(true); + // Defaults after init are 434.0MHz, 13dBm, Bw = 125 kHz, Cr = 4/5, Sf = // 128chips/symbol, CRC on @@ -518,27 +522,24 @@ void loop() { memcpy(&packet, buf, sizeof(packet)); EnqueuePacket(packet, current_freq); - if (!cmd_queue.empty()) { - auto& cmd = cmd_queue.front(); - if (cmd.command.id == packet.response_ID) { - if (cmd.command.command == CommandType::SET_FREQ) { - set_freq_local_bug_fix(cmd.command.freq); - Serial.print(R"({"type": "freq_success", "frequency":)"); - Serial.print(cmd.command.freq); - Serial.println("}"); - } else if (cmd.command.command == CommandType::RESET_KF) { - Serial.println(R"({"type": "reset_KF_success")"); - Serial.println("}"); - } - cmd_queue.pop(); - } else { - cmd.retry_count++; - if (cmd.retry_count >= max_command_retries) { - cmd_queue.pop(); - Serial.println(json_send_failure); - } - } - } + // if (!cmd_queue.empty()) { + // auto& cmd = cmd_queue.front(); + // if (cmd.command.id == packet.response_ID) { + // if (cmd.command.command == CommandType::SET_FREQ) { + // set_freq_local_bug_fix(cmd.command.freq); + // Serial.print(R"({"type": "freq_success", "frequency":)"); + // Serial.print(cmd.command.freq); + // Serial.println("}"); + // } + // cmd_queue.pop(); + // } else { + // cmd.retry_count++; + // if (cmd.retry_count >= max_command_retries) { + // cmd_queue.pop(); + // Serial.println(json_send_failure); + // } + // } + // } process_command_queue();