From ec4355a8ef0adfc6d8f45b13db6112fd1a63d7d0 Mon Sep 17 00:00:00 2001
From: Wolfgang Hoenig <whoenig@usc.edu>
Date: Fri, 22 Jun 2018 17:18:00 -0700
Subject: [PATCH 1/2] Allow more than 256 logging vars/parameters

This addresses the firmware side of issue #313 by adding additional
CRTP commands (called V2) that uses 16 instead of 8bits for indexing.

This change keeps the old API the same, but caps the number of vars
to 255 to allow older clients to keep working. Eventually, the old
API should be removed.

This change reduced the possible length of variables by 1 byte,
which affected two variables whose name had to be shortened.

Tested with crazyflie_cpp commit 3eb3203. Other clients will need
similar changes to use more than the first 255 variables.
---
 src/modules/src/log.c                         | 203 ++++++++++++-
 src/modules/src/param.c                       | 273 +++++++++++++-----
 src/modules/src/position_estimator_altitude.c |   8 +-
 src/modules/src/sensfusion6.c                 |   8 +-
 4 files changed, 401 insertions(+), 91 deletions(-)

diff --git a/src/modules/src/log.c b/src/modules/src/log.c
index 71aabe4517..7aeba13585 100644
--- a/src/modules/src/log.c
+++ b/src/modules/src/log.c
@@ -47,9 +47,9 @@
 
 #include "console.h"
 #include "cfassert.h"
+#include "debug.h"
 
 #if 0
-#include "debug.h"
 #define LOG_DEBUG(fmt, ...) DEBUG_PRINT("D/log " fmt, ## __VA_ARGS__)
 #define LOG_ERROR(fmt, ...) DEBUG_PRINT("E/log " fmt, ## __VA_ARGS__)
 #else
@@ -97,20 +97,29 @@ struct ops_setting {
     uint8_t id;
 } __attribute__((packed));
 
+struct ops_setting_v2 {
+    uint8_t logType;
+    uint16_t id;
+} __attribute__((packed));
+
 
 #define TOC_CH      0
 #define CONTROL_CH  1
 #define LOG_CH      2
 
-#define CMD_GET_ITEM 0
-#define CMD_GET_INFO 1
+#define CMD_GET_ITEM    0 // original version: up to 255 entries
+#define CMD_GET_INFO    1 // original version: up to 255 entries
+#define CMD_GET_ITEM_V2 2 // version 2: up to 16k entries
+#define CMD_GET_INFO_V2 3 // version 2: up to 16k entries
 
-#define CONTROL_CREATE_BLOCK 0
-#define CONTROL_APPEND_BLOCK 1
-#define CONTROL_DELETE_BLOCK 2
-#define CONTROL_START_BLOCK  3
-#define CONTROL_STOP_BLOCK   4
-#define CONTROL_RESET        5
+#define CONTROL_CREATE_BLOCK    0
+#define CONTROL_APPEND_BLOCK    1
+#define CONTROL_DELETE_BLOCK    2
+#define CONTROL_START_BLOCK     3
+#define CONTROL_STOP_BLOCK      4
+#define CONTROL_RESET           5
+#define CONTROL_CREATE_BLOCK_V2 6
+#define CONTROL_APPEND_BLOCK_V2 7
 
 #define BLOCK_ID_FREE -1
 
@@ -130,7 +139,7 @@ extern struct log_s _log_stop;
 static struct log_s * logs;
 static int logsLen;
 static uint32_t logsCrc;
-static int logsCount = 0;
+static uint16_t logsCount = 0;
 
 static CRTPPacket p;
 
@@ -138,7 +147,9 @@ static bool isInit = false;
 
 /* Log management functions */
 static int logAppendBlock(int id, struct ops_setting * settings, int len);
+static int logAppendBlockV2(int id, struct ops_setting_v2 * settings, int len);
 static int logCreateBlock(unsigned char id, struct ops_setting * settings, int len);
+static int logCreateBlockV2(unsigned char id, struct ops_setting_v2 * settings, int len);
 static int logDeleteBlock(int id);
 static int logStartBlock(int id, unsigned int period);
 static int logStopBlock(int id);
