add fsm ability!

Author: mpkarpov-ui

MIDAS/src/hilsim/kal.cpp

@@ -27,14 +27,16 @@ void k_init_sensordata() {
 }
 
 // ---- Kamaji Thread ----
-DECLARE_THREAD(hilsim, void*arg) {
+DECLARE_THREAD(hilsim, RocketSystems* arg) {
     uint8_t header[5];
     uint8_t crc_buf[2];
     uint32_t timestamp;
     uint8_t discriminant;
     size_t data_size;
     uint8_t data_buf[128];
     uint16_t crc;
+
+    sys_flags_t sflags;
 
     while (true) {
         int delim;
@@ -43,14 +45,19 @@ DECLARE_THREAD(hilsim, void*arg) {
             delim = Serial.read();
             THREAD_SLEEP(1);
 
+            if (sflags.fsm_target != FSMState::FSM_STATE_COUNT) {
+                arg->rocket_data.fsm_state.update(sflags.fsm_target);
+                sflags.fsm_target = FSMState::FSM_STATE_COUNT;
+            }
+
         } while(delim != '$' && delim != '#');
 
         if(delim == '#') {
           if (!k_read_exact(data_buf, 16, 10)) continue;
 
           // Sys message is 14 byte data, 2 byte crc  
           memcpy(&crc, data_buf + 14, sizeof(crc_buf));
-          k_handle_sys_msg(data_buf, crc);
+          k_handle_sys_msg(data_buf, crc, &sflags);
           continue;
         }
 
@@ -107,7 +114,7 @@ DECLARE_THREAD(hilsim, void*arg) {
     START_THREAD(fsm, SENSOR_CORE, config, 8);
     START_THREAD(buzzer, SENSOR_CORE, config, 6);
     START_THREAD(telemetry, SENSOR_CORE, config, 15);
-    START_THREAD(hilsim, DATA_CORE, nullptr, 1);
+    START_THREAD(hilsim, DATA_CORE, config, 1);
 
     config->buzzer.play_tune(free_bird, FREE_BIRD_LENGTH);
 

MIDAS/src/hilsim/kal.h

@@ -49,14 +49,23 @@ enum sys_instr_t {
     RESERVED = 0,
     REPORT_EN = 1,
     VERIFY_CHECKSUM = 2,
+    FSM_SET = 3,
 };
 
+struct sys_flags_t {
+    FSMState fsm_target = FSMState::FSM_STATE_COUNT;
+};
+
+
+
 // System message: instruct the Kamaji driver to do something
 // # <INSTR> <data[13]>
 // INSTR
 // - 0x00 --> reserved
 // - 0x01 --> REPORT_EN
-inline void k_handle_sys_msg(uint8_t* data, uint16_t crc) {
+// - 0x02 --> VERIFY_CHECKSUM
+// - 0x03 --> FSM_SET
+inline void k_handle_sys_msg(uint8_t* data, uint16_t crc, sys_flags_t* sflags) {
     sys_instr_t instr = (sys_instr_t) data[0];
 
     switch(instr) {
@@ -77,6 +86,12 @@ inline void k_handle_sys_msg(uint8_t* data, uint16_t crc) {
                 }
             }
             break;
+        case (sys_instr_t::FSM_SET): 
+            {
+            FSMState new_fsm = (FSMState) data[1];
+            sflags->fsm_target = new_fsm;
+            }
+            break;
         default:
             k_INVALIDINSTR();
             return;
@@ -99,7 +114,7 @@ inline void k_clear_inbuf() {
 }
 
 // ---- Kamaji Thread ----
-void hilsim_thread(void* arg);
+void hilsim_thread(RocketSystems* arg);
 
 // Run the Kamaji process
 [[noreturn]] void k_start();

MIDAS/src/hilsim/stream/main.cpp

@@ -158,7 +158,29 @@ class InputReader {
     }
 };
 
+enum sys_instr_t {
+    RESERVED = 0,
+    REPORT_EN = 1,
+    VERIFY_CHECKSUM = 2,
+    FSM_SET = 3,
+};
+
+// System message: instruct the Kamaji driver to do something
+// # <INSTR> <data[13]>
+void send_sys_msg(serialib& s, sys_instr_t instr_t, uint8_t* dat, size_t dat_size) {
+    
+    uint8_t send_buf[17];
+    uint8_t instr_as_byte = (uint8_t) instr_t;
+    send_buf[0] = '#';
+    memcpy(send_buf + 1, &instr_as_byte, 1);
+    if (dat_size > 0) { memcpy(send_buf + 2, dat, dat_size); }
+    s.writeBytes(send_buf, sizeof(send_buf));
+
+}
 
+void set_fsm(serialib& s, uint8_t fsm_state) {
+    send_sys_msg(s, sys_instr_t::FSM_SET, &fsm_state, 1);
+}
 
 void send_data(serialib& s, entry_t& dat) {
     // $ 4 bytes ts, 1 byte disc, n bytes payload, 2 bytes crc
@@ -222,6 +244,14 @@ int main(int argc, char** argv) {
         if(Serial.isDeviceOpen()) {
             // do something
 
+            int av = Serial.available();
+            if (av) {
+                uint8_t buf[256];
+                Serial.readBytes(buf, 255);
+                buf[255] = '\0';
+                printf("from thingy: %s", (char*)buf);
+            }
+
             // TBD
         }
 
@@ -252,8 +282,27 @@ int main(int argc, char** argv) {
         // T <filter_thresh:float> - Entries will be probabilistically skipped if there is high latency. This sets the threshold (in seconds) at which 100% will be skipped.
         //    If unset / 0, then no entries are skipped
 
+        // F <state_id:int> - Set FSM state directly.
+
 
         switch(_inbuf[0]) {
+            case 'F':
+                    {
+                    uint8_t fsm_int;
+                    sscanf(_inbuf + 1, " %i", &fsm_int);
+
+                    if (!Serial.isDeviceOpen()) {
+                        std::cerr << "No serial open" << std::endl;
+                        fflush(stdout);
+                        break;
+                    }
+
+                    set_fsm(Serial, fsm_int);                    
+                    printf(".FSM %i\n", fsm_int);
+                    fflush(stdout);
+                }
+
+                break;
             case 'i':
                     {
                     int discrim_int;