Merge branch 'master' of github.com:bitcraze/crazyflie-firmware

Makefile

@@ -167,7 +167,7 @@ PROJ_OBJ_CF2 += platformservice.o sound_cf2.o extrx.o sysload.o
 
 # Stabilizer modules
 PROJ_OBJ += commander.o crtp_commander.o crtp_commander_rpyt.o
-PROJ_OBJ += crtp_commander_generic.o ext_position.o
+PROJ_OBJ += crtp_commander_generic.o crtp_localization_service.o
 PROJ_OBJ += attitude_pid_controller.o sensfusion6.o stabilizer.o
 PROJ_OBJ += position_estimator_altitude.o position_controller_pid.o
 PROJ_OBJ += estimator_$(ESTIMATOR).o controller_$(CONTROLLER).o

src/deck/drivers/src/lpsTwrTag.c

@@ -26,13 +26,15 @@
 
 
 #include <string.h>
+#include <math.h>
 
 #include "lpsTwrTag.h"
 
 #include "FreeRTOS.h"
 #include "task.h"
 
 #include "log.h"
+#include "crtp_localization_service.h"
 
 #include "stabilizer_types.h"
 #ifdef ESTIMATOR_TYPE_kalman
@@ -235,11 +237,13 @@ static uint32_t twrTagOnEvent(dwDevice_t *dev, uwbEvent_t event)
           options->rangingState |= (1<<current_anchor);
         }
 
+        locSrvSendRangeFloat(current_anchor, NAN);
         failedRanging[current_anchor]++;
       } else {
         options->rangingState |= (1<<current_anchor);
         options->failedRanging[current_anchor] = 0;
 
+        locSrvSendRangeFloat(current_anchor, options->distance[current_anchor]);
         succededRanging[current_anchor]++;
       }
 

src/modules/interface/crtp.h

@@ -42,34 +42,34 @@ typedef enum {
   CRTP_PORT_SETPOINT         = 0x03,
   CRTP_PORT_MEM              = 0x04,
   CRTP_PORT_LOG              = 0x05,
-  CRTP_PORT_POSITION         = 0x06,
+  CRTP_PORT_LOCALIZATION     = 0x06,
   CRTP_PORT_SETPOINT_GENERIC = 0x07,
   CRTP_PORT_PLATFORM         = 0x0D,
   CRTP_PORT_LINK             = 0x0F,
 } CRTPPort;
 
 typedef struct _CRTPPacket
 {
-  uint8_t size;
+  uint8_t size;                         //< Size of data
   union {
     struct {
       union {
-        uint8_t header;
+        uint8_t header;                 //< Header selecting channel and port
         struct {
 #ifndef CRTP_HEADER_COMPAT
-          uint8_t channel     : 2;
+          uint8_t channel     : 2;      //< Selected channel within port
           uint8_t reserved    : 2;
-          uint8_t port        : 4;
+          uint8_t port        : 4;      //< Selected port
 #else
           uint8_t channel  : 2;
           uint8_t port     : 4;
           uint8_t reserved : 2;
 #endif
         };
       };
-      uint8_t data[CRTP_MAX_DATA_SIZE];
+      uint8_t data[CRTP_MAX_DATA_SIZE]; //< Data
     };
-    uint8_t raw[CRTP_MAX_DATA_SIZE+1];
+    uint8_t raw[CRTP_MAX_DATA_SIZE+1];  //< The full packet "raw"
   };
 } __attribute__((packed)) CRTPPacket;
 

src/modules/interface/ext_position.h → src/modules/interface/crtp_localization_service.h

@@ -23,8 +23,8 @@
  *
  */
 
-#ifndef _EXT_POSITION_H_
-#define _EXT_POSITION_H_
+#ifndef _CRTP_LOCALIZATION_SERVICE_H_
+#define _CRTP_LOCALIZATION_SERVICE_H_
 
 #include "stabilizer_types.h"
 
@@ -38,10 +38,19 @@ struct CrtpExtPosition
   float z; // in m
 } __attribute__((packed));
 
-// Set up the callback for the CRTP_PORT_POSITION
-void extPositionInit(void);
+typedef enum
+{
+  RANGE_STREAM_FLOAT = 0,
+  RANGE_STREAM_FP16  = 1,
+} locsrv_t;
+
+// Set up the callback for the CRTP_PORT_LOCALIZATION
+void locSrvInit(void);
 
 // Get the current position from the cache
 bool getExtPosition(state_t *state);
 
-#endif /* _EXT_POSITION_H_ */
+// Send range in float. After 5 ranges it will send the packet.
+void locSrvSendRangeFloat(uint8_t id, float range);
+
+#endif /* _CRTP_LOCALIZATION_SERVICE_H_ */

src/modules/src/comm.c

@@ -41,6 +41,7 @@
 #include "usblink.h"
 #include "platformservice.h"
 #include "syslink.h"
+#include "crtp_localization_service.h"
 
 static bool isInit;
 
@@ -78,7 +79,8 @@ void commInit(void)
 #endif
   logInit();
   paramInit();
-
+  locSrvInit();
+
   //setup CRTP communication channel
   //TODO: check for USB first and prefer USB over radio
   //if (usbTest())

src/modules/src/ext_position.c → src/modules/src/crtp_localization_service.c

@@ -23,18 +23,39 @@
  *
  *
  */
+#include <string.h>
+#include <stdint.h>
 
 #include "FreeRTOS.h"
 #include "task.h"
 
-#include "ext_position.h"
 #include "crtp.h"
+#include "crtp_localization_service.h"
 #include "log.h"
