From 77e022f03be51b0772939b1c4b311da101e8d307 Mon Sep 17 00:00:00 2001 From: Mark Qvist Date: Fri, 8 Feb 2019 15:48:13 +0100 Subject: [PATCH] Configuration and audio level monitoring implemented --- .gitignore | 1 - hardware/AFSK.c | 18 ++- hardware/LED.c | 9 +- hardware/Serial.c | 4 +- protocol/KISS.c | 119 +++++++++++++++- protocol/KISS.h | 20 ++- testkit/audiolevel.py | 309 ++++++++++++++++++++++++++++++++++++++++++ util/Config.c | 128 ++++++++++++++--- util/Config.h | 18 +++ 9 files changed, 594 insertions(+), 32 deletions(-) create mode 100644 testkit/audiolevel.py diff --git a/.gitignore b/.gitignore index 8f5d990..a4971f4 100755 --- a/.gitignore +++ b/.gitignore @@ -13,7 +13,6 @@ images/*.map images/*.elf images/*.sym images/*.hex -testkit flashdefault flashtestkit flashcurrent diff --git a/hardware/AFSK.c b/hardware/AFSK.c index c652dbf..a4174f8 100755 --- a/hardware/AFSK.c +++ b/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). diff --git a/hardware/LED.c b/hardware/LED.c index 50a0341..d688b82 100644 --- a/hardware/LED.c +++ b/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; - OCR0A = ledIntensity; + config_led_intensity = value; + OCR0A = config_led_intensity; } void LED_COM_ON(void) { diff --git a/hardware/Serial.c b/hardware/Serial.c index 0b76fc9..9d45250 100755 --- a/hardware/Serial.c +++ b/hardware/Serial.c @@ -1,6 +1,7 @@ #include "Serial.h" #include #include +#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) { diff --git a/protocol/KISS.c b/protocol/KISS.c index 174680c..433ef6e 100755 --- a/protocol/KISS.c +++ b/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); } \ No newline at end of file diff --git a/protocol/KISS.h b/protocol/KISS.h index 918b5af..dd008b2 100755 --- a/protocol/KISS.h +++ b/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_FLUSHQUEUE_DEBUG 0x08 -#define CMD_LED_INTENSITY 0x09 +#define CMD_SAVE_CONFIG 0x07 +#define CMD_LED_INTENSITY 0x08 +#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 \ No newline at end of file diff --git a/testkit/audiolevel.py b/testkit/audiolevel.py new file mode 100644 index 0000000..181dc35 --- /dev/null +++ b/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() diff --git a/util/Config.c b/util/Config.c index 7773703..d07e180 100644 --- a/util/Config.c +++ b/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(); - wdt_enable(WDTO_15MS); - while(true) { } + config_soft_reboot(); +} + +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); } \ No newline at end of file diff --git a/util/Config.h b/util/Config.h index 714820e..d97942d 100644 --- a/util/Config.h +++ b/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);