Added modem mode KISS commands

protocol/KISS.c

@@ -400,6 +400,7 @@ void kiss_serialCallback(uint8_t sbyte) {
         // TODO: Remove this
         } else if (command == CMD_PRINT_CONFIG) {
             config_print();
+            kiss_output_modem_mode();
         } else if (command == CMD_AUDIO_PEAK) {
             if (sbyte == 0x01) {
                 kiss_output_afsk_peak();
@@ -410,11 +411,28 @@ void kiss_serialCallback(uint8_t sbyte) {
             } else {
                 config_enable_diagnostics();
             }
+        }  else if (command == CMD_MODE) {
+            if (sbyte == 0x00) {
+                kiss_output_modem_mode();
+            }
         }
         
     }
 }
 
+void kiss_output_modem_mode(void) {
+    fputc(FEND, &serial->uart0);
+    fputc(CMD_MODE, &serial->uart0);
+    if (BITRATE == 300) {
+        fputc(MODE_AFSK_300, &serial->uart0);
+    } else if (BITRATE == 1200) {
+        fputc(MODE_AFSK_1200, &serial->uart0);
+    } else if (BITRATE == 2400) {
+        fputc(MODE_AFSK_2400, &serial->uart0);
+    }
+    fputc(FEND, &serial->uart0);
+}
+
 void kiss_output_afsk_peak(void) {
     fputc(FEND, &serial->uart0);
     fputc(CMD_AUDIO_PEAK, &serial->uart0);

protocol/KISS.h

@@ -33,6 +33,7 @@
 #define CMD_REBOOT_CONFIRM 0x9A
 #define CMD_AUDIO_PEAK 0x12
 #define CMD_ENABLE_DIAGNOSTICS 0x13
+#define CMD_MODE 0x14
 #define CMD_PRINT_CONFIG 0xF0
 #define CMD_RETURN 0xFF
 
@@ -43,6 +44,7 @@ void kiss_flushQueue(void);
 void kiss_csma(void);
 void kiss_poll(void);
 
+void kiss_output_modem_mode(void);
 void kiss_output_config(uint8_t* data, size_t length);
 void kiss_output_afsk_peak(void);
 

util/constants.h

@@ -7,3 +7,7 @@
 #define START_FROM_POWERON 0x02
 #define START_FROM_BROWNOUT 0x03
 #define START_FROM_JTAG 0x04
+
+#define MODE_AFSK_300 0x01
+#define MODE_AFSK_1200 0x02
+#define MODE_AFSK_2400 0x03