@@ -170,8 +181,8 @@ void logInit(void)
         groupLength = strlen(group);
       }
     } else {
-      // CMD_GET_ITEM result's size is: 3 + strlen(logs[i].name) + groupLength + 2
-      if (strlen(logs[i].name) + groupLength + 2 > 27) {
+      // CMD_GET_ITEM_V2 result's size is: 3 + strlen(logs[i].name) + groupLength + 2
+      if (strlen(logs[i].name) + groupLength + 2 > 26) {
         LOG_ERROR("'%s.%s' too long\n", group, logs[i].name);
         ASSERT_FAILED();
       }
@@ -231,18 +242,24 @@ void logTOCProcess(int command)
 {
   int ptr = 0;
   char * group = "plop";
-  int n=0;
+  uint16_t n=0;
+  uint16_t logId=0;
 
   switch (command)
   {
   case CMD_GET_INFO: //Get info packet about the log implementation
+    DEBUG_PRINT("Client uses old logging API!\n");
     LOG_DEBUG("Packet is TOC_GET_INFO\n");
     ptr = 0;
     group = "";
     p.header=CRTP_HEADER(CRTP_PORT_LOG, TOC_CH);
     p.size=8;
     p.data[0]=CMD_GET_INFO;
-    p.data[1]=logsCount;
+    if (logsCount < 255) {
+      p.data[1]=logsCount;
+    } else {
+      p.data[1]=255;
+    }
     memcpy(&p.data[2], &logsCrc, 4);
     p.data[6]=LOG_MAX_BLOCKS;
     p.data[7]=LOG_MAX_OPS;
@@ -287,6 +304,59 @@ void logTOCProcess(int command)
       crtpSendPacket(&p);
     }
     break;
+  case CMD_GET_INFO_V2: //Get info packet about the log implementation
+    LOG_DEBUG("Packet is TOC_GET_INFO\n");
+    ptr = 0;
+    group = "";
+    p.header=CRTP_HEADER(CRTP_PORT_LOG, TOC_CH);
+    p.size=9;
+    p.data[0]=CMD_GET_INFO_V2;
+    memcpy(&p.data[1], &logsCount, 2);
+    memcpy(&p.data[3], &logsCrc, 4);
+    p.data[7]=LOG_MAX_BLOCKS;
+    p.data[8]=LOG_MAX_OPS;
+    crtpSendPacket(&p);
+    break;
+  case CMD_GET_ITEM_V2:  //Get log variable
+    memcpy(&logId, &p.data[1], 2);
+    LOG_DEBUG("Packet is TOC_GET_ITEM Id: %d\n", logId);
+    for (ptr=0; ptr<logsLen; ptr++) //Ptr points a group
+    {
+      if (logs[ptr].type & LOG_GROUP)
+      {
+        if (logs[ptr].type & LOG_START)
+          group = logs[ptr].name;
+        else
+          group = "";
+      }
+      else                          //Ptr points a variable
+      {
+        if (n==logId)
+          break;
+        n++;
+      }
+    }
+
+    if (ptr<logsLen)
+    {
+      LOG_DEBUG("    Item is \"%s\":\"%s\"\n", group, logs[ptr].name);
+      p.header=CRTP_HEADER(CRTP_PORT_LOG, TOC_CH);
+      p.data[0]=CMD_GET_ITEM_V2;
+      memcpy(&p.data[1], &logId, 2);
+      p.data[3]=logs[ptr].type;
+      p.size=4+2+strlen(group)+strlen(logs[ptr].name);
+      ASSERT(p.size <= CRTP_MAX_DATA_SIZE); // Too long! The name of the group or the parameter may be too long.
+      memcpy(p.data+4, group, strlen(group)+1);
+      memcpy(p.data+4+strlen(group)+1, logs[ptr].name, strlen(logs[ptr].name)+1);
+      crtpSendPacket(&p);
+    } else {
+      LOG_DEBUG("    Index out of range!");
+      p.header=CRTP_HEADER(CRTP_PORT_LOG, TOC_CH);
+      p.data[0]=CMD_GET_ITEM_V2;
+      p.size=1;
+      crtpSendPacket(&p);
+    }
+    break;
   }
 }
 
@@ -319,6 +389,16 @@ void logControlProcess()
       logReset();
       ret = 0;
       break;
+    case CONTROL_CREATE_BLOCK_V2:
+      ret = logCreateBlockV2( p.data[1],
+                            (struct ops_setting_v2*)&p.data[2],
+                            (p.size-2)/sizeof(struct ops_setting_v2) );
+      break;
+    case CONTROL_APPEND_BLOCK_V2:
+      ret = logAppendBlockV2( p.data[1],
+                            (struct ops_setting_v2*)&p.data[2],
+                            (p.size-2)/sizeof(struct ops_setting_v2) );
+      break;
   }
 
   //Commands answer
