Merge branch 'master' of https://github.com/bitcraze/crazyflie-firmwa…

…re.git

Makefile

@@ -414,3 +414,6 @@ include tools/make/targets.mk
 
 #include dependencies
 -include $(DEPS)
+
+unit:
+	rake unit "DEFINES=$(CFLAGS)" "FILES=$(FILES)"

README.md

@@ -148,9 +148,37 @@ openocd    : Launch OpenOCD
 
 # Unit testing
 
+## Running all unit tests
+
+With the environment set up locally
+
+        make unit
+        
+with the docker builder image and the toolbelt
+
+        tb make unit
+        
+## Running one unit test
+       
+When working with one specific file it is often convinient to run only one unit test
+
+       make unit FILES=test/utils/src/TestNum.c
+
+or with the toolbelt        
+
+       tb make unit FILES=test/utils/src/TestNum.c
+              
+## Running unit tests with specific build settings
+      
+Defines are managed by make and are passed on to the unit test code. Use the 
+normal ways of configuring make when running tests. For instance to run test
+for Crazyflie 1
+
+      make unit PLATFORM=CF1
+
 ## Dependencies
 
-Frameworks for unit testing are pulled in as git submodules.
+Frameworks for unit testing and mocking are pulled in as git submodules.
 
 The testing framework uses ruby and rake to generate and run code. 
 
