midas reset handling

Author: NPIPHI

MIDAS/src/gnc/yessir.h

@@ -25,7 +25,7 @@ class Yessir : public KalmanFilter<NUM_STATES, NUM_SENSOR_INPUTS>
 
     void tick(float dt, float sd, Barometer &barometer, Acceleration acceleration, Orientation &orientation, FSMState state);
 
-
+    bool should_reinit = false;
 private:
     float s_dt = 0.05f;
     float spectral_density_ = 13.0f;

MIDAS/src/hardware/telemetry_backend.h

@@ -60,14 +60,30 @@ class TelemetryBackend {
      * @return bool indicating a successful read and write to buffer
     */
     template<typename T>
-    bool read(T* write) {
+    bool read(T* write, 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);
+
+        // 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*) write, &len)) {
-            return len == sizeof(T);
-        } else {
-            return false;
+            if (sizeof(T) == len) {
+                return true;
+            } else {
+                return false;
+            }
         }
+        return false;
     }
 
 private:

MIDAS/src/systems.cpp

@@ -133,6 +133,11 @@ DECLARE_THREAD(kalman, RocketSystems* arg) {
     TickType_t last = xTaskGetTickCount();
 
     while (true) {
+        if(yessir.should_reinit){
+            yessir.initialize(arg);
+            TickType_t last = xTaskGetTickCount();
+            yessir.should_reinit = false;
+        }
         // add the tick update function
         Barometer current_barom_buf = arg->rocket_data.barometer.getRecent();
         Orientation current_orientation = arg->rocket_data.orientation.getRecent();
@@ -159,8 +164,26 @@ DECLARE_THREAD(kalman, RocketSystems* arg) {
 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)) {
+                if(command.valid()) {
+                    arg->tlm.acknowledgeReceived();
+                    switch(command.command) {
+                        case CommandType::RESET_KF:
+                            yessir.should_reinit = true;
+                            break;
+                        default:
+                            break; 
+                    }
+                }
+
+            }
+        } else {
+            THREAD_SLEEP(1);
+        }
     }
 }
 

MIDAS/src/telemetry.cpp

@@ -59,6 +59,14 @@ 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);
+}
+
+void Telemetry::acknowledgeReceived() {
+    received_count ++;
+}
+
 /**
  * @brief creates the packet to send through the telemetry system
  * 
@@ -78,8 +86,7 @@ TelemetryPacket Telemetry::makePacket(RocketData& data) {
 
     packet.lat = gps.latitude;
     packet.lon = gps.longitude;
-    packet.alt = (int16_t) gps.altitude;
-    // Convert range of value so that we can also account for negative altitudes
+    packet.alt = (((int16_t) gps.altitude) & 0xfffe) | (received_count & 0x0001);    // Convert range of value so that we can also account for negative altitudes
     packet.baro_alt = inv_convert_range<int16_t>(barometer.altitude, 1 << 17);
 
     auto [ax,ay,az] = pack_highg_tilt(highg, map(static_cast<long>(orientation.tilt * 100),0, 314, 0, 63));

MIDAS/src/telemetry.h

@@ -26,7 +26,10 @@ class Telemetry {
     ErrorCode __attribute__((warn_unused_result)) init();
 
     void transmit(RocketData& rocket_data, LEDController& led);
+    bool receive(TelemetryCommand* command, int wait_milliseconds);
+    void acknowledgeReceived();
 private:
+    int received_count;
     TelemetryPacket makePacket(RocketData& data);
 
     TelemetryBackend backend;

MIDAS/src/telemetry_packet.h

@@ -11,7 +11,7 @@
 struct TelemetryPacket {
     int32_t lat;
     int32_t lon;
-    uint16_t alt;
+    uint16_t alt; //15 bit meters, 1 bit command ack
     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
@@ -25,3 +25,21 @@ struct TelemetryPacket {
 
     uint8_t fsm_callsign_satcount; //4 bit fsm state, 1 bit is_sustainer_callsign, 3 bits sat count
 };
+
+
+// Commands transmitted from ground station to rocket
+enum class CommandType: uint8_t { RESET_KF };
+
+/**
+ * @struct TelemetryCommand
+ * 
+ * @brief format of the packet that telemetry receives
+*/
+struct TelemetryCommand {
+    CommandType command;
+    std::array<char, 3> verify = {{'B', 'R', 'K'}};
+
+    bool valid() {
+        return verify == std::array<char, 3>{{'B','R','K'}};
+    }
+};