@@ -356,6 +436,35 @@ static int logCreateBlock(unsigned char id, struct ops_setting * settings, int l
   return logAppendBlock(id, settings, len);
 }
 
+static int logCreateBlockV2(unsigned char id, struct ops_setting_v2 * settings, int len)
+{
+  int i;
+
+  for (i=0; i<LOG_MAX_BLOCKS; i++)
+    if (id == logBlocks[i].id) return EEXIST;
+
+  for (i=0; i<LOG_MAX_BLOCKS; i++)
+    if (logBlocks[i].id == BLOCK_ID_FREE) break;
+
+  if (i == LOG_MAX_BLOCKS)
+    return ENOMEM;
+
+  logBlocks[i].id = id;
+  logBlocks[i].timer = xTimerCreate( "logTimer", M2T(1000),
+                                     pdTRUE, &logBlocks[i], logBlockTimed );
+  logBlocks[i].ops = NULL;
+
+  if (logBlocks[i].timer == NULL)
+  {
+  logBlocks[i].id = BLOCK_ID_FREE;
+  return ENOMEM;
+  }
+
+  LOG_DEBUG("Added block ID %d\n", id);
+
+  return logAppendBlockV2(id, settings, len);
+}
+
 static int blockCalcLength(struct log_block * block);
 static struct log_ops * opsMalloc();
 static void opsFree(struct log_ops * ops);
@@ -428,6 +537,72 @@ static int logAppendBlock(int id, struct ops_setting * settings, int len)
   return 0;
 }
 
