Simulator: Input, outputs and FSM

8 files changed, 786 insertions, 175 deletions

diff --git a/src/common/includes/Core/delay.h b/src/common/includes/Core/delay.h
new file mode 100644
index 0000000..73ed669
--- /dev/null
+++ b/src/common/includes/Core/delay.h
@@ -0,0 +1,33 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2016 Matthias P. Braendli, Maximilien Cuony
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+*/
+
+
+// High-precisions delay (with approximations)
+
+
+// Thoses functions works only if interupts are disabled
+void delay_us(uint32_t micros);
+void delay_ms(uint32_t millis);
+
+void delay_init();
diff --git a/src/common/includes/Core/fsm.h b/src/common/includes/Core/fsm.h
new file mode 100644
index 0000000..41f0503
--- /dev/null
+++ b/src/common/includes/Core/fsm.h
@@ -0,0 +1,103 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2016 Matthias P. Braendli
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+*/
+
+#ifndef _FSM_H_
+#define _FSM_H_
+
+// List of all states the FSM of the relay can be in
+enum fsm_state_e {
+    FSM_OISIF = 0,       // Idle
+    FSM_OPEN1,           // 1750 Hz received and squelch open
+    FSM_OPEN2,           // Squelch closed
+    FSM_LETTRE,          // Transmit single status letter
+    FSM_ECOUTE,          // Repeater open, waiting for QSO
+    FSM_ATTENTE,         // No QSO after a short while
+    FSM_QSO,             // QSO ongoing
+    FSM_ANTI_BAVARD,     // QSO too long, cut transmission
+    FSM_BLOQUE,          // Backoff after ANTI_BAVARD
+    FSM_TEXTE_73,        // Transmit 73 after QSO
+    FSM_TEXTE_HB9G,      // Transmit HB9G after QSO
+    FSM_TEXTE_LONG,      // Transmit either HB9G JN36BK or HB9G 1628M after QSO
+    FSM_BALISE_LONGUE,   // Full-length 2-hour beacon
+    FSM_BALISE_SPECIALE, // 2-hour beacon when in QRP or with high power return mode
+    FSM_BALISE_COURTE,   // Short intermittent beacon
+    _NUM_FSM_STATES      // Dummy state to count the number of states
+};
+
+typedef enum fsm_state_e fsm_state_t;
+
+// All signals that the FSM can read, most of them are actually booleans
+struct fsm_input_signals_t {
+    /* Signals coming from repeater electronics */
+    int sq;                // Squelch detection
+    int discrim_u;         // FM discriminator says RX is too high in frequency
+    int qrp;               // The relay is currently running with low power
+    int start_tm;          // 2-hour pulse
+    float temp;            // temperature in degrees C
+    float humidity;        // relative humidity, range [0-100] %
+    int wind_generator_ok; // false if the generator is folded out of the wind
+    int discrim_d;         // FM discriminator says RX is too low in frequency
+    int tone_1750;         // Detect 1750Hz tone
+    int sstv_mode;         // The 1750Hz filter is disabled, permitting SSTV usage
+
+    /* Signals coming from CW and PSK generator */
+    int cw_psk31_done;     // The CW and PSK generator has finished transmitting the message
+
+    /* Signal coming from the standing wave ratio meter */
+    int swr_high;          // We see a lot of return power
+
+};
+
+// All signals the FSM has to control
+struct fsm_output_signals_t {
+    /* Signals to the repeater electronics */
+    int qrp;               // Place the repeater in QRP mode // TODO move out of FSM
+    int tx_on;             // Enable TX circuitry
+    int modulation;        // Enable repeater RX to TX modulation
+
+    /* Signals to the CW and PSK generator */
+    const char* msg;       // The message to transmit
+    int msg_frequency;     // What audio frequency for the CW or PSK message
+    int cw_dit_duration;   // CW speed, dit duration in ms
+    int cw_psk31_trigger;  // Set to true to trigger a CW or PSK31 transmission.
+                           // PSK31 is sent if cw_dit_duration is 0
+
+    /* Acknowledgements for input signals */
+    int ack_start_tm;      // Set to 1 to clear start_tm
+};
+
+// Initialise local structures
+void fsm_init();
+
+// Call the FSM once and update the internal state
+void fsm_update();
+
+// Setter for inputs
+void fsm_update_inputs(struct fsm_input_signals_t* inputs);
+
+// Getter for outputs
+void fsm_get_outputs(struct fsm_output_signals_t* out);
+
+#endif // _FSM_H_
+
diff --git a/src/common/includes/GPIO/pio.h b/src/common/includes/GPIO/pio.h
new file mode 100644
index 0000000..19d3e15
--- /dev/null
+++ b/src/common/includes/GPIO/pio.h
@@ -0,0 +1,47 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2015 Matthias P. Braendli
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+*/
+
+#ifndef _PIO_H_
+#define _PIO_H_
+
+#include <stddef.h>
+#include <stdint.h>
+#include "Core/fsm.h"
+
+/* Analog inputs */
+// TODO: SWR forward power
+// TODO: SWR reflected power
+
+void pio_init(void);
+
+void pio_set_tx(int on);
+void pio_set_mod_off(int mod_off);
+void pio_set_qrp(int on);
+
+void pio_set_fsm_signals(struct fsm_input_signals_t* sig);
+
+int pio_read_button();
+
+#endif // _PIO_H_
+
diff --git a/src/common/includes/GPIO/usart.h b/src/common/includes/GPIO/usart.h
index 62c86c9..21936d7 100644
--- a/src/common/includes/GPIO/usart.h
+++ b/src/common/includes/GPIO/usart.h
@@ -48,6 +48,7 @@ void usart_debug(const char *format, ...);
 
 // Send a string to the PC
 void usart_debug_puts(const char* str);
