diff --git a/src/deck/drivers/interface/usddeck.h b/src/deck/drivers/interface/usddeck.h
index 30203c7af9..f8fb23be2f 100644
--- a/src/deck/drivers/interface/usddeck.h
+++ b/src/deck/drivers/interface/usddeck.h
@@ -4,18 +4,9 @@
 #include <stdint.h>
 #include <stdbool.h>
 
-#define USDLOG_ACC              (0x01)
-#define USDLOG_GYRO             (0x02)
-#define USDLOG_BARO             (0x04)
-#define USDLOG_MAG              (0x08)
-#define USDLOG_STABILIZER       (0x10)
-#define USDLOG_CONTROL          (0x20)
-#define USDLOG_RANGE            (0x40)
-
 typedef struct usdLogDataPtr_s {
   uint32_t* tick;
-  float* floats;
-  int* ints;
+  uint8_t* data;
 } usdLogQueuePtr_t;
 
 typedef struct usdLogConfig_s {
@@ -23,8 +14,9 @@ typedef struct usdLogConfig_s {
   uint8_t items;
   uint16_t frequency;
   uint8_t bufferSize;
-  uint8_t floatSlots;
-  uint8_t intSlots;
+  uint16_t numSlots;
+  uint16_t numBytes;
+  int* varIds; // dynamically allocated
 } usdLogConfig_t;
 
 #define USD_WRITE(FILE, MESSAGE, BYTES, BYTES_WRITTEN, CRC_VALUE, CRC_FINALXOR, CRC_TABLE) \
diff --git a/src/deck/drivers/src/usddeck.c b/src/deck/drivers/src/usddeck.c
index f7366a0d92..dd7b42bee4 100644
--- a/src/deck/drivers/src/usddeck.c
+++ b/src/deck/drivers/src/usddeck.c
@@ -34,6 +34,7 @@
 
 #include <stdint.h>
 #include <string.h>
+#include <stdlib.h>
 #include "stm32fxxx.h"
 
 #include "FreeRTOS.h"
@@ -194,227 +195,156 @@ static bool isInit = false;
 static void usdInit(DeckInfo *info)
 {
   if (!isInit) {
-      /* create driver structure */
-      FATFS_AddDriver(&fatDrv, 0);
-      vTaskDelay(M2T(100));
-      /* try to mount drives before creating the tasks */
-      if (f_mount(&FatFs, "", 1) == FR_OK)
-        {
-          DEBUG_PRINT("mount SD-Card [OK].\n");
-          /* try to open config file */
-          if (f_open(&logFile, "config", FA_READ) == FR_OK)
-            {
-              /* try to read configuration */
-              char readBuffer[5];
-              unsigned int bytesRead;
-              if (f_read(&logFile, readBuffer, 4, &bytesRead) == FR_OK)
-                {
-                  /* assign bytes to config struct */
-                  usdLogConfig.items = readBuffer[0];
-                  usdLogConfig.frequency = readBuffer[1]<<8 | readBuffer[2];
-                  usdLogConfig.bufferSize = readBuffer[3];
-                  if ( (f_read(&logFile, usdLogConfig.filename, 10, &bytesRead)
-                      != FR_OK) && ~bytesRead )
-                    {
-                      bytesRead = 12;
-                      while(bytesRead--)
-                        usdLogConfig.filename[bytesRead] = '0';
-                      usdLogConfig.filename[12] = '\0';
-                    }
-                  else {
-                      usdLogConfig.filename[bytesRead + 0] = '0';
-                      usdLogConfig.filename[bytesRead + 1] = '0';
-                      usdLogConfig.filename[bytesRead + 2] = '\0';
-                  }
-
-                  f_close(&logFile);
-
-                  usdLogConfig.floatSlots =
-                      ((usdLogConfig.items & USDLOG_ACC) ? 3 : 0)
-#ifdef LOG_SEC_IMU
-                      + ((usdLogConfig.items & USDLOG_ACC) ? 3 : 0)
-#endif
-                      + ((usdLogConfig.items & USDLOG_GYRO) ? 3 : 0)
-#ifdef LOG_SEC_IMU
-                      + ((usdLogConfig.items & USDLOG_GYRO) ? 3 : 0)
-#endif
-                      + ((usdLogConfig.items & USDLOG_BARO) ? 3 : 0)
-                      + ((usdLogConfig.items & USDLOG_MAG) ? 3 : 0)
-                      + ((usdLogConfig.items & USDLOG_STABILIZER) ? 4 : 0)
-                      + ((usdLogConfig.items & USDLOG_CONTROL) ? 3 : 0);
-
-                  usdLogConfig.intSlots =
-                      ((usdLogConfig.items & USDLOG_RANGE) ? 1 : 0);
-
-                  DEBUG_PRINT("Config read [OK].\n");
-                  DEBUG_PRINT("Frequency: %dHz. Buffer size: %d\n",
-                              usdLogConfig.frequency, usdLogConfig.bufferSize);
-                  DEBUG_PRINT("Filename: %s.\n", usdLogConfig.filename);
-
-                  /* create usd-log task */
-                  xTaskCreate(usdLogTask, USDLOG_TASK_NAME,
-                              USDLOG_TASK_STACKSIZE, NULL,
-                              USDLOG_TASK_PRI, NULL);
-                }
-              else
-                DEBUG_PRINT("Config read [FAIL].\n");
+    /* create driver structure */
+    FATFS_AddDriver(&fatDrv, 0);
+    vTaskDelay(M2T(100));
+    /* try to mount drives before creating the tasks */
+    if (f_mount(&FatFs, "", 1) == FR_OK) {
+      DEBUG_PRINT("mount SD-Card [OK].\n");
+      /* try to open config file */
+      bool success = false;
+      while (f_open(&logFile, "config.txt", FA_READ) == FR_OK) {
+        /* try to read configuration */
+        char readBuffer[32];
+        char* endptr;
+        TCHAR* line = f_gets(readBuffer, sizeof(readBuffer), &logFile);
+        if (!line) break;
+        usdLogConfig.frequency = strtol(line, &endptr, 10);
+        // strtol(line, &usdLogConfig.frequency, 10);
+        line = f_gets(readBuffer, sizeof(readBuffer), &logFile);
+        if (!line) break;
+        usdLogConfig.bufferSize = strtol(line, &endptr, 10);
+        // strtol(line, &usdLogConfig.bufferSize, 10);
+        line = f_gets(usdLogConfig.filename, sizeof(usdLogConfig.filename), &logFile);
+        if (!line) break;
+
+        int l = strlen(usdLogConfig.filename);
+        if (l > sizeof(usdLogConfig.filename) - 2) {
+          l = sizeof(usdLogConfig.filename) - 2;
+        }
+        usdLogConfig.filename[l-1] = '0';
+        usdLogConfig.filename[l] = '0';
+        usdLogConfig.filename[l+1] = 0;
+
+        usdLogConfig.numSlots = 0;
+        usdLogConfig.numBytes = 0;
+        while (line) {
+          line = f_gets(readBuffer, sizeof(readBuffer), &logFile);
+          if (!line) break;
+          char* group = line;
+          char* name = 0;
+          for (int i = 0; i < strlen(line); ++i) {
+            if (line[i] == '.') {
+              line[i] = 0;
+              name = &line[i+1];
+              i = strlen(name);
+              if (name[i-1] == '\n') {
+                name[i-1] = 0; // remove newline at the end
+              }
+              break;
             }
+          }
+          int varid = logGetVarId(group, name);
+          if (varid == -1) {
+            DEBUG_PRINT("Unknown log variable %s.%s.\n", group, name);
+            continue;
+          }
+
+          ++usdLogConfig.numSlots;
+          usdLogConfig.numBytes += logVarSize(logGetType(varid));
         }
-      else
-        DEBUG_PRINT("mount SD-Card [FAIL].\n");
-  }
-  isInit = true;
-}
+        f_close(&logFile);
+
+        DEBUG_PRINT("Config read [OK].\n");
+        DEBUG_PRINT("Frequency: %dHz. Buffer size: %d\n",
+                    usdLogConfig.frequency, usdLogConfig.bufferSize);
+        DEBUG_PRINT("Filename: %s.\n", usdLogConfig.filename);
+        DEBUG_PRINT("slots: %d, %d.\n", usdLogConfig.numSlots, usdLogConfig.numBytes);
 
-/* checks if log ids are in usual range */
-static bool checkLogIds(int* idsPtr, uint8_t idsToAdd) {
-  for (int* id = idsPtr; id < idsPtr+idsToAdd; id++) {
-      if (*id < 0)
-        return false;
+        /* create usd-log task */
+        xTaskCreate(usdLogTask, USDLOG_TASK_NAME,
+                    USDLOG_TASK_STACKSIZE, NULL,
+                    USDLOG_TASK_PRI, NULL);
+
+        success = true;
+        break;
+      }
+      
+      if (!success) {
+          DEBUG_PRINT("Config read [FAIL].\n");
+      }
+    }
+    else {
+      DEBUG_PRINT("mount SD-Card [FAIL].\n");
+    }
   }
-  return true;
+  isInit = true;
 }
 
 static void usdLogTask(void* prm)
 {
-  int floatIds[usdLogConfig.floatSlots];
-  int intIds[usdLogConfig.intSlots];
+  TickType_t lastWakeTime = xTaskGetTickCount();
+
+  DEBUG_PRINT("wait for sensors\n");
+
+  /* wait until sensor calibration is done
+   * (memory of bias calculation buffer is free again) */
+  while(!sensorsAreCalibrated()) {
+    vTaskDelayUntil(&lastWakeTime, F2T(10));
+  }
+
+  usdLogConfig.varIds = pvPortMalloc(usdLogConfig.numSlots * sizeof(int*));
+  DEBUG_PRINT("Free heap: %d bytes\n", xPortGetFreeHeapSize());
+
+  // store logging variable ids
   {
-    uint8_t usedSlots = 0;
-    /* acquire log ids */
-    DEBUG_PRINT("Log items:\n");
-    if (usdLogConfig.items & USDLOG_ACC) {
-        floatIds[0] = logGetVarId("acc", "x");
-        floatIds[1] = logGetVarId("acc", "y");
-        floatIds[2] = logGetVarId("acc", "z");
-        if (checkLogIds(&floatIds[usedSlots], 3))
-          {
-            usedSlots += 3;
-            DEBUG_PRINT("* Accel\n");
-        }
-        else
-          usdLogConfig.items &= ~USDLOG_ACC;
-    }
-#ifdef LOG_SEC_IMU
-    if (usdLogConfig.items & USDLOG_ACC)
-      {
-        floatIds[0 + usedSlots] = logGetVarId("accSec", "x");
-        floatIds[1 + usedSlots] = logGetVarId("accSec", "y");
-        floatIds[2 + usedSlots] = logGetVarId("accSec", "z");
-        if (checkLogIds(&floatIds[usedSlots], 3))
-          usedSlots += 3;
-    }
-#endif
-    if (usdLogConfig.items & USDLOG_GYRO)
-      {
-        floatIds[0 + usedSlots] = logGetVarId("gyro", "x");
-        floatIds[1 + usedSlots] = logGetVarId("gyro", "y");
-        floatIds[2 + usedSlots] = logGetVarId("gyro", "z");
-        if (checkLogIds(&floatIds[usedSlots], 3))
-          {
-            usedSlots += 3;
-            DEBUG_PRINT("* Gyro\n");
-        }
-        else
-          usdLogConfig.items &= ~USDLOG_GYRO;
-    }
-#ifdef LOG_SEC_IMU
-    if (usdLogConfig.items & USDLOG_GYRO)
-      {
-        floatIds[0 + usedSlots] = logGetVarId("gyroSec", "x");
-        floatIds[1 + usedSlots] = logGetVarId("gyroSec", "y");
-        floatIds[2 + usedSlots] = logGetVarId("gyroSec", "z");
-        if (checkLogIds(&floatIds[usedSlots], 3))
-          usedSlots += 3;
-    }
-#endif
-    if (usdLogConfig.items & USDLOG_BARO)
-      {
-        floatIds[0 + usedSlots] = logGetVarId("baro", "asl");
-        floatIds[1 + usedSlots] = logGetVarId("baro", "temp");
-        floatIds[2 + usedSlots] = logGetVarId("baro", "pressure");
-        if (checkLogIds(&floatIds[usedSlots], 3))
-          {
-            usedSlots += 3;
-            DEBUG_PRINT("* Baro\n");
-        }
-        else
-          usdLogConfig.items &= ~USDLOG_BARO;
-    }
-    if (usdLogConfig.items & USDLOG_MAG)
-      {
-        floatIds[0 + usedSlots] = logGetVarId("mag", "x");
-        floatIds[1 + usedSlots] = logGetVarId("mag", "y");
-        floatIds[2 + usedSlots] = logGetVarId("mag", "z");
-        if (checkLogIds(&floatIds[usedSlots], 3))
-          {
-            usedSlots += 3;
-            DEBUG_PRINT("* Mag\n");
-        }
-        else
-          usdLogConfig.items &= ~USDLOG_MAG;
-    }
-    if (usdLogConfig.items & USDLOG_STABILIZER)
-      {
-        floatIds[0 + usedSlots] = logGetVarId("stabilizer", "roll");
-        floatIds[1 + usedSlots] = logGetVarId("stabilizer", "pitch");
-        floatIds[2 + usedSlots] = logGetVarId("stabilizer", "yaw");
-        floatIds[3 + usedSlots] = logGetVarId("stabilizer", "thrust");
-        if (checkLogIds(&floatIds[usedSlots], 4))
-          {
-            usedSlots += 4;
-            DEBUG_PRINT("* Stabilizer\n");
-        }
-        else
-          usdLogConfig.items &= ~USDLOG_STABILIZER;
-    }
-    if (usdLogConfig.items & USDLOG_CONTROL)
-      {
-        floatIds[0 + usedSlots] = logGetVarId("ctrltarget", "roll");
-        floatIds[1 + usedSlots] = logGetVarId("ctrltarget", "pitch");
-        floatIds[2 + usedSlots] = logGetVarId("ctrltarget", "yaw");
-        if (checkLogIds(&floatIds[usedSlots], 3))
-          {
-            usedSlots += 3;
-            DEBUG_PRINT("* Control\n");
+    uint32_t idx = 0;
+
+    while (f_open(&logFile, "config.txt", FA_READ) == FR_OK) {
+      /* try to read configuration */
+      char readBuffer[32];
+      TCHAR* line = f_gets(readBuffer, sizeof(readBuffer), &logFile);
+      if (!line) break;
+      line = f_gets(readBuffer, sizeof(readBuffer), &logFile);
+      if (!line) break;
+      line = f_gets(readBuffer, sizeof(readBuffer), &logFile);
+      if (!line) break;
+
+      while (line) {
+        line = f_gets(readBuffer, sizeof(readBuffer), &logFile);
+        if (!line) break;
+        char* group = line;
+        char* name = 0;
+        for (int i = 0; i < strlen(line); ++i) {
+          if (line[i] == '.') {
+            line[i] = 0;
+            name = &line[i+1];
+            i = strlen(name);
+            if (name[i-1] == '\n') {
+              name[i-1] = 0; // remove newline at the end
+            }
+            break;
+          }
         }
-        else
-          usdLogConfig.items &= ~USDLOG_CONTROL;
-    }
-    /* replace number of slots by the calculated one,
-     * ('cause it was purged of unavailable log items) */
-    usdLogConfig.floatSlots = usedSlots;
-    usedSlots = 0;
-    if (usdLogConfig.items & USDLOG_RANGE)
-      {
-        intIds[0] = logGetVarId("range", "zrange");
-        if (checkLogIds(&intIds[usedSlots], 1))
-          {
-            usedSlots += 1;
-            DEBUG_PRINT("* Z-Range\n");
+        int varid = logGetVarId(group, name);
+        if (varid == -1) {
+          continue;
         }
-        else
-          usdLogConfig.items &= ~USDLOG_RANGE;
+
+        usdLogConfig.varIds[idx++] = varid;
+      }
+      break;
     }
-    usdLogConfig.intSlots = usedSlots;
+    f_close(&logFile);
   }
 
-  TickType_t lastWakeTime = xTaskGetTickCount();
-  int i;
-
   /* struct definition for buffering data to write
    * requires up to 100 elements for 1kHz logging */
   struct usdLogStruct {
     uint32_t tick;
-    float floats[usdLogConfig.floatSlots];
-    int ints[usdLogConfig.intSlots];
+    uint8_t data[usdLogConfig.numBytes];
   };
 
-  /* wait until sensor calibration is done
-   * (memory of bias calculation buffer is free again) */
-  while(!sensorsAreCalibrated())
-    vTaskDelayUntil(&lastWakeTime, F2T(10));
-
   /* allocate memory for buffer */
   DEBUG_PRINT("malloc buffer ...\n");
   // vTaskDelay(10); // small delay to allow debug message to be send
@@ -434,39 +364,63 @@ static void usdLogTask(void* prm)
               USDWRITE_TASK_STACKSIZE, usdLogQueue,
               USDWRITE_TASK_PRI, &xHandleWriteTask);
 
-  /*  */
   usdLogQueuePtr_t usdLogQueuePtr;
   uint8_t queueMessagesWaiting = 0;
 
   while(1) {
-      vTaskDelayUntil(&lastWakeTime, F2T(usdLogConfig.frequency));
-      queueMessagesWaiting = (uint8_t)uxQueueMessagesWaiting(usdLogQueue);
-      /* trigger writing once there exists at least one queue item,
-       * frequency will result itself */
-      if (queueMessagesWaiting)
-        vTaskResume(xHandleWriteTask);
-      /* skip if queue is full, one slot will be spared as mutex */
-      if (queueMessagesWaiting == (usdLogConfig.bufferSize - 1))
-        continue;
-
-      /* write data into buffer */
-      usdLogBuffer->tick = lastWakeTime;
-      for (i = usdLogConfig.floatSlots-1; i >= 0; i--)
+    vTaskDelayUntil(&lastWakeTime, F2T(usdLogConfig.frequency));
+    queueMessagesWaiting = (uint8_t)uxQueueMessagesWaiting(usdLogQueue);
+    /* trigger writing once there exists at least one queue item,
+     * frequency will result itself */
+    if (queueMessagesWaiting) {
+      vTaskResume(xHandleWriteTask);
+    }
+    /* skip if queue is full, one slot will be spared as mutex */
+    if (queueMessagesWaiting == (usdLogConfig.bufferSize - 1)) {
+      continue;
+    }
+
+    /* write data into buffer */
+    usdLogBuffer->tick = lastWakeTime;
+    int offset = 0;
+    for (int i = 0; i < usdLogConfig.numSlots; ++i) {
+      int varid = usdLogConfig.varIds[i];
+      switch (logGetType(varid)) {
+        case LOG_UINT8:
+        case LOG_INT8:
         {
-          usdLogBuffer->floats[i] = logGetFloat(floatIds[i]);
-      }
-      for (i = usdLogConfig.intSlots-1; i >= 0; i--)
+          memcpy(&usdLogBuffer->data[offset], logGetAddress(varid), sizeof(uint8_t));
+          offset += sizeof(uint8_t);
+          break;
+        }
+        case LOG_UINT16:
+        case LOG_INT16:
         {
-          usdLogBuffer->ints[i] = logGetInt(intIds[i]);
+          memcpy(&usdLogBuffer->data[offset], logGetAddress(varid), sizeof(uint16_t));
+          offset += sizeof(uint16_t);
+          break;
+        }
+        case LOG_UINT32:
+        case LOG_INT32:
+        case LOG_FLOAT:
+        {
+          memcpy(&usdLogBuffer->data[offset], logGetAddress(varid), sizeof(uint32_t));
+          offset += sizeof(uint32_t);
+          break;
+        }
+        default:
+          ASSERT(false);
       }
-      /* set pointer on latest data and queue */
-      usdLogQueuePtr.tick = &usdLogBuffer->tick;
-      usdLogQueuePtr.floats = usdLogBuffer->floats;
-      usdLogQueuePtr.ints = usdLogBuffer->ints;
-      xQueueSend(usdLogQueue, &usdLogQueuePtr, 0);
-      /* set pointer to next buffer item */
-      if (++usdLogBuffer >= usdLogBufferStart+usdLogConfig.bufferSize)
-        usdLogBuffer = usdLogBufferStart;
+    }
+
+    /* set pointer on latest data and queue */
+    usdLogQueuePtr.tick = &usdLogBuffer->tick;
+    usdLogQueuePtr.data = usdLogBuffer->data;
+    xQueueSend(usdLogQueue, &usdLogQueuePtr, 0);
+    /* set pointer to next buffer item */
+    if (++usdLogBuffer >= usdLogBufferStart+usdLogConfig.bufferSize) {
+      usdLogBuffer = usdLogBufferStart;
+    }
   }
 }
 
@@ -491,75 +445,77 @@ static void usdWriteTask(void* usdLogQueue)
   {
     FILINFO fno;
     uint8_t NUL = 0;
-    while(usdLogConfig.filename[NUL] != '\0')
+    while(usdLogConfig.filename[NUL] != '\0') {
       NUL++;
-    while (f_stat(usdLogConfig.filename, &fno) == FR_OK)
-      {
-        /* increase file */
-        switch(usdLogConfig.filename[NUL-1]) {
-          case '9':
-            usdLogConfig.filename[NUL-1] = '0';
-            usdLogConfig.filename[NUL-2]++;
-            break;
-          default:
-            usdLogConfig.filename[NUL-1]++;
-        }
+    }
+    while (f_stat(usdLogConfig.filename, &fno) == FR_OK) {
+      /* increase file */
+      switch(usdLogConfig.filename[NUL-1]) {
+        case '9':
+          usdLogConfig.filename[NUL-1] = '0';
+          usdLogConfig.filename[NUL-2]++;
+          break;
+        default:
+          usdLogConfig.filename[NUL-1]++;
+      }
     }
   }
+
   /* try to create file */
   if (f_open(&logFile, usdLogConfig.filename, FA_CREATE_ALWAYS | FA_WRITE)
       == FR_OK)
     {
       /* write dataset header */
       {
-        uint8_t logWidth = 1 + usdLogConfig.floatSlots + usdLogConfig.intSlots;
+        uint8_t logWidth = 1 + usdLogConfig.numSlots;
         f_write(&logFile, &logWidth, 1, &bytesWritten);
         crcValue = crcByByte(&logWidth, 1, INITIAL_REMAINDER, 0, crcTable);
       }
-      USD_WRITE(&logFile, (uint8_t*)"Itick", 5, &bytesWritten,
+      USD_WRITE(&logFile, (uint8_t*)"tick(I),", 8, &bytesWritten,
                 crcValue, 0, crcTable)
 
-      if (usdLogConfig.items & USDLOG_ACC)
-        {
-          USD_WRITE(&logFile, (uint8_t*)"faccxfaccyfaccz", 15, &bytesWritten,
-                    crcValue, 0, crcTable)
-#ifdef LOG_SEC_IMU
-          USD_WRITE(&logFile, (uint8_t*)"fac2xfac2yfac2z", 15, &bytesWritten,
-                    crcValue, 0, crcTable)
-#endif
-      }
-
-      if (usdLogConfig.items & USDLOG_GYRO) {
-          USD_WRITE(&logFile, (uint8_t*)"fgyrxfgyryfgyrz", 15, &bytesWritten,
-                    crcValue, 0, crcTable)
-#ifdef LOG_SEC_IMU
-          USD_WRITE(&logFile, (uint8_t*)"fgy2xfgy2yfgy2z", 15, &bytesWritten,
-                    crcValue, 0, crcTable)
-#endif
-      }
-
-      if (usdLogConfig.items & USDLOG_BARO) {
-          USD_WRITE(&logFile, (uint8_t*)"f aslftempfpres", 15, &bytesWritten,
-                    crcValue, 0, crcTable)
-      }
-
-      if (usdLogConfig.items & USDLOG_MAG) {
-          USD_WRITE(&logFile, (uint8_t*)"fmagxfmagyfmagz", 15, &bytesWritten,
+      for (int i = 0; i < usdLogConfig.numSlots; ++i) {
+        char* group;
+        char* name;
+        int varid = usdLogConfig.varIds[i];
+        logGetGroupAndName(varid, &group, &name);
+        USD_WRITE(&logFile, (uint8_t*)group, strlen(group), &bytesWritten,
+          crcValue, 0, crcTable)
+        USD_WRITE(&logFile, (uint8_t*)".", 1, &bytesWritten,
+          crcValue, 0, crcTable)
+        USD_WRITE(&logFile, (uint8_t*)name, strlen(name), &bytesWritten,
+          crcValue, 0, crcTable)
+        USD_WRITE(&logFile, (uint8_t*)"(", 1, &bytesWritten,
                     crcValue, 0, crcTable)
-      }
-
-      if (usdLogConfig.items & USDLOG_STABILIZER) {
-          USD_WRITE(&logFile, (uint8_t*)"fsrolfspitfsyawfsthr", 20,
-                    &bytesWritten, crcValue, 0, crcTable)
-      }
-
-      if (usdLogConfig.items & USDLOG_CONTROL) {
-          USD_WRITE(&logFile, (uint8_t*)"fcrolfcpitfcyaw", 15, &bytesWritten,
+        char typeChar;
+        switch (logGetType(varid)) {
+          case LOG_UINT8:
+            typeChar = 'B';
+            break;
+          case LOG_INT8:
+            typeChar = 'b';
+            break;
+          case LOG_UINT16:
+            typeChar = 'H';
+            break;
+          case LOG_INT16:
+            typeChar = 'h';
+            break;
+          case LOG_UINT32:
+            typeChar = 'I';
+            break;
+          case LOG_INT32:
+            typeChar = 'i';
+            break;
+          case LOG_FLOAT:
+            typeChar = 'f';
+            break;
+          default:
+            ASSERT(false);
+        }
+        USD_WRITE(&logFile, (uint8_t*)&typeChar, 1, &bytesWritten,
                     crcValue, 0, crcTable)
-      }
-
-      if (usdLogConfig.items & USDLOG_RANGE) {
-          USD_WRITE(&logFile, (uint8_t*)"irang", 5, &bytesWritten,
+        USD_WRITE(&logFile, (uint8_t*)"),", 2, &bytesWritten,
                     crcValue, 0, crcTable)
       }
 
@@ -568,66 +524,58 @@ static void usdWriteTask(void* usdLogQueue)
       f_write(&logFile, &crcValue, 4, &bytesWritten);
       f_close(&logFile);
 
-      uint8_t floatBytes = usdLogConfig.floatSlots * 4;
-      uint8_t intBytes = usdLogConfig.intSlots * 4;
       usdLogQueuePtr_t usdLogQueuePtr;
 
       while (1) {
-          /* sleep */
-          vTaskSuspend(NULL);
-          /* determine how many sets can be written */
-          setsToWrite = (uint8_t)uxQueueMessagesWaiting(usdLogQueue);
-          /* try to open file in append mode */
-          if (f_open(&logFile, usdLogConfig.filename, FA_OPEN_APPEND | FA_WRITE)
-              != FR_OK)
-            continue;
-          f_write(&logFile, &setsToWrite, 1, &bytesWritten);
-          crcValue = crcByByte(&setsToWrite, 1, INITIAL_REMAINDER, 0, crcTable);
-          do {
-              /* receive data pointer from queue */
-              xQueueReceive(usdLogQueue, &usdLogQueuePtr, 0);
-              /* write binary data and point on next item */
-              USD_WRITE(&logFile, (uint8_t*)usdLogQueuePtr.tick, 4,
-                        &bytesWritten, crcValue, 0, crcTable)
-              if (usdLogConfig.floatSlots)
-                USD_WRITE(&logFile, (uint8_t*)usdLogQueuePtr.floats,
-                          floatBytes, &bytesWritten, crcValue, 0, crcTable)
-                          if (usdLogConfig.intSlots)
-                            USD_WRITE(&logFile, (uint8_t*)usdLogQueuePtr.ints,
-                                      intBytes, &bytesWritten,
-                                      crcValue, 0, crcTable)
-          } while(--setsToWrite);
-          /* final xor and negate crc value */
-          crcValue = ~(crcValue^FINAL_XOR_VALUE);
-          f_write(&logFile, &crcValue, 4, &bytesWritten);
-          /* close file */
-          f_close(&logFile);
+        /* sleep */
+        vTaskSuspend(NULL);
+        /* determine how many sets can be written */
+        setsToWrite = (uint8_t)uxQueueMessagesWaiting(usdLogQueue);
+        /* try to open file in append mode */
+        if (f_open(&logFile, usdLogConfig.filename, FA_OPEN_APPEND | FA_WRITE)
+            != FR_OK)
+          continue;
+        f_write(&logFile, &setsToWrite, 1, &bytesWritten);
+        crcValue = crcByByte(&setsToWrite, 1, INITIAL_REMAINDER, 0, crcTable);
+        do {
+          /* receive data pointer from queue */
+          xQueueReceive(usdLogQueue, &usdLogQueuePtr, 0);
+          /* write binary data and point on next item */
+          USD_WRITE(&logFile, (uint8_t*)usdLogQueuePtr.tick, 4,
+                    &bytesWritten, crcValue, 0, crcTable)
+          USD_WRITE(&logFile, (uint8_t*)usdLogQueuePtr.data,
+                    usdLogConfig.numBytes, &bytesWritten, crcValue, 0, crcTable)
+        } while(--setsToWrite);
+        /* final xor and negate crc value */
+        crcValue = ~(crcValue^FINAL_XOR_VALUE);
+        f_write(&logFile, &crcValue, 4, &bytesWritten);
+        /* close file */
+        f_close(&logFile);
       }
-  } else f_mount(NULL, "", 0);
+  } else {
+    f_mount(NULL, "", 0);
+  }
   /* something went wrong */
   vTaskDelete(NULL);
 }
 
 static bool usdTest()
 {
-  if (!isInit)
-    {
-      DEBUG_PRINT("Error while initializing uSD deck\n");
-    }
+  if (!isInit) {
+    DEBUG_PRINT("Error while initializing uSD deck\n");
+  }
 #ifdef USD_RUN_DISKIO_FUNCTION_TESTS
   int result;
   extern int test_diskio (BYTE pdrv, UINT ncyc, DWORD* buff, UINT sz_buff);
 
   result = test_diskio(0, 1, (DWORD*)&workBuff, sizeof(workBuff));
-  if (result)
-    {
-      DEBUG_PRINT("(result=%d)\nFatFs diskio functions test [FAIL].\n", result);
-      isInit = false;
-    }
-  else
-    {
-      DEBUG_PRINT("FatFs diskio functions test [OK].\n");
-    }
+  if (result) {
+    DEBUG_PRINT("(result=%d)\nFatFs diskio functions test [FAIL].\n", result);
+    isInit = false;
+  }
+  else {
+    DEBUG_PRINT("FatFs diskio functions test [OK].\n");
+  }
 #endif
 
   return isInit;
diff --git a/src/modules/interface/log.h b/src/modules/interface/log.h
index c2d88b00ef..fae74ed46c 100644
--- a/src/modules/interface/log.h
+++ b/src/modules/interface/log.h
@@ -36,6 +36,10 @@ bool logTest(void);
 
 /* Internal access of log variables */
 int logGetVarId(char* group, char* name);
+int logGetType(int varid);
+void logGetGroupAndName(int varid, char** group, char** name);
+void* logGetAddress(int varid);
+uint8_t logVarSize(int type);
 float logGetFloat(int varid);
 int logGetInt(int varid);
 unsigned int logGetUint(int varid);
diff --git a/src/modules/src/log.c b/src/modules/src/log.c
index 8ce5cb241c..71aabe4517 100644
--- a/src/modules/src/log.c
+++ b/src/modules/src/log.c
@@ -753,6 +753,42 @@ int logGetVarId(char* group, char* name)
   return -1;
 }
 
+int logGetType(int varid)
+{
+  return logs[varid].type;
+}
+
+void logGetGroupAndName(int varid, char** group, char** name)
+{
+  char * currgroup = "";
+  *group = 0;
+  *name = 0;
+
+  for(int i=0; i<logsLen; i++) {
+    if (logs[i].type & LOG_GROUP) {
+      if (logs[i].type & LOG_START) {
+        currgroup = logs[i].name;
+      }
+    }
+
+    if (i == varid) {
+      *group = currgroup;
+      *name = logs[i].name;
+      break;
+    }
+  }
+}
+
+void* logGetAddress(int varid)
+{
+  return logs[varid].address;
+}
+
+uint8_t logVarSize(int type)
+{
+  return typeLength[type];
+}
+
 int logGetInt(int varid)
 {
   int valuei = 0;
diff --git a/tools/usdlog/CF_functions.py b/tools/usdlog/CF_functions.py
index e7aad90031..14df5bda4f 100644
--- a/tools/usdlog/CF_functions.py
+++ b/tools/usdlog/CF_functions.py
@@ -9,9 +9,6 @@
 import numpy as np
 import os
 
-# lookup dictionary to determine size of data types
-fmtChars = {'c': 1, 'b': 1, 'b': 1, 'B': 1, '?': 1, 'h': 2, 'H': 2,
-            'i': 4, 'I': 4, 'l': 4, 'L': 4, 'q': 8, 'Q': 8, 'f': 4, 'd': 8} 
 
 def decode(filName):
     # read file as binary
@@ -25,26 +22,33 @@ def decode(filName):
     # process file header
     setWidth = struct.unpack('B', filCon[:1])
     setNames = []
+    idx = 1
     for ii in range(0, setWidth[0]):
-        setNames.append(filCon[ii*5+1:ii*5+6])
+        startIdx = idx
+        while True:
+            if filCon[idx] == ','.encode('ascii')[0]:
+                break
+            idx += 1
+        print(filCon[startIdx:idx], startIdx, idx)
+        setNames.append(filCon[startIdx:idx])
+        idx += 1
     print("[CRC] of file header:", end="")
-    crcVal = crc32(filCon[0:setWidth[0]*5+1+4]) & 0xffffffff
+    crcVal = crc32(filCon[0:idx+4]) & 0xffffffff
     crcErrors = 0
     if ( crcVal == 0xffffffff):
         print("\tOK\t["+hex(crcVal)+"]")
     else:
         print("\tERROR\t["+hex(crcVal)+"]")
         crcErrors += 1
-    offset = setWidth[0]*5+5
+    offset = idx + 4
     
     # process data sets
-    setCon = np.zeros(statinfo.st_size // 4)
+    setCon = np.zeros(statinfo.st_size) # upper bound...
     idx = 0
-    fmtStr = ""
-    setBytes = 0
+    fmtStr = "<"
     for setName in setNames:
-        fmtStr += chr(setName[0])
-        setBytes += fmtChars[chr(setName[0])]
+        fmtStr += chr(setName[-2])
+    setBytes = struct.calcsize(fmtStr)
     while(offset < len(filCon)):
         setNumber = struct.unpack('B', filCon[offset:offset+1])
         offset += 1
@@ -71,55 +75,5 @@ def decode(filName):
     # create output dictionary
     output = {}
     for ii in range(setWidth[0]):
-        output[setNames[ii][1:].decode("utf-8").strip()] = setCon[ii]
+        output[setNames[ii][0:-3].decode("utf-8").strip()] = setCon[ii]
     return output
-
-def createConfig():
-    import re
-
-    temp = 0
-    
-    print("Which data should be logged?")
-    inStr = input(" * Acceleration ([Y]es / [n]o): ")
-    if ((re.search('^[Yy]', inStr)) or (inStr == '')):
-        temp += 1
-        
-    inStr = input(" * Gyroscope ([Y]es / [n]o): ")
-    if ((re.search('^[Yy]', inStr)) or (inStr == '')):
-        temp += 2
-        
-    inStr = input(" * Barometer ([Y]es / [n]o): ")
-    if ((re.search('^[Yy]', inStr)) or (inStr == '')):
-        temp += 4
-        
-    inStr = input(" * Magnetometer ([Y]es / [n]o): ")
-    if ((re.search('^[Yy]', inStr)) or (inStr == '')):
-        temp += 8
-        
-    inStr = input(" * Stabilizer ([Y]es / [n]o): ")
-    if ((re.search('^[Yy]', inStr)) or (inStr == '')):
-        temp += 16
-        
-    inStr = input(" * Control ([Y]es / [n]o): ")
-    if ((re.search('^[Yy]', inStr)) or (inStr == '')):
-        temp += 32
-        
-    inStr = input(" * Z-Range ([Y]es / [n]o): ")
-    if ((re.search('^[Yy]', inStr)) or (inStr == '')):
-        temp += 64
-        
-    config = temp.to_bytes(1, byteorder='big', signed=False)
-    
-    config += int(input("\nEnter the log frequency [1-1000]: ")).to_bytes(2, byteorder='big', signed=False)
-    
-    config += int(input("Enter buffer size [0-255]: ")).to_bytes(1, byteorder='big', signed=False)
-    
-    config += bytes(input("Filename [max. 10 letters]: ").encode('ascii'))
-    
-    # write config to file
-    filObj = open("config", 'wb')
-    filObj.write(config)
-    filObj.close()
-    
-if __name__=='__main__':
-    createConfig()
\ No newline at end of file
diff --git a/tools/usdlog/config.txt b/tools/usdlog/config.txt
new file mode 100644
index 0000000000..076dcf584a
--- /dev/null
+++ b/tools/usdlog/config.txt
@@ -0,0 +1,23 @@
+250
+50
+log
+acc.x
+acc.y
+acc.z
+gyro.x
+gyro.y
+gyro.z
+baro.asl
+baro.temp
+baro.pressure
+mag.x
+mag.y
+mag.z
+stabilizer.roll
+stabilizer.pitch
+stabilizer.yaw
+stabilizer.thrust
+ctrltarget.roll
+ctrltarget.pitch
+ctrltarget.yaw
+range.zrange
\ No newline at end of file
diff --git a/tools/usdlog/example.py b/tools/usdlog/example.py
index d4a94254fa..f6ff18e9df 100644
--- a/tools/usdlog/example.py
+++ b/tools/usdlog/example.py
@@ -15,7 +15,7 @@
     
 # number of columns and rows for suplot
 plotCols = 1;
-plotRows = 0;
+plotRows = 1;
 
 # let's see which keys exists in current data set
 keys = ""
@@ -24,7 +24,7 @@
 
 # get plot config from user
 plotGyro = 0
-if re.search('gyr', keys):
+if re.search('gyro', keys):
     inStr = input("plot gyro data? ([Y]es / [n]o): ")
     if ((re.search('^[Yy]', inStr)) or (inStr == '')):
         plotGyro = 1
@@ -38,32 +38,32 @@
         plotRows += 1
 
 plotMag = 0
-if re.search('mag', keys):   
+if re.search('mag', keys):
     inStr = input("plot magnetometer data? ([Y]es / [n]o): ")
     if ((re.search('^[Yy]', inStr)) or (inStr == '')):
         plotMag = 1
         plotRows += 1
 
 plotBaro = 0
-if re.search('bar', keys):    
+if re.search('baro', keys):
     inStr = input("plot barometer data? ([Y]es / [n]o): ")
     if ((re.search('^[Yy]', inStr)) or (inStr == '')):
         plotBaro = 1
         plotRows += 1
 
 plotCtrl = 0
-if re.search('crol', keys):
+if re.search('ctrltarget', keys):
     inStr = input("plot control data? ([Y]es / [n]o): ")
     if ((re.search('^[Yy]', inStr)) or (inStr == '')):
         plotCtrl = 1
         plotRows += 1
 
-plotStab = 0       
-if re.search('srol', keys):
+plotStab = 0
+if re.search('stabilizer', keys):
     inStr = input("plot stabilizer data? ([Y]es / [n]o): ")
     if ((re.search('^[Yy]', inStr)) or (inStr == '')):
         plotStab = 1
-        plotRows += 1        
+        plotRows += 1
     
 # current plot for simple subplot usage
 plotCurrent = 0;
@@ -74,9 +74,9 @@
 if plotGyro:
     plotCurrent += 1
     plt.subplot(plotRows, plotCols, plotCurrent)
-    plt.plot(logData['tick'], logData['gyrx'], '-', label='X')
-    plt.plot(logData['tick'], logData['gyry'], '-', label='Y')
-    plt.plot(logData['tick'], logData['gyrz'], '-', label='Z')
+    plt.plot(logData['tick'], logData['gyro.x'], '-', label='X')
+    plt.plot(logData['tick'], logData['gyro.y'], '-', label='Y')
+    plt.plot(logData['tick'], logData['gyro.z'], '-', label='Z')
     plt.xlabel('RTOS Ticks')
     plt.ylabel('Gyroscope [°/s]')
     plt.legend(loc=9, ncol=3, borderaxespad=0.)
@@ -84,9 +84,9 @@
 if plotAccel:
     plotCurrent += 1
     plt.subplot(plotRows, plotCols, plotCurrent)
-    plt.plot(logData['tick'], logData['accx'], '-', label='X')
-    plt.plot(logData['tick'], logData['accy'], '-', label='Y')
-    plt.plot(logData['tick'], logData['accz'], '-', label='Z')
+    plt.plot(logData['tick'], logData['acc.x'], '-', label='X')
+    plt.plot(logData['tick'], logData['acc.y'], '-', label='Y')
+    plt.plot(logData['tick'], logData['acc.z'], '-', label='Z')
     plt.xlabel('RTOS Ticks')
     plt.ylabel('Accelerometer [g]')
     plt.legend(loc=9, ncol=3, borderaxespad=0.)
@@ -95,9 +95,9 @@
 if plotMag:
     plotCurrent += 1
     plt.subplot(plotRows, plotCols, plotCurrent)
-    plt.plot(logData['tick'], logData['magx'], '-', label='X')
-    plt.plot(logData['tick'], logData['magy'], '-', label='Y')
-    plt.plot(logData['tick'], logData['magz'], '-', label='Z')
+    plt.plot(logData['tick'], logData['mag.x'], '-', label='X')
+    plt.plot(logData['tick'], logData['mag.y'], '-', label='Y')
+    plt.plot(logData['tick'], logData['mag.z'], '-', label='Z')
     plt.xlabel('RTOS Ticks')
     plt.ylabel('Magnetometer')
     plt.legend(loc=9, ncol=3, borderaxespad=0.)
@@ -105,22 +105,22 @@
 if plotBaro:
     plotCurrent += 1
     plt.subplot(plotRows, plotCols, plotCurrent)
-    plt.plot(logData['tick'], logData['pres'], '-')
+    plt.plot(logData['tick'], logData['baro.pressure'], '-')
     plt.xlabel('RTOS Ticks')
     plt.ylabel('Pressure [hPa]')
     
     plotCurrent += 1
     plt.subplot(plotRows, plotCols, plotCurrent)
-    plt.plot(logData['tick'], logData['temp'], '-')
+    plt.plot(logData['tick'], logData['baro.temp'], '-')
     plt.xlabel('RTOS Ticks')
     plt.ylabel('Temperature [degC]')
 
 if plotCtrl:
     plotCurrent += 1
     plt.subplot(plotRows, plotCols, plotCurrent)
-    plt.plot(logData['tick'], logData['crol'], '-', label='roll')
-    plt.plot(logData['tick'], logData['cpit'], '-', label='pitch')
-    plt.plot(logData['tick'], logData['cyaw'], '-', label='yaw')
+    plt.plot(logData['tick'], logData['ctrltarget.roll'], '-', label='roll')
+    plt.plot(logData['tick'], logData['ctrltarget.pitch'], '-', label='pitch')
+    plt.plot(logData['tick'], logData['ctrltarget.yaw'], '-', label='yaw')
     plt.xlabel('RTOS Ticks')
     plt.ylabel('Control')
     plt.legend(loc=9, ncol=3, borderaxespad=0.)
@@ -128,10 +128,10 @@
 if plotStab:
     plotCurrent += 1
     plt.subplot(plotRows, plotCols, plotCurrent)
-    plt.plot(logData['tick'], logData['srol'], '-', label='roll')
-    plt.plot(logData['tick'], logData['spit'], '-', label='pitch')
-    plt.plot(logData['tick'], logData['syaw'], '-', label='yaw')
-    plt.plot(logData['tick'], logData['sthr'], '-', label='thrust')
+    plt.plot(logData['tick'], logData['stabilizer.roll'], '-', label='roll')
+    plt.plot(logData['tick'], logData['stabilizer.pitch'], '-', label='pitch')
+    plt.plot(logData['tick'], logData['stabilizer.yaw'], '-', label='yaw')
+    plt.plot(logData['tick'], logData['stabilizer.thrust'], '-', label='thrust')
     plt.xlabel('RTOS Ticks')
     plt.ylabel('Stabilizer')
     plt.legend(loc=9, ncol=4, borderaxespad=0.)