Configuration and audio level monitoring implemented

.gitignore

@@ -13,7 +13,6 @@ images/*.map
 images/*.elf
 images/*.sym
 images/*.hex
-testkit
 flashdefault
 flashtestkit
 flashcurrent

hardware/AFSK.c

@@ -12,6 +12,9 @@ bool hw_afsk_dac_isr = false;
 bool hw_5v_ref = false;
 Afsk *AFSK_modem;
 
+int8_t afsk_peak = 0;
+uint16_t peak_ticks = 0;
+
 // Forward declerations
 int afsk_getchar(FILE *strem);
 int afsk_putchar(char c, FILE *stream);
@@ -376,8 +379,21 @@ static bool hdlcParse(Hdlc *hdlc, bool bit, FIFOBuffer *fifo) {
     return ret;
 }
 
-
+#define AFSK_PEAK_DECAY 96
 void AFSK_adc_isr(Afsk *afsk, int8_t currentSample) {
+    if (config_output_diagnostics) {
+        peak_ticks++;
+        if (currentSample > afsk_peak) {
+            afsk_peak = currentSample;
+        } else {
+            if (peak_ticks >= AFSK_PEAK_DECAY) {
+                peak_ticks = 0;
+                if (afsk_peak > 0) {
+                    afsk_peak--;
+                }
+            }
+        }
+    }
     // To determine the received frequency, and thereby
     // the bit of the sample, we multiply the sample by
     // a sample delayed by (samples per bit / 2).

hardware/LED.c

@@ -1,9 +1,10 @@
 #include "LED.h"
 #include "util/time.h"
+#include "util/Config.h"
 
 bool LED_softblock_enabled = false;
 
-uint8_t ledIntensity = CONFIG_LED_INTENSITY;
+//uint8_t ledIntensity = CONFIG_LED_INTENSITY;
 ticks_t led_status_ticks_top = 0;
 ticks_t led_status_ticks = 0;
 ticks_t com_led_timeout = 0;
@@ -27,7 +28,7 @@ void LED_init(void) {
 
     TCCR0B = _BV(CS00);
 
-    OCR0A = ledIntensity;
+    OCR0A = config_led_intensity;
 }
 
 void LED_softblock_on(void) {
@@ -39,8 +40,8 @@ void LED_softblock_off(void) {
 }
 
 void LED_setIntensity(uint8_t value) {
-	ledIntensity = value;
+	config_led_intensity = value;
-	OCR0A = ledIntensity;
+	OCR0A = config_led_intensity;
 }
 
 void LED_COM_ON(void) {

hardware/Serial.c

@@ -1,6 +1,7 @@
 #include "Serial.h"
 #include <stdio.h>
 #include <string.h>
+#include "util/Config.h"
 
 extern volatile uint8_t queue_height;
 
@@ -8,7 +9,7 @@ void serial_init(Serial *serial) {
     memset(serial, 0, sizeof(*serial));
     memset(uart0Buf, 0, sizeof(uart0Buf));
     memset(uart1Buf, 0, sizeof(uart1Buf));
-
+    
     serial_setbaudrate_115200(0);
     serial_setbaudrate_115200(1);
 
@@ -27,6 +28,7 @@ void serial_init(Serial *serial) {
 
     fifo_init(&uart0FIFO, uart0Buf, sizeof(uart0Buf));
     fifo_init(&uart1FIFO, uart1Buf, sizeof(uart1Buf));
+
 }
 
 bool serial_available(uint8_t index) {

protocol/KISS.c

@@ -26,7 +26,10 @@ size_t packet_lengths_buf[CONFIG_QUEUE_MAX_LENGTH+1];
 AX25Ctx *ax25ctx;
 Afsk *channel;
 Serial *serial;
+
 volatile ticks_t last_serial_read = 0;
+extern volatile int8_t afsk_peak;
+
 size_t frame_len;
 bool IN_FRAME;
 bool ESCAPE;
@@ -283,9 +286,6 @@ void kiss_serialCallback(uint8_t sbyte) {
     } else if (IN_FRAME && frame_len < AX25_MAX_PAYLOAD) {
         // Have a look at the command byte first
         if (frame_len == 0 && command == CMD_UNKNOWN) {
-            // OpenModem supports only one HDLC port, so we
-            // strip off the port nibble of the command byte
-            sbyte = sbyte & 0x0F;
             command = sbyte;
             if (command == CMD_DATA) current_packet_start = queue_cursor;
         } else if (command == CMD_DATA) {
@@ -311,6 +311,12 @@ void kiss_serialCallback(uint8_t sbyte) {
             config_slottime = sbyte * 10UL;
         } else if (command == CMD_P) {
             config_p = sbyte;
+        } else if (command == CMD_SAVE_CONFIG) {
+            config_save();
+        } else if (command == CMD_REBOOT) {
+            if (sbyte == CMD_REBOOT_CONFIRM) {
+                config_soft_reboot();
+            }
         } else if (command == CMD_LED_INTENSITY) {
             if (sbyte == FESC) {
                 ESCAPE = true;
@@ -320,9 +326,114 @@ void kiss_serialCallback(uint8_t sbyte) {
                     if (sbyte == TFESC) sbyte = FESC;
                     ESCAPE = false;
                 }
-                LED_setIntensity(sbyte);   
+                LED_setIntensity(sbyte);
+            }
+        } else if (command == CMD_OUTPUT_GAIN) {
+            if (sbyte == FESC) { ESCAPE = true; } else {
+                if (ESCAPE) {
+                    if (sbyte == TFEND) sbyte = FEND;
+                    if (sbyte == TFESC) sbyte = FESC;
+                    ESCAPE = false;
+                }
+                config_set_output_gain(sbyte);
+            }
+        } else if (command == CMD_INPUT_GAIN) {
+            if (sbyte == FESC) { ESCAPE = true; } else {
+                if (ESCAPE) {
+                    if (sbyte == TFEND) sbyte = FEND;
+                    if (sbyte == TFESC) sbyte = FESC;
+                    ESCAPE = false;
+                }
+                config_set_input_gain(sbyte);
+            }
+        } else if (command == CMD_PASSALL) {
+            if (sbyte == FESC) { ESCAPE = true; } else {
+                if (ESCAPE) {
+                    if (sbyte == TFEND) sbyte = FEND;
+                    if (sbyte == TFESC) sbyte = FESC;
+                    ESCAPE = false;
+                }
+                config_set_passall(sbyte);
+            }
+        } else if (command == CMD_LOG_PACKETS) {
+            if (sbyte == FESC) { ESCAPE = true; } else {
+                if (ESCAPE) {
+                    if (sbyte == TFEND) sbyte = FEND;
+                    if (sbyte == TFESC) sbyte = FESC;
+                    ESCAPE = false;
+                }
+                config_set_log_packets(sbyte);
+            }
+        } else if (command == CMD_GPS_MODE) {
+            if (sbyte == FESC) { ESCAPE = true; } else {
+                if (ESCAPE) {
+                    if (sbyte == TFEND) sbyte = FEND;
+                    if (sbyte == TFESC) sbyte = FESC;
+                    ESCAPE = false;
+                }
+                config_set_gps_mode(sbyte);
+            }
+        } else if (command == CMD_BT_MODE) {
+            if (sbyte == FESC) { ESCAPE = true; } else {
+                if (ESCAPE) {
+                    if (sbyte == TFEND) sbyte = FEND;
+                    if (sbyte == TFESC) sbyte = FESC;
+                    ESCAPE = false;
+                }
+                config_set_bt_mode(sbyte);
+            }
+        } else if (command == CMD_SERIAL_BAUDRATE) {
+            config_set_serial_baudrate(sbyte);
+        // TODO: Remove this
+        } else if (command == CMD_PRINT_CONFIG) {
+            config_print();
+        } else if (command == CMD_AUDIO_PEAK) {
+            if (sbyte == 0x01) {
+                kiss_output_afsk_peak();
+            }
+        }  else if (command == CMD_ENABLE_DIAGNOSTICS) {
+            if (sbyte == 0x00) {
+                config_disable_diagnostics();
+            } else {
+                config_enable_diagnostics();
             }
         }
         
     }
+}
+
+void kiss_output_afsk_peak(void) {
+    fputc(FEND, &serial->uart0);
+    fputc(CMD_AUDIO_PEAK, &serial->uart0);
+    uint8_t b = afsk_peak;
+
+    if (b == FEND) {
+        fputc(FESC, &serial->uart0);
+        fputc(TFEND, &serial->uart0);
+    } else if (b == FESC) {
+        fputc(FESC, &serial->uart0);
+        fputc(TFESC, &serial->uart0);
+    } else {
+        fputc(b, &serial->uart0);
+    }
+
+    fputc(FEND, &serial->uart0);
+}
+
+void kiss_output_config(uint8_t* data, size_t length) {
+    fputc(FEND, &serial->uart0);
+    fputc(CMD_PRINT_CONFIG, &serial->uart0);
+    for (unsigned i = 0; i < length; i++) {
+        uint8_t b = data[i];
+        if (b == FEND) {
+            fputc(FESC, &serial->uart0);
+            fputc(TFEND, &serial->uart0);
+        } else if (b == FESC) {
+            fputc(FESC, &serial->uart0);
+            fputc(TFESC, &serial->uart0);
+        } else {
+            fputc(b, &serial->uart0);
+        }
+    }
+    fputc(FEND, &serial->uart0);
 }

protocol/KISS.h

@@ -19,10 +19,21 @@
 #define CMD_TXTAIL 0x04
 #define CMD_FULLDUPLEX 0x05
 #define CMD_SETHARDWARE 0x06
-#define CMD_FLUSHQUEUE 0x07
+#define CMD_SAVE_CONFIG 0x07
-#define CMD_FLUSHQUEUE_DEBUG 0x08
+#define CMD_LED_INTENSITY 0x08
-#define CMD_LED_INTENSITY 0x09
+#define CMD_OUTPUT_GAIN 0x09
+#define CMD_INPUT_GAIN 0x0A
+#define CMD_PASSALL 0x0B
+#define CMD_LOG_PACKETS 0x0C
+#define CMD_GPS_MODE 0x0D
+#define CMD_BT_MODE	0x0E
 #define CMD_READY 0x0F
+#define CMD_SERIAL_BAUDRATE 0x10
+#define CMD_REBOOT 0x11
+#define CMD_REBOOT_CONFIRM 0x9A
+#define CMD_AUDIO_PEAK 0x12
+#define CMD_ENABLE_DIAGNOSTICS 0x13
+#define CMD_PRINT_CONFIG 0xF0
 #define CMD_RETURN 0xFF
 
 void kiss_init(AX25Ctx *ax25, Afsk *afsk, Serial *ser);
@@ -32,4 +43,7 @@ void kiss_flushQueue(void);
 void kiss_csma(void);
 void kiss_poll(void);
 
+void kiss_output_config(uint8_t* data, size_t length);
+void kiss_output_afsk_peak(void);
+
 #endif

testkit/audiolevel.py

@@ -0,0 +1,309 @@
+from __future__ import print_function
+from time import sleep
+import sys
+import serial
+import threading
+import time
+import argparse
+import struct
+
+class RNS():
+	@staticmethod
+	def log(msg):
+		logtimefmt   = "%Y-%m-%d %H:%M:%S"
+		timestamp = time.time()
+		logstring = "["+time.strftime(logtimefmt)+"] "+msg
+		print(logstring)
+
+	@staticmethod
+	def hexrep(data, delimit=True):
+		delimiter = ":"
+		if not delimit:
+			delimiter = ""
+		hexrep = delimiter.join("{:02x}".format(ord(c)) for c in data)
+		return hexrep
+
+	@staticmethod
+	def prettyhexrep(data):
+		delimiter = ""
+		hexrep = "<"+delimiter.join("{:02x}".format(ord(c)) for c in data)+">"
+		return hexrep
+
+class Interface:
+    IN  = False
+    OUT = False
+    FWD = False
+    RPT = False
+    name = None
+
+    def __init__(self):
+        pass
+
+class KISS():
+	FEND			= chr(0xC0)
+	FESC			= chr(0xDB)
+	TFEND			= chr(0xDC)
+	TFESC			= chr(0xDD)
+	CMD_UNKNOWN		= chr(0xFE)
+	CMD_DATA		= chr(0x00)
+	CMD_TXDELAY		= chr(0x01)
+	CMD_P			= chr(0x02)
+	CMD_SLOTTIME	= chr(0x03)
+	CMD_TXTAIL		= chr(0x04)
+	CMD_FULLDUPLEX	= chr(0x05)
+	CMD_SETHARDWARE	= chr(0x06)
+	CMD_READY       = chr(0x0F)
+	CMD_AUDIO_PEAK  = chr(0x12)
+	CMD_EN_DIAGS    = chr(0x13)
+	CMD_RETURN		= chr(0xFF)
+
+	@staticmethod
+	def escape(data):
+		data = data.replace(chr(0xdb), chr(0xdb)+chr(0xdd))
+		data = data.replace(chr(0xc0), chr(0xdb)+chr(0xdc))
+		return data
+
+class KISSInterface(Interface):
+	MAX_CHUNK = 32768
+
+	owner    = None
+	port     = None
+	speed    = None
+	databits = None
+	parity   = None
+	stopbits = None
+	serial   = None
+
+	def __init__(self, owner, name, port, speed, databits, parity, stopbits, preamble, txtail, persistence, slottime, flow_control):
+		self.serial   = None
+		self.owner    = owner
+		self.name     = name
+		self.port     = port
+		self.speed    = speed
+		self.databits = databits
+		self.parity   = serial.PARITY_NONE
+		self.stopbits = stopbits
+		self.timeout  = 100
+		self.online   = False
+
+		self.packet_queue    = []
+		self.flow_control = flow_control
+		self.interface_ready = False
+
+		self.preamble    = preamble if preamble != None else 350;
+		self.txtail      = txtail if txtail != None else 20;
+		self.persistence = persistence if persistence != None else 64;
+		self.slottime    = slottime if slottime != None else 20;
+
+		if parity.lower() == "e" or parity.lower() == "even":
+			self.parity = serial.PARITY_EVEN
+
+		if parity.lower() == "o" or parity.lower() == "odd":
+			self.parity = serial.PARITY_ODD
+
+		try:
+			RNS.log("Opening serial port "+self.port+"...")
+			self.serial = serial.Serial(
+				port = self.port,
+				baudrate = self.speed,
+				bytesize = self.databits,
+				parity = self.parity,
+				stopbits = self.stopbits,
+				xonxoff = False,
+				rtscts = False,
+				timeout = 0,
+				inter_byte_timeout = None,
+				write_timeout = None,
+				dsrdtr = False,
+			)
+		except Exception as e:
+			RNS.log("Could not open serial port "+self.port)
+			raise e
+
+		if self.serial.is_open:
+			# Allow time for interface to initialise before config
+			sleep(2.0)
+			thread = threading.Thread(target=self.readLoop)
+			thread.setDaemon(True)
+			thread.start()
+			self.online = True
+			RNS.log("Serial port "+self.port+" is now open")
+			RNS.log("Configuring KISS interface parameters...")
+			self.setPreamble(self.preamble)
+			self.setTxTail(self.txtail)
+			self.setPersistence(self.persistence)
+			self.setSlotTime(self.slottime)
+			self.setFlowControl(self.flow_control)
+			self.interface_ready = True
+			RNS.log("KISS interface configured")
+		else:
+			raise IOError("Could not open serial port")
+
+
+	def askForPeak(self):
+		kiss_command = KISS.FEND+KISS.CMD_AUDIO_PEAK+chr(0x01)+KISS.FEND
+		written = self.serial.write(kiss_command)
+		if written != len(kiss_command):
+			raise IOError("Could not ask for peak data")
+
+	def displayPeak(self, peak):
+		peak_value = struct.unpack("b", peak)
+		#RNS.log("Peak is: "+RNS.hexrep(peak))
+		RNS.log("Peak is: "+str(peak_value[0]))
+
+	def setPreamble(self, preamble):
+		preamble_ms = preamble
+		preamble = int(preamble_ms / 10)
+		if preamble < 0:
+			preamble = 0
+		if preamble > 255:
+			preamble = 255
+
+		RNS.log("Setting preamble to "+str(preamble)+" "+chr(preamble))
+		kiss_command = KISS.FEND+KISS.CMD_TXDELAY+chr(preamble)+KISS.FEND
+		written = self.serial.write(kiss_command)
+		if written != len(kiss_command):
+			raise IOError("Could not configure KISS interface preamble to "+str(preamble_ms)+" (command value "+str(preamble)+")")
+
+	def setTxTail(self, txtail):
+		txtail_ms = txtail
+		txtail = int(txtail_ms / 10)
+		if txtail < 0:
+			txtail = 0
+		if txtail > 255:
+			txtail = 255
+
+		kiss_command = KISS.FEND+KISS.CMD_TXTAIL+chr(txtail)+KISS.FEND
+		written = self.serial.write(kiss_command)
+		if written != len(kiss_command):
+			raise IOError("Could not configure KISS interface TX tail to "+str(txtail_ms)+" (command value "+str(txtail)+")")
+
+	def setPersistence(self, persistence):
+		if persistence < 0:
+			persistence = 0
+		if persistence > 255:
+			persistence = 255
+
+		kiss_command = KISS.FEND+KISS.CMD_P+chr(persistence)+KISS.FEND
+		written = self.serial.write(kiss_command)
+		if written != len(kiss_command):
+			raise IOError("Could not configure KISS interface persistence to "+str(persistence))
+
+	def setSlotTime(self, slottime):
+		slottime_ms = slottime
+		slottime = int(slottime_ms / 10)
+		if slottime < 0:
+			slottime = 0
+		if slottime > 255:
+			slottime = 255
+
+		kiss_command = KISS.FEND+KISS.CMD_SLOTTIME+chr(slottime)+KISS.FEND
+		written = self.serial.write(kiss_command)
+		if written != len(kiss_command):
+			raise IOError("Could not configure KISS interface slot time to "+str(slottime_ms)+" (command value "+str(slottime)+")")
+
+	def setFlowControl(self, flow_control):
+		kiss_command = KISS.FEND+KISS.CMD_READY+chr(0x01)+KISS.FEND
+		written = self.serial.write(kiss_command)
+		if written != len(kiss_command):
+			if (flow_control):
+				raise IOError("Could not enable KISS interface flow control")
+			else:
+				raise IOError("Could not enable KISS interface flow control")
+
+	def enableDiagnostics(self):
+		kiss_command = KISS.FEND+KISS.CMD_EN_DIAGS+chr(0x01)+KISS.FEND
+		written = self.serial.write(kiss_command)
+		if written != len(kiss_command):
+			raise IOError("Could not enable KISS interface diagnostics")
+
+	def disableDiagnostics(self):
+		kiss_command = KISS.FEND+KISS.CMD_EN_DIAGS+chr(0x00)+KISS.FEND
+		written = self.serial.write(kiss_command)
+		if written != len(kiss_command):
+			raise IOError("Could not disable KISS interface diagnostics")
+			
+
+
+	def processIncoming(self, data):
+		RNS.log("Decoded packet");
+
+
+	def processOutgoing(self,data):
+		pass
+
+	def queue(self, data):
+		pass
+
+	def process_queue(self):
+		pass
+
+	def readLoop(self):
+		try:
+			in_frame = False
+			escape = False
+			command = KISS.CMD_UNKNOWN
+			data_buffer = ""
+			last_read_ms = int(time.time()*1000)
+
+			while self.serial.is_open:
+				if self.serial.in_waiting:
+					byte = self.serial.read(1)
+					last_read_ms = int(time.time()*1000)
+
+					if (in_frame and byte == KISS.FEND and command == KISS.CMD_DATA):
+						in_frame = False
+						self.processIncoming(data_buffer)
+					elif (byte == KISS.FEND):
+						in_frame = True
+						command = KISS.CMD_UNKNOWN
+						data_buffer = ""
+					elif (in_frame and len(data_buffer) < 611):
+						if (len(data_buffer) == 0 and command == KISS.CMD_UNKNOWN):
+							command = byte
+						elif (command == KISS.CMD_DATA):
+							if (byte == KISS.FESC):
+								escape = True
+							else:
+								if (escape):
+									if (byte == KISS.TFEND):
+										byte = KISS.FEND
+									if (byte == KISS.TFESC):
+										byte = KISS.FESC
+									escape = False
+								data_buffer = data_buffer+byte
+						elif (command == KISS.CMD_AUDIO_PEAK):
+							self.displayPeak(byte)
+				else:
+					time_since_last = int(time.time()*1000) - last_read_ms
+					if len(data_buffer) > 0 and time_since_last > self.timeout:
+			 			data_buffer = ""
+			 			in_frame = False
+			 			command = KISS.CMD_UNKNOWN
+			 			escape = False
+			 		sleep(0.08)
+			 		self.askForPeak()
+
+		except Exception as e:
+			self.online = False
+			RNS.log("A serial port error occurred, the contained exception was: "+str(e))
+			RNS.log("The interface "+str(self.name)+" is now offline. Restart Reticulum to attempt reconnection.")
+
+	def __str__(self):
+		return "KISSInterface["+self.name+"]"
+
+if __name__ == "__main__":
+		parser = argparse.ArgumentParser(description="OpenModem Audio Level Monitor")
+		parser.add_argument("port", nargs="?", default=None, help="serial port where RNode is attached", type=str)
+		args = parser.parse_args()
+
+		if args.port:
+			kiss_interface = KISSInterface(None, "OpenModem Interface", args.port, 115200, 8, "N", 1, 150, 10, 255, 20, False)
+			kiss_interface.enableDiagnostics()
+			raw_input();
+
+		else:
+			print("")
+			parser.print_help()
+			print("")
+			exit()

util/Config.c

@@ -7,10 +7,14 @@
 #include "device.h"
 #include "hardware/crypto/MD5.h"
 #include "hardware/AFSK.h"
+#include "hardware/Serial.h"
+#include "hardware/VREF.h"
+#include "protocol/KISS.h"
 
 
 void config_init(void) {
 	config_source = CONFIG_SOURCE_NONE;
+	config_output_diagnostics = false;
 
 	bool has_valid_eeprom_config = config_validate_eeprom();
 
@@ -20,6 +24,28 @@ void config_init(void) {
 		config_load_defaults();
 		config_save_to_eeprom();
 	}
+
+	config_apply();
+}
+
+void config_apply(void) {
+	if (config_serial_baudrate == CONFIG_BAUDRATE_1200) serial_setbaudrate_1200(0);
+	if (config_serial_baudrate == CONFIG_BAUDRATE_2400) serial_setbaudrate_2400(0);
+	if (config_serial_baudrate == CONFIG_BAUDRATE_4800) serial_setbaudrate_4800(0);
+	if (config_serial_baudrate == CONFIG_BAUDRATE_9600) serial_setbaudrate_9600(0);
+	if (config_serial_baudrate == CONFIG_BAUDRATE_14400) serial_setbaudrate_14400(0);
+	if (config_serial_baudrate == CONFIG_BAUDRATE_19200) serial_setbaudrate_19200(0);
+	if (config_serial_baudrate == CONFIG_BAUDRATE_28800) serial_setbaudrate_28800(0);
+	if (config_serial_baudrate == CONFIG_BAUDRATE_38400) serial_setbaudrate_38400(0);
+	if (config_serial_baudrate == CONFIG_BAUDRATE_57600) serial_setbaudrate_57600(0);
+	if (config_serial_baudrate == CONFIG_BAUDRATE_76800) serial_setbaudrate_76800(0);
+	if (config_serial_baudrate == CONFIG_BAUDRATE_115200) serial_setbaudrate_115200(0);
+	if (config_serial_baudrate == CONFIG_BAUDRATE_230400) serial_setbaudrate_230400(0);
+}
+
+void config_save(void) {
+	config_save_to_eeprom();
+	config_save_to_sd();
 }
 
 void config_wipe_eeprom(void) {
@@ -133,21 +159,6 @@ void config_load_from_eeprom(void) {
 	config_gps_mode = config_data[ADDR_E_GPS_MODE];
 	config_bluetooth_mode = config_data[ADDR_E_BLUETOOTH_MODE];
 	config_serial_baudrate = config_data[ADDR_E_SERIAL_BAUDRATE];
-
-	// printf("Configuration loaded from EEPROM:\r\n");
-	// printf("\tP\t\t%02X\r\n", config_p);
-	// printf("\tSlottime\t%lu\r\n", config_slottime);
-	// printf("\tPreamble\t%lu\r\n", config_preamble);
-	// printf("\tTail\t\t%lu\r\n", config_tail);
-	// printf("\tLEDs\t\t%02X\r\n", config_led_intensity);
-	// printf("\tOut gain\t%02X\r\n", config_output_gain);
-	// printf("\tIn gain\t\t%02X\r\n", config_input_gain);
-	// printf("\tPassall\t\t%02X\r\n", config_passall);
-	// printf("\tLog pkts\t%02X\r\n", config_log_packets);
-	// printf("\tCrypto lock\t%02X\r\n", config_crypto_lock);
-	// printf("\tGPS mode\t%02X\r\n", config_gps_mode);
-	// printf("\tBT Mode\t\t%02X\r\n", config_bluetooth_mode);
-	// printf("\tBaudrate\t%02X\r\n", config_serial_baudrate);
 }
 
 bool config_validate_sd(void) {
@@ -171,9 +182,49 @@ void config_crypto_lock_enable(void) {
 
 void config_crypto_lock_disable(void) {
 	config_crypto_lock = false;
-	config_save_to_eeprom();
+	config_soft_reboot();
-	wdt_enable(WDTO_15MS);
+}
-	while(true) { }
+
+void config_set_serial_baudrate(uint8_t baudrate) {
+	if (baudrate >= CONFIG_BAUDRATE_1200 && baudrate <= CONFIG_BAUDRATE_230400) {
+		config_serial_baudrate = baudrate;
+	}
+}
+
+void config_set_output_gain(uint8_t gain) {
+	vref_setDAC(gain);
+}
+
+void config_set_input_gain(uint8_t gain) {
+	vref_setADC(0xFF - gain);
+}
+
+void config_set_passall(uint8_t passall) {
+	if (passall == 0x00) {
+		config_passall = false;
+	} else {
+		config_passall = true;
+	}
+}
+
+void config_set_log_packets(uint8_t log_packets) {
+	if (log_packets == 0x00) {
+		config_log_packets = false;
+	} else {
+		config_log_packets = true;
+	}
+}
+
+void config_set_gps_mode(uint8_t mode) {
+	if (mode == CONFIG_GPS_OFF) config_gps_mode = CONFIG_GPS_OFF;
+	if (mode == CONFIG_GPS_AUTODETECT) config_gps_mode = CONFIG_GPS_AUTODETECT;
+	if (mode == CONFIG_GPS_REQUIRED) config_gps_mode = CONFIG_GPS_REQUIRED;
+}
+
+void config_set_bt_mode(uint8_t mode) {
+	if (mode == CONFIG_BLUETOOTH_OFF) config_bluetooth_mode = CONFIG_BLUETOOTH_OFF;
+	if (mode == CONFIG_BLUETOOTH_AUTODETECT) config_bluetooth_mode = CONFIG_BLUETOOTH_AUTODETECT;
+	if (mode == CONFIG_BLUETOOTH_REQUIRED) config_bluetooth_mode = CONFIG_BLUETOOTH_REQUIRED;
 }
 
 void EEPROM_writebyte(uint16_t addr, uint8_t data) {
@@ -219,4 +270,45 @@ void EEPROM_updatebyte(uint16_t addr, uint8_t data) {
 	if (byte != data) {
 		EEPROM_writebyte(addr, data);
 	}
+}
+
+void config_soft_reboot(void) {
+	config_save_to_eeprom();
+	wdt_enable(WDTO_15MS);
+	while(true) { }
+}
+
+void config_enable_diagnostics(void) {
+	config_output_diagnostics = true;
+}
+
+void config_disable_diagnostics(void) {
+	config_output_diagnostics = false;
+}
+
+// TODO: remove this
+void config_print(void) {
+	uint8_t config_size = ADDR_E_END;
+	uint8_t config_data[config_size];
+
+	for (uint16_t addr = 0; addr < config_size; addr++) {
+		config_data[addr] = EEPROM_readbyte(addr);
+	}
+
+	kiss_output_config(config_data, config_size);
+
+	// printf(PSTR("Running configuration:\r\n"));
+	// printf("\tP\t\t%02X\r\n", config_p);
+	// printf("\tSlottime\t%lu\r\n", config_slottime);
+	// printf("\tPreamble\t%lu\r\n", config_preamble);
+	// printf("\tTail\t\t%lu\r\n", config_tail);
+	// printf("\tLEDs\t\t%02X\r\n", config_led_intensity);
+	// printf("\tOut gain\t%02X\r\n", config_output_gain);
+	// printf("\tIn gain\t\t%02X\r\n", 0xFF - config_input_gain);
+	// printf("\tPassall\t\t%02X\r\n", config_passall);
+	// printf("\tLog pkts\t%02X\r\n", config_log_packets);
+	// printf("\tCrypto lock\t%02X\r\n", config_crypto_lock);
+	// printf("\tGPS mode\t%02X\r\n", config_gps_mode);
+	// printf("\tBT Mode\t\t%02X\r\n", config_bluetooth_mode);
+	// printf("\tBaudrate\t%02X\r\n", config_serial_baudrate);
 }

util/Config.h

@@ -69,7 +69,11 @@ uint8_t config_gps_mode;
 uint8_t config_bluetooth_mode;
 uint8_t config_serial_baudrate;
 
+bool config_output_diagnostics;
+
 void config_init(void);
+void config_apply(void);
+void config_save(void);
 
 bool config_validate_eeprom(void);
 bool config_validate_sd(void);
@@ -85,6 +89,20 @@ void config_load_from_sd(void);
 void config_crypto_lock_enable(void);
 void config_crypto_lock_disable(void);
 
+void config_set_serial_baudrate(uint8_t baudrate);
+void config_set_output_gain(uint8_t gain);
+void config_set_input_gain(uint8_t gain);
+void config_set_passall(uint8_t passall);
+void config_set_log_packets(uint8_t log_packets);
+void config_set_gps_mode(uint8_t mode);
+void config_set_bt_mode(uint8_t mode);
+
+void config_enable_diagnostics(void);
+void config_disable_diagnostics(void);
+
+void config_soft_reboot(void);
+void config_print(void);
+
 void EEPROM_updatebyte(uint16_t addr, uint8_t data);
 uint8_t EEPROM_readbyte(uint16_t addr);
 void EEPROM_writebyte(uint16_t addr, uint8_t data);