MIDAS commanding from feather

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<typename T>
- 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)){
+ rf95.handleInterrupt();
+ break;
+ }
  }
+
+ 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;
 };

MIDAS/src/systems.cpp

@@ -1,4 +1,4 @@
-#include "systems.h"
+ #include "systems.h"
 
 #include "hal.h"
 #include "gnc/yessir.h"
@@ -154,7 +154,20 @@ 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, 2000)) {
+ // handle command
+ if (command.command == CommandType::RESET_KF && command.do_KF_reset) {
+ yessir.initialize();
+ Serial.println("Reset KF");
+ }
+ Serial.println(command.callsign);
+ }
+ } else {
+ THREAD_SLEEP(1);
+ }
  }
 }
 

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"

MIDAS/src/telemetry.cpp

@@ -1,6 +1,7 @@
 #include <cmath>
 
 #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
  * 

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);
 

MIDAS/src/telemetry_received.h

@@ -0,0 +1,22 @@
+#pragma once
+#include <array>
+
+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;
+ bool do_KF_reset;
+ };
+ std::array<char, 6> verify = {{'A', 'Y', 'B', 'E', 'R', 'K'}};
+};

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;

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
@@ -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<char, 6> verify = {{'A', 'Y', 'B', 'E', 'R', 'K'}};
 };
@@ -401,6 +402,12 @@ 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("}");
  } else {
  SerialError();
  return;
@@ -413,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();
@@ -446,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
 
@@ -510,24 +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("}");
- }
- 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();