@@ -159,13 +187,3 @@ image (bitcraze/builder) that contains all tools needed. All scripts in the
 tools/build directory are intended to be run in the image. The 
 [toolbelt](https://wiki.bitcraze.io/projects:dockerbuilderimage:index) makes it
 easy to run the tool scripts.
-
-### Running unit tests
-
-With the environment set up locally
-
-        rake
-
-with the docker builder image and the toolbelt
-
-        tb test

Rakefile

@@ -14,7 +14,14 @@ DEFAULT_CONFIG_FILE = './tools/test/gcc.yml'
 configure_toolchain(DEFAULT_CONFIG_FILE)
 
 task :unit do
-  parse_and_run_tests(ARGV[1..-1])
+  # This prevents all argumets after 'unit' to be interpreted as targets by rake
+  ARGV.each { |a| task a.to_sym do ; end }
+
+  if ARGV.length == 0
+    parse_and_run_tests([])
+  else
+    parse_and_run_tests(ARGV[1..-1])
+  end
 end
 
 desc "Generate test summary"

src/deck/drivers/src/lpsTdoaTag.c

@@ -36,24 +36,35 @@
 #include "estimator_kalman.h"
 #endif // ESTIMATOR_TYPE_kalman
 
-float uwbTdoaDistDiff[LOCODECK_NR_OF_ANCHORS];
-static toaMeasurement_t lastTOA;
 
 static lpsAlgoOptions_t* options;
 
+float uwbTdoaDistDiff[LOCODECK_NR_OF_ANCHORS];
+static toaMeasurement_t lastTOA;
+
 static rangePacket_t rxPacketBuffer[LOCODECK_NR_OF_ANCHORS];
 static dwTime_t arrivals[LOCODECK_NR_OF_ANCHORS];
 
-static double frameTimeInMasterClock = 0.0;
-static double localClockCorrection = 1.0;
+static double frameTime_in_cl_M = 0.0;
+static double clockCorrection_T_To_M = 1.0;
 
-#define MASTER 0
+static int64_t tagClockWrapOffset = 0;
+static int64_t tagClockLatestTime = 0;
 
-#define MEASUREMENT_NOISE_STD 0.5f
+static int64_t masterClockWrapOffset = 0;
+static int64_t masterClockLatestTime = 0;
 
-#define CAP_timer
+typedef struct {
+  int64_t offset;
+  int64_t latestTime;
+} clockWrap_t;
 
-// The maximum diff in distances that we concider to be valid
+static clockWrap_t clockWrapTag, clockWrapMaster;
+
+#define MASTER 0
+#define MEASUREMENT_NOISE_STD 0.5f
+
+// The maximum diff in distances that we consider to be valid
 // Used to sanity check results and remove results that are wrong due to packet loss
 #define MAX_DISTANCE_DIFF (300.0f)
 
@@ -68,44 +79,37 @@ static uint64_t truncateToTimeStamp(uint64_t fullTimeStamp) {
   return fullTimeStamp & 0x00FFFFFFFFFFul;
 }
 
-//tdoaMeasurement_t tdoa;
-//
-//tdoa.stdDev = MEASUREMENT_NOISE_STD;
-//memcpy(&(tdoa.measurement[1]), &TOA, sizeof(toaMeasurement_t));
-//memcpy(&(tdoa.measurement[0]), &lastTOA, sizeof(toaMeasurement_t));
-//memcpy(&lastTOA, &TOA, sizeof(toaMeasurement_t));
-//
-//stateEstimatorEnqueueTDOA(&tdoa);
-//
-// From:Michael Hamer
-// TOA is the current packet that has just come in
-//
-//toaMeasurement_t TOA = {
-//    .rx = sysRxTime,
-//    .tx = rxPacket.sysTxTime,
-//    .x = rxPacket.x,
-//    .y = rxPacket.y,
-//    .z = rxPacket.z,
-//    .senderId = rxPacket.senderID
-//};
-static void enqueueTDOA(uint8_t senderId, int64_t rxT, int64_t txAn_A0time) {
+static int64_t eliminateClockWrap(clockWrap_t* data, int64_t time) {
+  if ((time < data->latestTime)) {
+    data->offset += 0x10000000000;
+  }
+
+  data->latestTime = time;
+
+  return time + data->offset;
+}
+
+static void enqueueTDOA(uint8_t anchor, int64_t rxAn_by_T_in_cl_T, int64_t txAn_in_cl_M) {
   tdoaMeasurement_t tdoa = {.stdDev = MEASUREMENT_NOISE_STD};
 
   memcpy(&(tdoa.measurement[0]), &lastTOA, sizeof(toaMeasurement_t));
 
-  tdoa.measurement[1].senderId = senderId;
-  tdoa.measurement[1].rx = rxT;
-  tdoa.measurement[1].tx = txAn_A0time;
-  tdoa.measurement[1].x = options->anchorPosition[senderId].x;
-  tdoa.measurement[1].y = options->anchorPosition[senderId].y;
-  tdoa.measurement[1].z = options->anchorPosition[senderId].z;
+  tdoa.measurement[1].senderId = anchor;
+  tdoa.measurement[1].rx = eliminateClockWrap(&clockWrapTag, rxAn_by_T_in_cl_T);
+  tdoa.measurement[1].tx = eliminateClockWrap(&clockWrapMaster, txAn_in_cl_M);
+  tdoa.measurement[1].x = options->anchorPosition[anchor].x;
+  tdoa.measurement[1].y = options->anchorPosition[anchor].y;
+  tdoa.measurement[1].z = options->anchorPosition[anchor].z;
 
   memcpy(&lastTOA, &tdoa.measurement[1], sizeof(toaMeasurement_t));
 #ifdef ESTIMATOR_TYPE_kalman
   stateEstimatorEnqueueTDOA(&tdoa);
 #endif
 }
 
+// A note on variable names. They might seem a bit verbose but express quite a lot of information
+// We have three actors: Master (M), Anchor n (An) and the deck on the CF called Tag (T)
+// rxM_by_An_in_cl_An should be interpreted as "The time when packet was received from the Master Anchor by Anchor N expressed in the clock of Anchor N"
 static void rxcallback(dwDevice_t *dev) {
   int dataLength = dwGetDataLength(dev);
   packet_t rxPacket;
@@ -120,48 +124,49 @@ static void rxcallback(dwDevice_t *dev) {
   if (anchor < LOCODECK_NR_OF_ANCHORS) {
     rangePacket_t* packet = (rangePacket_t*)rxPacket.payload;
 
-    if (anchor == MASTER) {
-      frameTimeInMasterClock = truncateToTimeStamp(timestampToUint64(packet->timestamps[MASTER]) - timestampToUint64(rxPacketBuffer[MASTER].timestamps[MASTER]));
-      double frameTimeInLocalClock = truncateToTimeStamp(arrival.full - arrivals[MASTER].full);
+    int64_t rxM_by_T_in_cl_T  = arrivals[MASTER].full;
+    int64_t rxAn_by_T_in_cl_T  = arrival.full;
 
-      localClockCorrection = 1.0;
-      if (frameTimeInLocalClock != 0.0) {
-        localClockCorrection = frameTimeInMasterClock / frameTimeInLocalClock;
+    if (anchor == MASTER) {
+      int64_t previous_txM_in_cl_M = timestampToUint64(rxPacketBuffer[MASTER].timestamps[MASTER]);
+      int64_t txM_in_cl_M = timestampToUint64(packet->timestamps[MASTER]);
+      frameTime_in_cl_M = truncateToTimeStamp(txM_in_cl_M - previous_txM_in_cl_M);
+      double frameTime_in_T = truncateToTimeStamp(rxAn_by_T_in_cl_T - rxM_by_T_in_cl_T);
+
+      clockCorrection_T_To_M = 1.0;
+      if (frameTime_in_T != 0.0) {
+        clockCorrection_T_To_M = frameTime_in_cl_M / frameTime_in_T;
       }
 
-      int64_t txA0_X = timestampToUint64(rxPacketBuffer[MASTER].timestamps[MASTER]);
-      int64_t rxT_0  = arrivals[MASTER].full;
-      enqueueTDOA(MASTER, rxT_0, txA0_X);
+      enqueueTDOA(MASTER, rxAn_by_T_in_cl_T, txM_in_cl_M);
     } else {
-      double frameTimeInAnchorClock = truncateToTimeStamp(timestampToUint64(packet->timestamps[MASTER]) - timestampToUint64(rxPacketBuffer[anchor].timestamps[MASTER]));
-
-      double anchorClockCorrection = 1.0;
-      if (frameTimeInAnchorClock != 0.0) {
-        anchorClockCorrection = frameTimeInMasterClock / frameTimeInAnchorClock;
-      }
+      int64_t previous_txAn_in_cl_An = timestampToUint64(rxPacketBuffer[anchor].timestamps[anchor]);
+      int64_t rxAn_by_M_in_cl_M = timestampToUint64(rxPacketBuffer[MASTER].timestamps[anchor]);
+      int64_t rxM_by_An_in_cl_An = timestampToUint64(packet->timestamps[MASTER]);
+      int64_t txM_in_cl_M = timestampToUint64(rxPacketBuffer[MASTER].timestamps[MASTER]);
 
-      float tdoaDistDiff;
+      int64_t previuos_rxM_by_An_in_cl_An = timestampToUint64(rxPacketBuffer[anchor].timestamps[MASTER]);
 
-      int64_t txAn_X = timestampToUint64(rxPacketBuffer[anchor].timestamps[anchor]);
-      int64_t txA0_X = timestampToUint64(rxPacketBuffer[MASTER].timestamps[MASTER]);
-      int64_t rxAn_0 = timestampToUint64(rxPacketBuffer[MASTER].timestamps[anchor]);
-      int64_t rxA0_n = timestampToUint64(packet->timestamps[MASTER]);
+      int64_t txAn_in_cl_An = timestampToUint64(packet->timestamps[anchor]);
 
-      int64_t rxT_0  = arrivals[MASTER].full;
-      int64_t rxT_n  = arrival.full;
-      int64_t txAn_X2 = timestampToUint64(packet->timestamps[anchor]);
+      double frameTime_in_cl_An = truncateToTimeStamp(rxM_by_An_in_cl_An - previuos_rxM_by_An_in_cl_An);
 
+      double clockCorrection_An_To_M = 1.0;
+      if (frameTime_in_cl_An != 0.0) {
+        clockCorrection_An_To_M = frameTime_in_cl_M / frameTime_in_cl_An;
+      }
 
-      int64_t tA0_n = (((truncateToTimeStamp(rxA0_n - txAn_X) * anchorClockCorrection) - truncateToTimeStamp(txA0_X - rxAn_0))) / 2.0;
-      int64_t txAn_A0time = (tA0_n + truncateToTimeStamp(txAn_X2 - rxA0_n) * anchorClockCorrection);
-      int64_t tT =  truncateToTimeStamp(rxT_n - rxT_0) * localClockCorrection - txAn_A0time;
+      int64_t tof_M_to_An_in_cl_M = (((truncateToTimeStamp(rxM_by_An_in_cl_An - previous_txAn_in_cl_An) * clockCorrection_An_To_M) - truncateToTimeStamp(txM_in_cl_M - rxAn_by_M_in_cl_M))) / 2.0;
+      int64_t delta_txM_to_txAn_in_cl_M = (tof_M_to_An_in_cl_M + truncateToTimeStamp(txAn_in_cl_An - rxM_by_An_in_cl_An) * clockCorrection_An_To_M);
+      int64_t timeDiffOfArrival_in_cl_M =  truncateToTimeStamp(rxAn_by_T_in_cl_T - rxM_by_T_in_cl_T) * clockCorrection_T_To_M - delta_txM_to_txAn_in_cl_M;
+      int64_t txAn_in_cl_M = txM_in_cl_M + delta_txM_to_txAn_in_cl_M;
 
-      tdoaDistDiff = SPEED_OF_LIGHT * tT / LOCODECK_TS_FREQ;
+      float tdoaDistDiff = SPEED_OF_LIGHT * timeDiffOfArrival_in_cl_M / LOCODECK_TS_FREQ;
 
       // Sanity check distances in case of missed packages
       if (tdoaDistDiff > -MAX_DISTANCE_DIFF && tdoaDistDiff < MAX_DISTANCE_DIFF) {
         uwbTdoaDistDiff[anchor] = tdoaDistDiff;
-        enqueueTDOA(anchor, rxT_n, txAn_A0time);
+        enqueueTDOA(anchor, rxAn_by_T_in_cl_T, txAn_in_cl_M);
       }
     }
 
@@ -195,9 +200,30 @@ static uint32_t onEvent(dwDevice_t *dev, uwbEvent_t event) {
   return MAX_TIMEOUT;
 }
 
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-parameter"
 static void Initialize(dwDevice_t *dev, lpsAlgoOptions_t* algoOptions) {
   options = algoOptions;
+
+  // Reset module state. Needed by unit tests
+  memset(uwbTdoaDistDiff, 0, sizeof(uwbTdoaDistDiff));
+  memset(&lastTOA, 0, sizeof(lastTOA));
+  memset(rxPacketBuffer, 0, sizeof(rxPacketBuffer));
+  memset(arrivals, 0, sizeof(arrivals));
+
+  frameTime_in_cl_M = 0.0;
+  clockCorrection_T_To_M = 1.0;
+
+  tagClockWrapOffset = 0;
+  tagClockLatestTime = 0;
+
+  masterClockWrapOffset = 0;
+  masterClockLatestTime = 0;
+
+  memset(&clockWrapTag, 0, sizeof(clockWrapTag));
+  memset(&clockWrapMaster, 0, sizeof(clockWrapMaster));
 }
+#pragma GCC diagnostic pop
 
 uwbAlgorithm_t uwbTdoaTagAlgorithm = {
   .init = Initialize,