+void usart_debug_puts_no_header(const char* str);
 
 // Get a MAX_NMEA_SENTENCE_LEN sized NMEA sentence
 // Return 1 on success
diff --git a/src/common/src/Core/common.c b/src/common/src/Core/common.c
index 476d1b4..2077a16 100644
--- a/src/common/src/Core/common.c
+++ b/src/common/src/Core/common.c
@@ -158,6 +158,11 @@ void common_init(void)
 static void common_increase_timestamp(TimerHandle_t t)
 {
     common_timestamp++;
+
+#ifdef SIMULATOR
+    //Simulator rate is 100ticks/s
+    common_timestamp += 9;
+#endif
 }
 
 uint64_t timestamp_now(void)
diff --git a/src/common/src/Core/fsm.c b/src/common/src/Core/fsm.c
new file mode 100644
index 0000000..e5d7ca7
--- /dev/null
+++ b/src/common/src/Core/fsm.c
@@ -0,0 +1,414 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2016 Matthias P. Braendli
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+*/
+
+#include <string.h>
+#include "Core/common.h"
+#include "Core/fsm.h"
+#include "GPIO/usart.h"
+
+static struct fsm_input_signals_t fsm_in;
+static struct fsm_output_signals_t fsm_out;
+
+static fsm_state_t current_state;
+
+// Keep track of when we last entered a given state, measured
+// in ms using the timestamp_now() function
+static uint64_t timestamp_state[_NUM_FSM_STATES];
+
+
+void fsm_state_switched(char * new_state);
+
+void fsm_init() {
+    memset(&fsm_in, 0, sizeof(fsm_in));
+    memset(&fsm_out, 0, sizeof(fsm_out));
+
+    memset(timestamp_state, 0, _NUM_FSM_STATES * sizeof(*timestamp_state));
+    timestamp_state[FSM_OISIF] = timestamp_now();
+
+    current_state = FSM_OISIF;
+}
+
+// Calculate the time spent in the current state
+uint64_t fsm_current_state_time_ms(void) {
+    return timestamp_now() - timestamp_state[current_state];
+}
+
+uint64_t fsm_current_state_time_s(void) {
+    return fsm_current_state_time_ms() / 1000;
+}
+
+// Between turns in a QSO, the repeater sends a letter in CW,
+// different messages are possible. They are sorted here from
+// low to high priority.
+const char* letter_all_ok    = "K";
+const char* letter_sstv      = "S";
+const char* letter_qrp       = "G";
+const char* letter_freq_high = "U";
+const char* letter_freq_low  = "D";
+const char* letter_swr_high  = "R";
+
+const char* fsm_select_letter(void) {
+    if (fsm_in.swr_high) {
+        return letter_swr_high;
+    }
+    else if (fsm_in.discrim_d) {
+        return letter_freq_low;
+    }
+    else if (fsm_in.discrim_u) {
+        return letter_freq_high;
+    }
+    else if (fsm_in.qrp) {
+        return letter_qrp;
+    }
+    else if (fsm_in.sstv_mode) {
+        return letter_sstv;
+    }
+
+    return letter_all_ok;
+}
+
+void fsm_update() {
+
+    fsm_state_t next_state = current_state;
+
+    // Some defaults for the outgoing signals
+    fsm_out.tx_on = 0;
+    fsm_out.modulation = 0;
+    fsm_out.cw_psk31_trigger = 0;
+    fsm_out.cw_dit_duration = 50;
+    fsm_out.msg_frequency = 960;
+    // other output signals keep their value
+
+    switch (current_state) {
+        case FSM_OISIF:
+            if (fsm_in.tone_1750 && fsm_in.sq) {
+                next_state = FSM_OPEN1;
+            }
+            else if (fsm_in.start_tm) {
+                if (fsm_in.qrp || fsm_in.swr_high) {
+                    next_state = FSM_BALISE_SPECIALE;
+                }
+                else {
+                    next_state = FSM_BALISE_LONGUE;
+                }
+            }
+            else if (!fsm_in.qrp && fsm_current_state_time_s() > 20 * 60) {
+                next_state = FSM_BALISE_COURTE;
+            }
+            break;
+
+        case FSM_OPEN1:
+            fsm_out.tx_on = 1;
+
+            if (!fsm_in.sq) {
+                next_state = FSM_OPEN2;
+            }
+            break;
+
+        case FSM_OPEN2:
+            fsm_out.tx_on = 1;
+            fsm_out.modulation = 1;
+
+            if (fsm_current_state_time_ms() > 200) {
+                next_state = FSM_LETTRE;
+            }
+            break;
+
+        case FSM_LETTRE:
+            fsm_out.tx_on = 1;
+            fsm_out.modulation = 1;
+            fsm_out.msg = fsm_select_letter();
+            if (fsm_out.msg[0] == 'G') {
+                // The letter 'G' is a bit different
+                fsm_out.msg_frequency    = 696;
+                fsm_out.cw_dit_duration = 70;
+            }
+            fsm_out.cw_psk31_trigger = 1;
+
+            if (fsm_in.cw_psk31_done) {
+                next_state = FSM_ECOUTE;
+            }
+            break;
+
+        case FSM_ECOUTE:
+            fsm_out.tx_on = 1;
+            fsm_out.modulation = 1;
+
+            if (fsm_in.sq) {
+                next_state = FSM_QSO;
+            }
+            else if (fsm_current_state_time_s() > 6 &&
+                    timestamp_state[FSM_ECOUTE] - timestamp_state[FSM_OPEN2] <
+                        1000ul * 5) {
+                next_state = FSM_ATTENTE;
+            }
+            else if (fsm_current_state_time_s() > 5 &&
+                    timestamp_state[FSM_ECOUTE] - timestamp_state[FSM_OPEN2] <
+                        1000ul * 5 * 60) {
+                next_state = FSM_OISIF;
+            }
+            else if (fsm_current_state_time_s() > 5 &&
+                    timestamp_state[FSM_ECOUTE] - timestamp_state[FSM_OPEN2] <
+                        1000ul * 10 * 60) {
+                next_state = FSM_TEXTE_73;
+            }
+            else if (fsm_current_state_time_s() > 5 &&
+                    timestamp_state[FSM_ECOUTE] - timestamp_state[FSM_OPEN2] <
+                        1000ul * 15 * 60) {
+                next_state = FSM_TEXTE_HB9G;
+            }
+            else if (fsm_current_state_time_s() > 5 &&
+                    timestamp_state[FSM_ECOUTE] - timestamp_state[FSM_OPEN2] >=
+                        1000ul * 15 * 60) {
+                next_state = FSM_TEXTE_LONG;
+            }
+            break;
+
+        case FSM_ATTENTE:
+            if (fsm_in.sq) {
+                next_state = FSM_ECOUTE;
+            }
+            else if (fsm_current_state_time_s() > 15) {
+                next_state = FSM_OISIF;
+            }
+            break;
+
+        case FSM_QSO:
+            fsm_out.tx_on = 1;
+            fsm_out.modulation = 1;
+
+            if (!fsm_in.sq) {
+                next_state = FSM_LETTRE;
+            }
+            else if (fsm_current_state_time_s() > 5 * 60) {
+                next_state = FSM_ANTI_BAVARD;
+            }
+            break;
+
+        case FSM_ANTI_BAVARD:
+            fsm_out.tx_on = 1;
+            // No modulation!
+            fsm_out.msg = "HI HI";
+            fsm_out.cw_psk31_trigger = 1;
+
+            if (fsm_in.cw_psk31_done) {
+                next_state = FSM_BLOQUE;
+            }
+            break;
+
+        case FSM_BLOQUE:
+            if (fsm_current_state_time_s() > 10) {
+                next_state = FSM_OISIF;
+            }
+            break;
+
+        case FSM_TEXTE_73:
+            fsm_out.tx_on = 1;
+            fsm_out.modulation = 1;
+            fsm_out.msg_frequency    = 696;
+            fsm_out.cw_dit_duration = 70;
+            fsm_out.msg = "73";
+            fsm_out.cw_psk31_trigger = 1;
+
+            if (fsm_in.sq) {
+                next_state = FSM_QSO;
+            }
+            else if (fsm_in.cw_psk31_done) {
+                next_state = FSM_OISIF;
+            }
+            break;
+
+        case FSM_TEXTE_HB9G:
+            fsm_out.tx_on = 1;
+            fsm_out.modulation = 1;
+            fsm_out.msg_frequency   = 696;
+            fsm_out.cw_dit_duration = 70;
+            fsm_out.msg = "HB9G";
+            fsm_out.cw_psk31_trigger = 1;
+
+            if (fsm_in.sq) {
+                next_state = FSM_QSO;
+            }
+            else if (fsm_in.cw_psk31_done) {
+                next_state = FSM_OISIF;
+            }
+            break;
+
+        case FSM_TEXTE_LONG:
+            fsm_out.tx_on = 1;
+            fsm_out.modulation = 1;
+
+            fsm_out.msg_frequency   = 696;
+            fsm_out.cw_dit_duration = 70;
+
+            if (random_bool()) {
+                fsm_out.msg = "HB9G 1628M";
+            }
+            else {
+                fsm_out.msg = "HB9G JN36BK";
+            }
+            fsm_out.cw_psk31_trigger = 1;
+
+            if (fsm_in.sq) {
+                next_state = FSM_QSO;
+            }
+            else if (fsm_in.cw_psk31_done) {
+                next_state = FSM_OISIF;
+            }
+            break;
+
+        case FSM_BALISE_LONGUE:
+            fsm_out.tx_on = 1;
+            fsm_out.msg_frequency   = 588;
+            fsm_out.cw_dit_duration = 110;
+            fsm_out.ack_start_tm    = 1;
+
+            // TODO transmit humidity
+            // TODO read voltage
+            if (fsm_in.wind_generator_ok) {
+                fsm_out.msg = "HB9G JN36BK 1628M U 10V5 =  T 11  73";
+                // = means same voltage as previous
+                // + means higher
+                // - means lower
+            }
+            else {
+                fsm_out.msg = "HB9G JN36BK 1628M U 10V5 =  T 11  #";
+                // The # is the SK digraph
+            }
+            fsm_out.cw_psk31_trigger = 1;
+
+            if (fsm_in.cw_psk31_done) {
+                next_state = FSM_OISIF;
+            }
+            break;
+
+        case FSM_BALISE_SPECIALE:
+            fsm_out.tx_on = 1;
+            fsm_out.msg_frequency   = 696;
+            fsm_out.cw_dit_duration = 70;
+            fsm_out.ack_start_tm    = 1;
+
+            // TODO read voltage
+            if (fsm_in.wind_generator_ok) {
+                fsm_out.msg = "HB9G U 10V5 73";
+            }
+            else {
+                fsm_out.msg = "HB9G U 10V5 #"; // The # is the SK digraph
+            }
+            fsm_out.cw_psk31_trigger = 1;
+
+            if (fsm_in.cw_psk31_done) {
+                next_state = FSM_OISIF;
+            }
+            break;
+
+        case FSM_BALISE_COURTE:
+            fsm_out.tx_on = 1;
+
+            fsm_out.msg_frequency   = 696;
+            fsm_out.cw_dit_duration = 70;
+
+            {
+                int rand = random_bool() * 2 + random_bool();
+
+                if (rand == 0) {
+                    fsm_out.msg = "HB9G";
+                }
+                else if (rand == 1) {
+                    fsm_out.msg = "HB9G JN36BK";
+                }
+                else if (rand == 2) {
+                    fsm_out.msg = "HB9G 1628M";
+                }
+                else {
+                    fsm_out.msg = "HB9G JN36BK 1628M";
+                }
+            }
+            fsm_out.cw_psk31_trigger = 1;
+
+            if (fsm_in.sq) {
+                next_state = FSM_OPEN2;
+            }
+            else if (fsm_in.cw_psk31_done) {
+                next_state = FSM_OISIF;
+            }
+            break;
+        default:
+        // Should never happen
+        next_state = FSM_OISIF;
+            break;
+    }
+
+
+    if (next_state != current_state) {
+        timestamp_state[next_state] = timestamp_now();
+        switch (next_state) {
+            case FSM_OISIF:
+                fsm_state_switched("FSM_OISIF"); break;
+            case FSM_OPEN1:
+                fsm_state_switched("FSM_OPEN1"); break;
+            case FSM_OPEN2:
+                fsm_state_switched("FSM_OPEN2"); break;
+            case FSM_LETTRE:
+                fsm_state_switched("FSM_LETTRE"); break;
+            case FSM_ECOUTE:
+                fsm_state_switched("FSM_ECOUTE"); break;
+            case FSM_ATTENTE:
+                fsm_state_switched("FSM_ATTENTE"); break;
+            case FSM_QSO:
+                fsm_state_switched("FSM_QSO"); break;
+            case FSM_ANTI_BAVARD:
+                fsm_state_switched("FSM_ANTI_BAVARD"); break;
+            case FSM_BLOQUE:
+                fsm_state_switched("FSM_BLOQUE"); break;
+            case FSM_TEXTE_73:
+                fsm_state_switched("FSM_TEXTE_73"); break;
+            case FSM_TEXTE_HB9G:
+                fsm_state_switched("FSM_TEXTE_HB9G"); break;
+            case FSM_TEXTE_LONG:
+                fsm_state_switched("FSM_TEXTE_LONG"); break;
+            case FSM_BALISE_LONGUE:
+                fsm_state_switched("FSM_BALISE_LONGUE"); break;
+            case FSM_BALISE_SPECIALE:
+                fsm_state_switched("FSM_BALISE_SPECIALE"); break;
+            case FSM_BALISE_COURTE:
+                fsm_state_switched("FSM_BALISE_COURTE"); break;
+            default:
+                fsm_state_switched("ERROR!"); break;
+        }
+    }
+    current_state = next_state;
+}
+
+void fsm_update_inputs(struct fsm_input_signals_t* inputs)
+{
+    fsm_in = *inputs;
+}
+
+void fsm_get_outputs(struct fsm_output_signals_t* out)
+{
+    *out = fsm_out;
+}
+
+
diff --git a/src/common/src/Core/main.c b/src/common/src/Core/main.c
index f1d8e61..3e99a11 100644
--- a/src/common/src/Core/main.c
+++ b/src/common/src/Core/main.c
@@ -36,13 +36,13 @@
 /* Includes */
 #include "Audio/audio.h"
 #include "Audio/cw.h"