+#include "param.h"
 
 #ifdef ESTIMATOR_TYPE_kalman
 #include "estimator_kalman.h"
 #endif
 
+#define NBR_OF_RANGES_IN_PACKET   5
+
+typedef enum
+{
+  EXT_POSITION  = 0,
+  GENERIC_TYPE  = 1,
+} locsrvChannels_t;
+
+typedef struct
+{
+  uint8_t type;
+  struct
+  {
+    uint8_t id;
+    float range;
+  } __attribute__((packed)) ranges[NBR_OF_RANGES_IN_PACKET];
+} __attribute__((packed)) rangePacket;
+
 /**
  * Position data cache
  */
@@ -48,21 +69,39 @@ typedef struct
 // Struct for logging position information
 static positionMeasurement_t ext_pos;
 static ExtPositionCache crtpExtPosCache;
-static void extPositionCrtpCB(CRTPPacket* pk);
-
+static CRTPPacket pkRange;
+static uint8_t rangeIndex;
+static bool enableRangeStreamFloat = false;
 static bool isInit = false;
 
-void extPositionInit()
+static void locSrvCrtpCB(CRTPPacket* pk);
+static void extPositionHandler(CRTPPacket* pk);
+
+void locSrvInit()
 {
   if (isInit) {
     return;
   }
 
-  crtpRegisterPortCB(CRTP_PORT_POSITION, extPositionCrtpCB);
+  crtpRegisterPortCB(CRTP_PORT_LOCALIZATION, locSrvCrtpCB);
   isInit = true;
 }
 
-static void extPositionCrtpCB(CRTPPacket* pk)
+static void locSrvCrtpCB(CRTPPacket* pk)
+{
+  switch (pk->channel)
+  {
+    case EXT_POSITION:
+      extPositionHandler(pk);
+      break;
+    case GENERIC_TYPE:
+      // TODO: Implement
+    default:
+      break;
+  }
+}
+
+static void extPositionHandler(CRTPPacket* pk)
 {
   crtpExtPosCache.targetVal[!crtpExtPosCache.activeSide] = *((struct CrtpExtPosition*)pk->data);
   crtpExtPosCache.activeSide = !crtpExtPosCache.activeSide;
@@ -87,9 +126,48 @@ bool getExtPosition(state_t *state)
   return false;
 }
 
+void locSrvSendPacket(locsrv_t type, uint8_t *data, uint8_t length)
+{
+  CRTPPacket pk;
+
+  ASSERT(length < CRTP_MAX_DATA_SIZE);
+
+  pk.port = CRTP_PORT_LOCALIZATION;
+  pk.channel = GENERIC_TYPE;
+  memcpy(pk.data, data, length);
+  crtpSendPacket(&pk);
+}
+
+void locSrvSendRangeFloat(uint8_t id, float range)
+{
+  rangePacket *rp = (rangePacket *)pkRange.data;
+
+  ASSERT(rangeIndex <= NBR_OF_RANGES_IN_PACKET);
+
+  if (enableRangeStreamFloat)
+  {
+    rp->ranges[rangeIndex].id = id;
+    rp->ranges[rangeIndex].range = range;
+    rangeIndex++;
+
+    if (rangeIndex >= 5)
+    {
+      rp->type = RANGE_STREAM_FLOAT;
+      pkRange.port = CRTP_PORT_LOCALIZATION;
+      pkRange.channel = GENERIC_TYPE;
+      pkRange.size = sizeof(rangePacket);
+      crtpSendPacket(&pkRange);
+      rangeIndex = 0;
+    }
+  }
+}
+
 LOG_GROUP_START(ext_pos)
   LOG_ADD(LOG_FLOAT, X, &ext_pos.x)
   LOG_ADD(LOG_FLOAT, Y, &ext_pos.y)
   LOG_ADD(LOG_FLOAT, Z, &ext_pos.z)
 LOG_GROUP_STOP(ext_pos)
 
+PARAM_GROUP_START(locSrv)
+PARAM_ADD(PARAM_UINT8, enRangeStreamFP32, &enableRangeStreamFloat)
+PARAM_GROUP_STOP(locSrv)

src/modules/src/stabilizer.c

@@ -36,7 +36,7 @@
 
 #include "sensors.h"
 #include "commander.h"
-#include "ext_position.h"
+#include "crtp_localization_service.h"
 #include "sitaw.h"
 #include "controller.h"
 #include "power_distribution.h"

src/modules/src/system.c

@@ -60,7 +60,6 @@
 #include "buzzer.h"
 #include "sound.h"
 #include "sysload.h"
-#include "ext_position.h"
 
 #ifdef PLATFORM_CF1
 #include "uart_cf1.h"
@@ -171,7 +170,6 @@ void systemTask(void *arg)
   systemInit();
   commInit();
   commanderInit();
-  extPositionInit(); // Set callback for CRTP_PORT_POSITION
   stabilizerInit();
 #ifdef PLATFORM_CF2
   deckInit();

test/deck/drivers/src/TestLpsTwrTag.c

@@ -7,6 +7,7 @@
 #include "unity.h"
 #include "mock_libdw1000.h"
 #include "mock_cfassert.h"
+#include "crtp_localization_serviceMocks.h"
 #include "dw1000Mocks.h"
 
 #ifdef ESTIMATOR_TYPE_kalman

test/testSupport/crtp_localization_serviceMocks.c

@@ -0,0 +1,6 @@
+#include <stdint.h>
+
+void locSrvSendRangeFloat(uint8_t id, float range)
+{
+  // Do nothing
+}