+static int logAppendBlockV2(int id, struct ops_setting_v2 * settings, int len)
+{
+  int i;
+  struct log_block * block;
+
+  LOG_DEBUG("Appending %d variable to block %d\n", len, id);
+
+  for (i=0; i<LOG_MAX_BLOCKS; i++)
+    if (logBlocks[i].id == id) break;
+
+  if (i >= LOG_MAX_BLOCKS) {
+    LOG_ERROR("Trying to append block id %d that doesn't exist.", id);
+    return ENOENT;
+  }
+
+  block = &logBlocks[i];
+
+  for (i=0; i<len; i++)
+  {
+    int currentLength = blockCalcLength(block);
+    struct log_ops * ops;
+    int varId;
+
+    if ((currentLength + typeLength[settings[i].logType&0x0F])>LOG_MAX_LEN) {
+      LOG_ERROR("Trying to append a full block. Block id %d.\n", id);
+      return E2BIG;
+    }
+
+    ops = opsMalloc();
+
+    if(!ops) {
+      LOG_ERROR("No more ops memory free!\n");
+      return ENOMEM;
+    }
+
+    if (settings[i].id != 255)  //TOC variable
+    {
+      varId = variableGetIndex(settings[i].id);
+
+      if (varId<0) {
+        LOG_ERROR("Trying to add variable Id %d that does not exists.", settings[i].id);
+        return ENOENT;
+      }
+
+      ops->variable    = logs[varId].address;
+      ops->storageType = logs[varId].type;
+      ops->logType     = settings[i].logType&0x0F;
+
+      LOG_DEBUG("Appended variable %d to block %d\n", settings[i].id, id);
+    } else {                     //Memory variable
+      //TODO: Check that the address is in ram
+      ops->variable    = (void*)(&settings[i]+1);
+      ops->storageType = (settings[i].logType>>4)&0x0F;
+      ops->logType     = settings[i].logType&0x0F;
+      i += 2;
+
+      LOG_DEBUG("Appended var addr 0x%x to block %d\n", (int)ops->variable, id);
+    }
+    blockAppendOps(block, ops);
+
+    LOG_DEBUG("   Now lenght %d\n", blockCalcLength(block));
+  }
+
+  return 0;
+}
+
 static int logDeleteBlock(int id)
 {
   int i;
diff --git a/src/modules/src/param.c b/src/modules/src/param.c
index 3092c6043c..b190fe22da 100644
--- a/src/modules/src/param.c
+++ b/src/modules/src/param.c
@@ -35,9 +35,9 @@
 #include "param.h"
 #include "crc.h"
 #include "console.h"
+#include "debug.h"
 
 #if 0
-#include "debug.h"
 #define PARAM_DEBUG(fmt, ...) DEBUG_PRINT("D/param " fmt, ## __VA_ARGS__)
 #define PARAM_ERROR(fmt, ...) DEBUG_PRINT("E/param " fmt, ## __VA_ARGS__)
 #else
@@ -55,8 +55,10 @@
 #define CMD_GET_NEXT 1
 #define CMD_GET_CRC 2
 
-#define CMD_GET_ITEM 0
-#define CMD_GET_INFO 1
+#define CMD_GET_ITEM    0 // original version: up to 255 entries
+#define CMD_GET_INFO    1 // original version: up to 255 entries
+#define CMD_GET_ITEM_V2 2 // version 2: up to 16k entries
+#define CMD_GET_INFO_V2 3 // version 2: up to 16k entries
 
 #define MISC_SETBYNAME 0
 
@@ -70,8 +72,8 @@ extern struct param_s _param_start;
 extern struct param_s _param_stop;
 
 //The following two function SHALL NOT be called outside paramTask!
-static void paramWriteProcess(int id, void*);
-static void paramReadProcess(int id);
+static void paramWriteProcess();
+static void paramReadProcess();
 static int variableGetIndex(int id);
 static char paramWriteByNameProcess(char* group, char* name, int type, void *valptr);
 
@@ -79,7 +81,10 @@ static char paramWriteByNameProcess(char* group, char* name, int type, void *val
 static struct param_s * params;
 static int paramsLen;
 static uint32_t paramsCrc;
-static int paramsCount = 0;
+static uint16_t paramsCount = 0;
+// indicates if read/write operation use V2 (i.e., 16-bit index)
+// This is set to true, if a client uses TOC_CH in V2
+static bool useV2 = false;
 
 static CRTPPacket p;
 
@@ -111,8 +116,8 @@ void paramInit(void)
         groupLength = strlen(group);
       }
     } else {
-      // CMD_GET_ITEM result's size is: 3 + strlen(params[i].name) + groupLength + 2
-      if (strlen(params[i].name) + groupLength + 2 > 27) {
+      // CMD_GET_ITEM_V2 result's size is: 4 + strlen(params[i].name) + groupLength + 2
+      if (strlen(params[i].name) + groupLength + 2 > 26) {
         PARAM_ERROR("'%s.%s' too long\n", group, params[i].name);
         ASSERT_FAILED();
       }
@@ -156,9 +161,9 @@ void paramTask(void * prm)
 		if (p.channel==TOC_CH)
 		  paramTOCProcess(p.data[0]);
 	  else if (p.channel==READ_CH)
-		  paramReadProcess(p.data[0]);
+		  paramReadProcess();
 		else if (p.channel==WRITE_CH)
-		  paramWriteProcess(p.data[0], &p.data[1]);
+		  paramWriteProcess();
     else if (p.channel==MISC_CH) {
       if (p.data[0] == MISC_SETBYNAME) {
         int i, nzero = 0;
@@ -195,17 +200,23 @@ void paramTOCProcess(int command)
 {
   int ptr = 0;
   char * group = "";
-  int n=0;
+  uint16_t n=0;
+  uint16_t paramId=0;
 
   switch (command)
   {
   case CMD_GET_INFO: //Get info packet about the param implementation
+    DEBUG_PRINT("Client uses old param API!\n");
     ptr = 0;
     group = "";
     p.header=CRTP_HEADER(CRTP_PORT_PARAM, TOC_CH);
     p.size=6;
     p.data[0]=CMD_GET_INFO;
-    p.data[1]=paramsCount;
+    if (paramsCount < 255) {
+      p.data[1]=paramsCount;
+    } else {
+      p.data[1]=255;
+    }
     memcpy(&p.data[2], &paramsCrc, 4);
     crtpSendPacket(&p);
     break;
@@ -245,45 +256,134 @@ void paramTOCProcess(int command)
       crtpSendPacket(&p);
     }
     break;
+  case CMD_GET_INFO_V2: //Get info packet about the param implementation
+    ptr = 0;
+    group = "";
+    p.header=CRTP_HEADER(CRTP_PORT_PARAM, TOC_CH);
+    p.size=7;
+    p.data[0]=CMD_GET_INFO_V2;
+    memcpy(&p.data[1], &paramsCount, 2);
+    memcpy(&p.data[3], &paramsCrc, 4);
+    crtpSendPacket(&p);
+    useV2 = true;
+    break;
+  case CMD_GET_ITEM_V2:  //Get param variable
+    memcpy(&paramId, &p.data[1], 2);
+    for (ptr=0; ptr<paramsLen; ptr++) //Ptr points a group
+    {
+      if (params[ptr].type & PARAM_GROUP)
+      {
+        if (params[ptr].type & PARAM_START)
+          group = params[ptr].name;
+        else
+          group = "";
+      }
+      else                          //Ptr points a variable
+      {
+        if (n==paramId)
+          break;
+        n++;
+      }
+    }
+
+    if (ptr<paramsLen)
+    {
+      p.header=CRTP_HEADER(CRTP_PORT_PARAM, TOC_CH);
+      p.data[0]=CMD_GET_ITEM_V2;
+      memcpy(&p.data[1], &paramId, 2);
+      p.data[3]=params[ptr].type;
+      p.size=4+2+strlen(group)+strlen(params[ptr].name);
+      ASSERT(p.size <= CRTP_MAX_DATA_SIZE); // Too long! The name of the group or the parameter may be too long.
+      memcpy(p.data+4, group, strlen(group)+1);
+      memcpy(p.data+4+strlen(group)+1, params[ptr].name, strlen(params[ptr].name)+1);
+      crtpSendPacket(&p);
+    } else {
+      p.header=CRTP_HEADER(CRTP_PORT_PARAM, TOC_CH);
+      p.data[0]=CMD_GET_ITEM_V2;
+      p.size=1;
+      crtpSendPacket(&p);
+    }
+    break;
   }
 }
 
-static void paramWriteProcess(int ident, void* valptr)
+static void paramWriteProcess()
 {
-  int id;
+  if (useV2) {
+    uint16_t ident;
+    memcpy(&ident, &p.data[0], 2);
 
-  id = variableGetIndex(ident);
+    void* valptr = &p.data[2];
+    int id;
 
-  if (id<0) {
-    p.data[0] = -1;
-    p.data[1] = ident;
-    p.data[2] = ENOENT;
-    p.size = 3;
+    id = variableGetIndex(ident);
 
-    crtpSendPacket(&p);
-    return;
-  }
+    if (id<0) {
+      p.data[2] = ENOENT;
+      p.size = 3;
 
-	if (params[id].type & PARAM_RONLY)
-		return;
+      crtpSendPacket(&p);
+      return;
+    }
 
-  switch (params[id].type & PARAM_BYTES_MASK)
-  {
- 	case PARAM_1BYTE:
- 		*(uint8_t*)params[id].address = *(uint8_t*)valptr;
- 		break;
-    case PARAM_2BYTES:
-  	  *(uint16_t*)params[id].address = *(uint16_t*)valptr;
-      break;
- 	case PARAM_4BYTES:
-      *(uint32_t*)params[id].address = *(uint32_t*)valptr;
-      break;
- 	case PARAM_8BYTES:
-      *(uint64_t*)params[id].address = *(uint64_t*)valptr;
+    if (params[id].type & PARAM_RONLY)
+      return;
+
+    switch (params[id].type & PARAM_BYTES_MASK)
+    {
+    case PARAM_1BYTE:
+      *(uint8_t*)params[id].address = *(uint8_t*)valptr;
       break;
-  }
+      case PARAM_2BYTES:
+        *(uint16_t*)params[id].address = *(uint16_t*)valptr;
+        break;
+    case PARAM_4BYTES:
+        *(uint32_t*)params[id].address = *(uint32_t*)valptr;
+        break;
+    case PARAM_8BYTES:
+        *(uint64_t*)params[id].address = *(uint64_t*)valptr;
+        break;
+    }
 
-  crtpSendPacket(&p);
+    crtpSendPacket(&p);
+  } else {
+    int ident = p.data[0];
+    void* valptr = &p.data[1];
+    int id;
+
+    id = variableGetIndex(ident);
+
+    if (id<0) {
+      p.data[0] = -1;
+      p.data[1] = ident;
+      p.data[2] = ENOENT;
+      p.size = 3;
+
+      crtpSendPacket(&p);
+      return;
+    }
+
+  	if (params[id].type & PARAM_RONLY)
+  		return;
+
+    switch (params[id].type & PARAM_BYTES_MASK)
+    {
+   	case PARAM_1BYTE:
+   		*(uint8_t*)params[id].address = *(uint8_t*)valptr;
+   		break;
+      case PARAM_2BYTES:
+    	  *(uint16_t*)params[id].address = *(uint16_t*)valptr;
+        break;
+   	case PARAM_4BYTES:
+        *(uint32_t*)params[id].address = *(uint32_t*)valptr;
+        break;
+   	case PARAM_8BYTES:
+        *(uint64_t*)params[id].address = *(uint64_t*)valptr;
+        break;
+    }
+
+    crtpSendPacket(&p);
+  }
 }
 
 static char paramWriteByNameProcess(char* group, char* name, int type, void *valptr) {
@@ -337,41 +437,76 @@ static char paramWriteByNameProcess(char* group, char* name, int type, void *val
   return 0;
 }
 
-static void paramReadProcess(int ident)
+static void paramReadProcess()
 {
-  int id;
+  if (useV2) {
+    uint16_t ident;
+    memcpy(&ident, &p.data[0], 2);
+    int id = variableGetIndex(ident);
 
-  id = variableGetIndex(ident);
+    if (id<0) {
+      p.data[2] = ENOENT;
+      p.size = 3;
 
-  if (id<0) {
-    p.data[0] = -1;
-    p.data[1] = ident;
-    p.data[2] = ENOENT;
-    p.size = 3;
+      crtpSendPacket(&p);
+      return;
+    }
 
-    crtpSendPacket(&p);
-    return;
-  }
+    p.data[2] = 0;
+    switch (params[id].type & PARAM_BYTES_MASK)
+    {
+    case PARAM_1BYTE:
+        memcpy(&p.data[3], params[id].address, sizeof(uint8_t));
+        p.size = 3+sizeof(uint8_t);
+        break;
+      break;
+      case PARAM_2BYTES:
+        memcpy(&p.data[3], params[id].address, sizeof(uint16_t));
+        p.size = 3+sizeof(uint16_t);
+        break;
+      case PARAM_4BYTES:
+        memcpy(&p.data[3], params[id].address, sizeof(uint32_t));
+        p.size = 3+sizeof(uint32_t);
+        break;
+      case PARAM_8BYTES:
+        memcpy(&p.data[3], params[id].address, sizeof(uint64_t));
+        p.size = 3+sizeof(uint64_t);
+        break;
+    }
+  } else {
+    uint8_t ident = p.data[0];
+    int id = variableGetIndex(ident);
 
-  switch (params[id].type & PARAM_BYTES_MASK)
-  {
- 	case PARAM_1BYTE:
-   		memcpy(&p.data[1], params[id].address, sizeof(uint8_t));
-   		p.size = 1+sizeof(uint8_t);
-   		break;
- 		break;
-    case PARAM_2BYTES:
-   		memcpy(&p.data[1], params[id].address, sizeof(uint16_t));
-   		p.size = 1+sizeof(uint16_t);
-   		break;
-    case PARAM_4BYTES:
-      memcpy(&p.data[1], params[id].address, sizeof(uint32_t));
-   		p.size = 1+sizeof(uint32_t);
-   		break;
- 	  case PARAM_8BYTES:
-      memcpy(&p.data[1], params[id].address, sizeof(uint64_t));
-   		p.size = 1+sizeof(uint64_t);
+    if (id<0) {
+      p.data[0] = -1;
+      p.data[1] = ident;
+      p.data[2] = ENOENT;
+      p.size = 3;
+
+      crtpSendPacket(&p);
+      return;
+    }
+
+    switch (params[id].type & PARAM_BYTES_MASK)
+    {
+   	case PARAM_1BYTE:
+     		memcpy(&p.data[1], params[id].address, sizeof(uint8_t));
+     		p.size = 1+sizeof(uint8_t);
+     		break;
    		break;
+      case PARAM_2BYTES:
+     		memcpy(&p.data[1], params[id].address, sizeof(uint16_t));
+     		p.size = 1+sizeof(uint16_t);
+     		break;
+      case PARAM_4BYTES:
+        memcpy(&p.data[1], params[id].address, sizeof(uint32_t));
+     		p.size = 1+sizeof(uint32_t);
+     		break;
+   	  case PARAM_8BYTES:
+        memcpy(&p.data[1], params[id].address, sizeof(uint64_t));
+     		p.size = 1+sizeof(uint64_t);
+     		break;
+    }
   }
 
   crtpSendPacket(&p);
diff --git a/src/modules/src/position_estimator_altitude.c b/src/modules/src/position_estimator_altitude.c
index aabf12b8a8..bf46bae9d8 100644
--- a/src/modules/src/position_estimator_altitude.c
+++ b/src/modules/src/position_estimator_altitude.c
@@ -109,16 +109,16 @@ static void positionUpdateVelocityInternal(float accWZ, float dt, struct selfSta
   state->velocityZ *= state->velZAlpha;
 }
 
-LOG_GROUP_START(posEstimatorAlt)
+LOG_GROUP_START(posEstAlt)
 LOG_ADD(LOG_FLOAT, estimatedZ, &state.estimatedZ)
 LOG_ADD(LOG_FLOAT, estVZ, &state.estimatedVZ)
 LOG_ADD(LOG_FLOAT, velocityZ, &state.velocityZ)
-LOG_GROUP_STOP(posEstimatorAlt)
+LOG_GROUP_STOP(posEstAlt)
 
-PARAM_GROUP_START(posEst)
+PARAM_GROUP_START(posEstAlt)
 PARAM_ADD(PARAM_FLOAT, estAlphaAsl, &state.estAlphaAsl)
 PARAM_ADD(PARAM_FLOAT, estAlphaZr, &state.estAlphaZrange)
 PARAM_ADD(PARAM_FLOAT, velFactor, &state.velocityFactor)
 PARAM_ADD(PARAM_FLOAT, velZAlpha, &state.velZAlpha)
 PARAM_ADD(PARAM_FLOAT, vAccDeadband, &state.vAccDeadband)
-PARAM_GROUP_STOP(posEstimatorAlt)
+PARAM_GROUP_STOP(posEstAlt)
diff --git a/src/modules/src/sensfusion6.c b/src/modules/src/sensfusion6.c
index 4a07c6777f..08338323dd 100644
--- a/src/modules/src/sensfusion6.c
+++ b/src/modules/src/sensfusion6.c
@@ -315,7 +315,7 @@ static void estimatedGravityDirection(float* gx, float* gy, float* gz)
   *gz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
 }
 
-LOG_GROUP_START(sensorfusion6)
+LOG_GROUP_START(sensfusion6)
   LOG_ADD(LOG_FLOAT, qw, &q0)
   LOG_ADD(LOG_FLOAT, qx, &q1)
   LOG_ADD(LOG_FLOAT, qy, &q2)
@@ -326,9 +326,9 @@ LOG_GROUP_START(sensorfusion6)
   LOG_ADD(LOG_FLOAT, accZbase, &baseZacc)
   LOG_ADD(LOG_UINT8, isInit, &isInit)
   LOG_ADD(LOG_UINT8, isCalibrated, &isCalibrated)
-LOG_GROUP_STOP(sensorfusion6)
+LOG_GROUP_STOP(sensfusion6)
 
-PARAM_GROUP_START(sensorfusion6)
+PARAM_GROUP_START(sensfusion6)
 #ifdef MADWICK_QUATERNION_IMU
 PARAM_ADD(PARAM_FLOAT, beta, &beta)
 #else // MAHONY_QUATERNION_IMU
@@ -336,4 +336,4 @@ PARAM_ADD(PARAM_FLOAT, kp, &twoKp)
 PARAM_ADD(PARAM_FLOAT, ki, &twoKi)
 #endif
 PARAM_ADD(PARAM_FLOAT, baseZacc, &baseZacc)
-PARAM_GROUP_STOP(sensorfusion6)
+PARAM_GROUP_STOP(sensfusion6)

From 1116d1f0b51d2d651349406edc432327811a813d Mon Sep 17 00:00:00 2001
From: Arnaud Taffanel <arnaud@bitcraze.io>
Date: Tue, 7 Aug 2018 11:07:38 +0200
Subject: [PATCH 2/2]  #313: Bump CRTP protocol version to 4

This indicates that a client can use the new log messages to list up
to 16K variables
---
 src/config/config.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/config/config.h b/src/config/config.h
index 1aab89a0fb..fee2d6bcaa 100644
--- a/src/config/config.h
+++ b/src/config/config.h
@@ -46,7 +46,7 @@
 #include "trace.h"
 #include "usec_time.h"
 
-#define PROTOCOL_VERSION 3
+#define PROTOCOL_VERSION 4
 
 #ifdef STM32F4XX
   #define P_NAME "Crazyflie 2.0"