-/* #include "pio.h" */
-/* #include "i2c.h" */
+#include "GPIO/pio.h"
+#include "GPIO/i2c.h"
 #include "GPS/gps.h"
-/* #include "fsm.h" */
+#include "Core/fsm.h"
 #include "Core/common.h"
 #include "GPIO/usart.h"
-/* #include "delay.h" */
+#include "Core/delay.h"
 #include "GPIO/temperature.h"
 #include "GPIO/leds.h"
 #include "vc.h"
@@ -78,16 +78,20 @@ void * threadscheduler(void * arg) {
 
 int main(void) {
     init();
-    /* delay_init(); */
+    delay_init();
     usart_init();
     usart_debug_puts("\r\n******* glutt-o-matique version " GIT_VERSION " *******\r\n");
-    /*  */
-    /* if (RCC_GetFlagStatus(RCC_FLAG_IWDGRST) != RESET) */
-    /* { */
-    /*     usart_debug_puts("WARNING: A IWDG Reset occured!\r\n"); */
-    /* } */
-    /* RCC_ClearFlag(); */
-    /*  */
+
+#ifndef SIMULATOR
+
+    if (RCC_GetFlagStatus(RCC_FLAG_IWDGRST) != RESET)
+    {
+        usart_debug_puts("WARNING: A IWDG Reset occured!\r\n");
+    }
+    RCC_ClearFlag();
+
+#endif
+
     TaskHandle_t task_handle;
     xTaskCreate(
             launcher_task,
@@ -134,13 +138,13 @@ static void launcher_task(void *pvParameters)
 {
     usart_debug_puts("CW init\r\n");
     cw_psk31_init(16000);
-    /*  */
-    /* usart_debug_puts("PIO init\r\n"); */
-    /* pio_init(); */
-    /*  */
-    /* usart_debug_puts("I2C init\r\n"); */
-    /* i2c_init(); */
-    /*  */
+
+    usart_debug_puts("PIO init\r\n");
+    pio_init();
+
+    usart_debug_puts("I2C init\r\n");
+    i2c_init();
+
     usart_debug_puts("common init\r\n");
     common_init();
 
@@ -150,35 +154,35 @@ static void launcher_task(void *pvParameters)
     usart_debug_puts("DS18B20 init\r\n");
     temperature_init();
 
-    /* usart_debug_puts("TaskButton init\r\n"); */
+    usart_debug_puts("TaskButton init\r\n");
 
     TaskHandle_t task_handle;
-    /* xTaskCreate( */
-    /*         detect_button_press, */
-    /*         "TaskButton", */
-    /*         4*configMINIMAL_STACK_SIZE, */
-    /*         (void*) NULL, */
-    /*         tskIDLE_PRIORITY + 2UL, */
-    /*         &task_handle); */
-    /*  */
-    /* if (!task_handle) { */
-    /*     trigger_fault(FAULT_SOURCE_MAIN); */
-    /* } */
-    /*  */
-    /* usart_debug_puts("TaskFSM init\r\n"); */
-    /*  */
-    /* xTaskCreate( */
-    /*         exercise_fsm, */
-    /*         "TaskFSM", */
-    /*         4*configMINIMAL_STACK_SIZE, */
-    /*         (void*) NULL, */
-    /*         tskIDLE_PRIORITY + 2UL, */
-    /*         &task_handle); */
-    /*  */
-    /* if (!task_handle) { */
-    /*     trigger_fault(FAULT_SOURCE_MAIN); */
-    /* } */
-    /*  */
+    xTaskCreate(
+            detect_button_press,
+            "TaskButton",
+            4*configMINIMAL_STACK_SIZE,
+            (void*) NULL,
+            tskIDLE_PRIORITY + 2UL,
+            &task_handle);
+
+    if (!task_handle) {
+        trigger_fault(FAULT_SOURCE_MAIN);
+    }
+
+    usart_debug_puts("TaskFSM init\r\n");
+
+    xTaskCreate(
+            exercise_fsm,
+            "TaskFSM",
+            4*configMINIMAL_STACK_SIZE,
+            (void*) NULL,
+            tskIDLE_PRIORITY + 2UL,
+            &task_handle);
+
+    if (!task_handle) {
+        trigger_fault(FAULT_SOURCE_MAIN);
+    }
+
     usart_debug_puts("TaskGPS init\r\n");
 
     xTaskCreate(
@@ -222,10 +226,6 @@ static void launcher_task(void *pvParameters)
      * for more info.
      */
 
-    cw_psk31_push_message("HB9G HI - TESTING", 50, 440);
-    cw_psk31_push_message("HB9G HI AGAIN", 50, 440);
-    cw_psk31_push_message("HB9G 73", 50, 440);
-
     while (1) {
         vTaskSuspend(NULL);
     }
@@ -234,45 +234,45 @@ static void launcher_task(void *pvParameters)
 
 static void detect_button_press(void *pvParameters)
 {
-    /* int pin_high_count = 0; */
-    /* int last_pin_high_count = 0; */
-    /* const int pin_high_thresh = 10; */
-    /* while (1) { */
-    /*     if (GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_0) == Bit_SET) { */
-    /*         if (pin_high_count < pin_high_thresh) { */
-    /*             pin_high_count++; */
-    /*         } */
-    /*     } */
-    /*     else { */
-    /*         if (pin_high_count > 0) { */
-    /*             pin_high_count--; */
-    /*         } */
-    /*     } */
-    /*  */
-    /*     vTaskDelay(10 / portTICK_RATE_MS); #<{(| Debounce Delay |)}># */
-    /*  */
-    /*     if (pin_high_count == pin_high_thresh && */
-    /*             last_pin_high_count != pin_high_count) { */
-    /*         tm_trigger_button = 1; */
-    /*         usart_debug_puts("Bouton bleu\r\n"); */
-    /*  */
-    /*         if (temperature_valid()) { */
-    /*  */
-    /*             float temp = temperature_get(); */
-    /*  */
-    /*             usart_debug("Temperature %f\r\n", temp); */
-    /*  */
-    /*         } else { */
-    /*             usart_debug_puts("No temp\r\n"); */
-    /*         } */
-    /*     } */
-    /*     else if (pin_high_count == 0 && */
-    /*             last_pin_high_count != pin_high_count) { */
-    /*         tm_trigger_button = 0; */
-    /*     } */
-    /*  */
-    /*     last_pin_high_count = pin_high_count; */
-    /* } */
+    int pin_high_count = 0;
+    int last_pin_high_count = 0;
+    const int pin_high_thresh = 10;
+    while (1) {
+        if (pio_read_button()) {
+            if (pin_high_count < pin_high_thresh) {
+                pin_high_count++;
+            }
+        }
+        else {
+            if (pin_high_count > 0) {
+                pin_high_count--;
+            }
+        }
+
+        vTaskDelay(10 / portTICK_RATE_MS); /* Debounce Delay */
+
+        if (pin_high_count == pin_high_thresh &&
+                last_pin_high_count != pin_high_count) {
+            tm_trigger_button = 1;
+            usart_debug_puts("Bouton bleu\r\n");
+
+            if (temperature_valid()) {
+
+                float temp = temperature_get();
+
+                usart_debug("Temperature %f\r\n", temp);
+
+            } else {
+                usart_debug_puts("No temp\r\n");
+            }
+        }
+        else if (pin_high_count == 0 &&
+                last_pin_high_count != pin_high_count) {
+            tm_trigger_button = 0;
+        }
+
+        last_pin_high_count = pin_high_count;
+    }
 }
 
 static void audio_callback(void* context, int select_buffer) {
@@ -352,95 +352,97 @@ static void gps_monit_task(void *pvParameters) {
 
         vTaskDelay(100 / portTICK_RATE_MS);
 
-        // Reload watchdog //TODO
-        /* IWDG_ReloadCounter(); */
+        // Reload watchdog
+#ifndef SIMULATOR
+        IWDG_ReloadCounter();
+#endif
     }
 }
 
-/* static struct fsm_input_signals_t fsm_input; */
+static struct fsm_input_signals_t fsm_input;
 static void exercise_fsm(void *pvParameters)
 {
-    /* int cw_last_trigger = 0; */
-    /* int last_tm_trigger = 0; */
-    /* int last_tm_trigger_button = 0; */
-    /*  */
-    /* int last_sq = 0; */
-    /* int last_1750 = 0; */
-    /* int last_qrp = 0; */
-    /* int last_cw_done = 0; */
-    /*  */
-    /* fsm_input.humidity = 0; */
-    /* fsm_input.temp = 15; */
-    /* fsm_input.swr_high = 0; */
-    /* fsm_input.sstv_mode = 0; */
-    /* fsm_input.wind_generator_ok = 1; */
-    /* while (1) { */
-    /*     vTaskDelay(10 / portTICK_RATE_MS); */
-    /*  */
-    /*     pio_set_fsm_signals(&fsm_input); */
-    /*  */
-    /*     if (last_sq != fsm_input.sq) { */
-    /*         last_sq = fsm_input.sq; */
-    /*         usart_debug("In SQ %d\r\n", last_sq); */
-    /*     } */
-    /*     if (last_1750 != fsm_input.tone_1750) { */
-    /*         last_1750 = fsm_input.tone_1750; */
-    /*         usart_debug("In 1750 %d\r\n", last_1750); */
-    /*     } */
-    /*     if (last_qrp != fsm_input.qrp) { */
-    /*         last_qrp = fsm_input.qrp; */
-    /*         usart_debug("In QRP %d\r\n", last_qrp); */
-    /*     } */
-    /*  */
-    /*  */
-    /*     if (tm_trigger_button == 1 && last_tm_trigger_button == 0) { */
-    /*         fsm_input.start_tm = 1; */
-    /*     } */
-    /*     last_tm_trigger_button = tm_trigger_button; */
-    /*  */
-    /*     if (tm_trigger == 1 && last_tm_trigger == 0) { */
-    /*         fsm_input.start_tm = 1; */
-    /*     } */
-    /*     last_tm_trigger = tm_trigger; */
-    /*  */
-    /*     int cw_done = !cw_psk31_busy(); */
-    /*     if (last_cw_done != cw_done) { */
-    /*         usart_debug("In CW done %d\r\n", cw_done); */
-    /*         last_cw_done = cw_done; */
-    /*  */
-    /*         fsm_input.cw_psk31_done = cw_done; */
-    /*     } */
-    /*     else { */
-    /*         fsm_input.cw_psk31_done = 0; */
-    /*     } */
-    /*  */
-    /*     if (fsm_input.cw_psk31_done) { */
-    /*         GPIO_ResetBits(GPIOD, GPIOD_BOARD_LED_ORANGE); */
-    /*     } */
-    /*     else { */
-    /*         GPIO_SetBits(GPIOD, GPIOD_BOARD_LED_ORANGE); */
-    /*     } */
-    /*  */
-    /*     fsm_update_inputs(&fsm_input); */
-    /*     fsm_update(); */
-    /*  */
-    /*     struct fsm_output_signals_t fsm_out; */
-    /*     fsm_get_outputs(&fsm_out); */
-    /*  */
-    /*     pio_set_tx(fsm_out.tx_on); */
-    /*     pio_set_mod_off(!fsm_out.modulation); */
-    /*     pio_set_qrp(fsm_out.qrp); // TODO move out of FSM */
-    /*  */
-    /*     // Add message to CW generator only on rising edge of trigger */
-    /*     if (fsm_out.cw_psk31_trigger && !cw_last_trigger) { */
-    /*         cw_psk31_push_message(fsm_out.msg, fsm_out.cw_dit_duration, fsm_out.msg_frequency); */
-    /*  */
-    /*         usart_debug_puts("Out CW trigger\r\n"); */
-    /*     } */
-    /*     cw_last_trigger = fsm_out.cw_psk31_trigger; */
-    /*  */
-    /*     if (fsm_out.ack_start_tm) { */
-    /*         fsm_input.start_tm = 0; */
-    /*     } */
-    /* } */
+    int cw_last_trigger = 0;
+    int last_tm_trigger = 0;
+    int last_tm_trigger_button = 0;
+
+    int last_sq = 0;
+    int last_1750 = 0;
+    int last_qrp = 0;
+    int last_cw_done = 0;
+
+    fsm_input.humidity = 0;
+    fsm_input.temp = 15;
+    fsm_input.swr_high = 0;
+    fsm_input.sstv_mode = 0;
+    fsm_input.wind_generator_ok = 1;
+    while (1) {
+        vTaskDelay(10 / portTICK_RATE_MS);
+
+        pio_set_fsm_signals(&fsm_input);
+
+        if (last_sq != fsm_input.sq) {
+            last_sq = fsm_input.sq;
+            usart_debug("In SQ %d\r\n", last_sq);
+        }
+        if (last_1750 != fsm_input.tone_1750) {
+            last_1750 = fsm_input.tone_1750;
+            usart_debug("In 1750 %d\r\n", last_1750);
+        }
+        if (last_qrp != fsm_input.qrp) {
+            last_qrp = fsm_input.qrp;
+            usart_debug("In QRP %d\r\n", last_qrp);
+        }
+
+
+        if (tm_trigger_button == 1 && last_tm_trigger_button == 0) {
+            fsm_input.start_tm = 1;
+        }
+        last_tm_trigger_button = tm_trigger_button;
+
+        if (tm_trigger == 1 && last_tm_trigger == 0) {
+            fsm_input.start_tm = 1;
+        }
+        last_tm_trigger = tm_trigger;
+
+        int cw_done = !cw_psk31_busy();
+        if (last_cw_done != cw_done) {
+            usart_debug("In CW done %d\r\n", cw_done);
+            last_cw_done = cw_done;
+
+            fsm_input.cw_psk31_done = cw_done;
+        }
+        else {
+            fsm_input.cw_psk31_done = 0;
+        }
+
+        if (fsm_input.cw_psk31_done) {
+            leds_turn_off(LED_ORANGE);
+        }
+        else {
+            leds_turn_on(LED_ORANGE);
+        }
+
+        fsm_update_inputs(&fsm_input);
+        fsm_update();
+
+        struct fsm_output_signals_t fsm_out;
+        fsm_get_outputs(&fsm_out);
+
+        pio_set_tx(fsm_out.tx_on);
+        pio_set_mod_off(!fsm_out.modulation);
+        pio_set_qrp(fsm_out.qrp); // TODO move out of FSM
+
+        // Add message to CW generator only on rising edge of trigger
+        if (fsm_out.cw_psk31_trigger && !cw_last_trigger) {
+            cw_psk31_push_message(fsm_out.msg, fsm_out.cw_dit_duration, fsm_out.msg_frequency);
+
+            usart_debug_puts("Out CW trigger\r\n");
+        }
+        cw_last_trigger = fsm_out.cw_psk31_trigger;
+
+        if (fsm_out.ack_start_tm) {
+            fsm_input.start_tm = 0;
+        }
+    }
 }
diff --git a/src/common/src/GPIO/usart.c b/src/common/src/GPIO/usart.c
index 77272f8..dd20073 100644
--- a/src/common/src/GPIO/usart.c
+++ b/src/common/src/GPIO/usart.c
@@ -80,6 +80,12 @@ void usart_debug_puts(const char* str) {
     xTaskResumeAll();
 }
 
+void usart_debug_puts_no_header(const char* str) {
+    vTaskSuspendAll();
+    usart_puts(USART2, str);
+    xTaskResumeAll();
+}
+
 int usart_get_nmea_sentence(char* nmea) {
     return xQueueReceive(usart_nmea_queue, nmea, portMAX_DELAY);
 }