diff --git a/Makefile b/Makefile
index 06132c2f8..09b100d45 100644
--- a/Makefile
+++ b/Makefile
@@ -1008,6 +1008,11 @@ SITLEXCLUDES = \
             drivers/system.c \
             drivers/rcc.c \
             drivers/serial_uart.c \
+            drivers/rx_xn297.c \
+            drivers/display_ug2864hsweg01.c \
+            telemetry/crsf.c \
+            telemetry/srxl.c \
+            io/displayport_oled.c
 
 
 # check if target.mk supplied
@@ -1143,7 +1148,7 @@ CC_SPEED_OPTIMISATION   := $(OPTIMISATION_BASE) $(OPTIMISE_SPEED)
 CC_SIZE_OPTIMISATION    := $(OPTIMISATION_BASE) $(OPTIMISE_SIZE)
 
 else #DEBUG
-OPTIMISE_DEFAULT    := -O0
+OPTIMISE_DEFAULT    := -Og
 
 CC_DEBUG_OPTIMISATION := $(OPTIMISE_DEFAULT)
 
diff --git a/src/main/target/SITL/parameter_group.ld b/src/main/target/SITL/parameter_group.ld
index 210f611be..6f9bfbe96 100644
--- a/src/main/target/SITL/parameter_group.ld
+++ b/src/main/target/SITL/parameter_group.ld
@@ -25,8 +25,7 @@ INSERT AFTER .text;
 
 __FLASH_CONFIG_Size = 0x2000; /* 8kB */
 SECTIONS {
-/*  .FLASH_CONFIG BLOCK( DEFINED(__section_alignment__) ? __section_alignment__ : 4 ) :   SUBALIGN(4)*/
-  .FLASH_CONFIG :
+  .FLASH_CONFIG BLOCK( DEFINED(__section_alignment__) ? __section_alignment__ : 4 ) :
   {
     PROVIDE_HIDDEN (__config_start = . );
     . = . + __FLASH_CONFIG_Size;
diff --git a/src/main/target/SITL/target.c b/src/main/target/SITL/target.c
index a42dc7cc8..c233dafa7 100644
--- a/src/main/target/SITL/target.c
+++ b/src/main/target/SITL/target.c
@@ -436,63 +436,58 @@ char _Min_Stack_Size;
 // fake EEPROM
 extern uint8_t __config_start;
 extern uint8_t __config_end;
-extern uint32_t __FLASH_CONFIG_Size;
 static FILE *eepromFd = NULL;
-const char *EEPROM_FILE = EEPROM_FILENAME;
 
 void FLASH_Unlock(void) {
 	uint8_t * const eeprom = &__config_start;
 
-	if(eepromFd != NULL) {
+	if (eepromFd != NULL) {
 		printf("[FLASH_Unlock] eepromFd != NULL\n");
 		return;
 	}
 
 	// open or create
-	eepromFd = fopen(EEPROM_FILE,"r+");
-	if(eepromFd != NULL) {
-		long lSize;
-		int c;
-
+	eepromFd = fopen(EEPROM_FILENAME,"r+");
+	if (eepromFd != NULL) {
 		// obtain file size:
 		fseek(eepromFd , 0 , SEEK_END);
-		lSize = ftell(eepromFd);
+		long lSize = ftell(eepromFd);
 		rewind(eepromFd);
 
 		printf("[FLASH_Unlock]size = %ld, %ld\n", lSize, (uintptr_t)(&__config_end - &__config_start));
-		for(unsigned int i=0; i<(uintptr_t)(&__config_end - &__config_start); i++){
-			c = fgetc(eepromFd);
+		for (unsigned i = 0; i < (uintptr_t)(&__config_end - &__config_start); i++) {
+		        int c = fgetc(eepromFd);
 			if(c == EOF) break;
 			eeprom[i] = (uint8_t)c;
 		}
-	}else{
-		eepromFd = fopen(EEPROM_FILE, "w+");
-		fwrite(eeprom, sizeof(uint8_t), (size_t)&__FLASH_CONFIG_Size, eepromFd);
+	} else {
+		eepromFd = fopen(EEPROM_FILENAME, "w+");
+		fwrite(eeprom, sizeof(uint8_t), &__config_end - &__config_start, eepromFd);
 	}
 }
+
 void FLASH_Lock(void) {
 	// flush & close
-	if(eepromFd != NULL) {
+	if (eepromFd != NULL) {
 		const uint8_t *eeprom = &__config_start;
 		fseek(eepromFd, 0, SEEK_SET);
-		fwrite(eeprom, sizeof(uint8_t), (size_t)&__FLASH_CONFIG_Size, eepromFd);
-		fflush(eepromFd);
+		fwrite(eeprom, 1, &__config_end - &__config_start, eepromFd);
 		fclose(eepromFd);
 		eepromFd = NULL;
 	}
 }
 
-FLASH_Status FLASH_ErasePage(uint32_t Page_Address) {
+FLASH_Status FLASH_ErasePage(uintptr_t Page_Address) {
 	UNUSED(Page_Address);
 //	printf("[FLASH_ErasePage]%x\n", Page_Address);
 	return FLASH_COMPLETE;
 }
-FLASH_Status FLASH_ProgramWord(uint32_t addr, uint32_t Data) {
-	if((addr >= (uintptr_t)&__config_start)&&(addr < (uintptr_t)&__config_end)) {
-		*((uint32_t*)(uintptr_t)addr) = Data;
-//		printf("[FLASH_ProgramWord]0x%x = %x\n", addr, *((uint32_t*)(uintptr_t)addr));
-	}else{
-		printf("[FLASH_ProgramWord]Out of Range!! 0x%x = %x\n", addr, *((uint32_t*)(uintptr_t)addr));
+FLASH_Status FLASH_ProgramWord(uintptr_t addr, uint32_t Data) {
+	if ((addr >= (uintptr_t)&__config_start)&&(addr < (uintptr_t)&__config_end)) {
+		*((uint32_t*)addr) = Data;
+		printf("[FLASH_ProgramWord]0x%p = %x\n", (void*)addr, *((uint32_t*)addr));
+	} else {
+	        printf("[FLASH_ProgramWord]Out of Range! 0x%p\n", (void*)addr);
 	}
 	return FLASH_COMPLETE;
 }
diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h
index 09409cf78..dc811d924 100644
--- a/src/main/target/SITL/target.h
+++ b/src/main/target/SITL/target.h
@@ -233,8 +233,8 @@ typedef struct {
 
 void FLASH_Unlock(void);
 void FLASH_Lock(void);
-FLASH_Status FLASH_ErasePage(uint32_t Page_Address);
-FLASH_Status FLASH_ProgramWord(uint32_t addr, uint32_t Data);
+FLASH_Status FLASH_ErasePage(uintptr_t Page_Address);
+FLASH_Status FLASH_ProgramWord(uintptr_t addr, uint32_t Data);
 
 uint64_t nanos64_real();
 uint64_t micros64_real();
diff --git a/src/main/target/SITL/udplink.c b/src/main/target/SITL/udplink.c
index ef9fb37cf..8080b7686 100644
--- a/src/main/target/SITL/udplink.c
+++ b/src/main/target/SITL/udplink.c
@@ -8,6 +8,7 @@
 #include <string.h>
 
 #include <fcntl.h>
+#include <sys/socket.h>
 
 #include "udplink.h"