Storage; Defragment kve storage when starting up by default

The kve will be defragmented before running the kve sanity check.
Defragmenting before checking will make sure that the kve is sane after defragmentation
and if not the kve will be reformatted.

This can be turned of in the Kconfig, so that users that do not want this to possibly
change their startup time (if eeprom is severely fragmented) can go back to only defragmenting
once kve is full

src/hal/src/Kconfig

@@ -43,4 +43,11 @@ config CPX_UART2_BAUDRATE
   help
       Set the baudrate that will be used for CPX on UART2
 
+config DEFRAG_STORAGE_ON_STARTUP
+    bool "Defrag_on_startup"
+    default y
+    help
+        This enables defragmentation of memory everytime the
+        CPU is started. It increases startup time, depending on
+        fragmentation level.
 endmenu

src/hal/src/storage.c

@@ -54,6 +54,14 @@
 #define KVE_PARTITION_START (1024)
 #define KVE_PARTITION_LENGTH (7*1024)
 
+#define DEFAULT_DEFRAG_ON_STARTUP true
+
+#ifdef CONFIG_DEFRAG_STORAGE_ON_STARTUP
+#define DEFRAG_ON_STARTUP CONFIG_DEFRAG_STORAGE_ON_STARTUP
+#else
+#define DEFRAG_ON_STARTUP DEFAULT_DEFRAG_ON_STARTUP
+#endif
+
 static SemaphoreHandle_t storageMutex;
 
 static size_t readEeprom(size_t address, void* data, size_t length)
@@ -127,6 +135,9 @@ void storageInit()
   storageMutex = xSemaphoreCreateMutex();
 
   isInit = true;
+  if (DEFRAG_ON_STARTUP) {
+    kveDefrag(&kve);
+  }
 }
 
 bool storageTest()