From acd9444468f856ad0b23d25ccd334418159e814d Mon Sep 17 00:00:00 2001
From: Wolfgang Hoenig <whoenig@usc.edu>
Date: Mon, 18 Mar 2019 16:22:20 -0700
Subject: [PATCH] Simplify logic for external position information.

Tested with the Crazyswarm using a VICON Mocap.
---
 .../interface/crtp_localization_service.h     |  3 -
 src/modules/src/crtp_localization_service.c   | 55 +++++--------------
 src/modules/src/stabilizer.c                  |  1 -
 3 files changed, 15 insertions(+), 44 deletions(-)

diff --git a/src/modules/interface/crtp_localization_service.h b/src/modules/interface/crtp_localization_service.h
index d9e86c038f..237bea1f61 100644
--- a/src/modules/interface/crtp_localization_service.h
+++ b/src/modules/interface/crtp_localization_service.h
@@ -52,9 +52,6 @@ typedef enum
 // Set up the callback for the CRTP_PORT_LOCALIZATION
 void locSrvInit(void);
 
-// Get the current position from the cache
-bool getExtPosition(state_t *state);
-
 // Send range in float. After 5 ranges it will send the packet.
 void locSrvSendRangeFloat(uint8_t id, float range);
 
diff --git a/src/modules/src/crtp_localization_service.c b/src/modules/src/crtp_localization_service.c
index 41aadfaf10..0f74d49704 100644
--- a/src/modules/src/crtp_localization_service.c
+++ b/src/modules/src/crtp_localization_service.c
@@ -70,22 +70,12 @@ typedef struct {
   int16_t z; // mm
 } __attribute__((packed)) extPositionPackedItem;
 
-/**
- * Position data cache
- */
-typedef struct
-{
-  struct CrtpExtPosition targetVal[2];
-  bool activeSide;
-  uint32_t timestamp; // FreeRTOS ticks
-} ExtPositionCache;
-
 // Struct for logging position information
 static positionMeasurement_t ext_pos;
-static ExtPositionCache crtpExtPosCache;
 static CRTPPacket pkRange;
 static uint8_t rangeIndex;
 static bool enableRangeStreamFloat = false;
+static float extPosStdDev = 0.01;
 static bool isInit = false;
 static uint8_t my_id;
 
@@ -125,9 +115,13 @@ static void locSrvCrtpCB(CRTPPacket* pk)
 
 static void extPositionHandler(CRTPPacket* pk)
 {
-  crtpExtPosCache.targetVal[!crtpExtPosCache.activeSide] = *((struct CrtpExtPosition*)pk->data);
-  crtpExtPosCache.activeSide = !crtpExtPosCache.activeSide;
-  crtpExtPosCache.timestamp = xTaskGetTickCount();
+  const struct CrtpExtPosition* data = (const struct CrtpExtPosition*)pk;
+
+  ext_pos.x = data->x;
+  ext_pos.y = data->y;
+  ext_pos.z = data->z;
+  ext_pos.stdDev = extPosStdDev;
+  estimatorEnqueuePosition(&ext_pos);
 }
 
 static void genericLocHandle(CRTPPacket* pk)
@@ -156,37 +150,17 @@ static void extPositionPackedHandler(CRTPPacket* pk)
   for (uint8_t i = 0; i < numItems; ++i) {
     const extPositionPackedItem* item = (const extPositionPackedItem*)&pk->data[i * sizeof(extPositionPackedItem)];
     if (item->id == my_id) {
-      struct CrtpExtPosition position;
-      position.x = item->x / 1000.0f;
-      position.y = item->y / 1000.0f;
-      position.z = item->z / 1000.0f;
-
-      crtpExtPosCache.targetVal[!crtpExtPosCache.activeSide] = position;
-      crtpExtPosCache.activeSide = !crtpExtPosCache.activeSide;
-      crtpExtPosCache.timestamp = xTaskGetTickCount();
+      ext_pos.x = item->x / 1000.0f;
+      ext_pos.y = item->y / 1000.0f;
+      ext_pos.z = item->z / 1000.0f;
+      ext_pos.stdDev = extPosStdDev;
+      estimatorEnqueuePosition(&ext_pos);
 
       break;
     }
   }
 }
 
-bool getExtPosition(state_t *state)
-{
-  // Only use position information if it's valid, recent, and if the kalman filter is enabled
-  if (getStateEstimator() == kalmanEstimator && 
-      (xTaskGetTickCount() - crtpExtPosCache.timestamp) < M2T(5)) {
-    // Get the updated position from the mocap
-    ext_pos.x = crtpExtPosCache.targetVal[crtpExtPosCache.activeSide].x;
-    ext_pos.y = crtpExtPosCache.targetVal[crtpExtPosCache.activeSide].y;
-    ext_pos.z = crtpExtPosCache.targetVal[crtpExtPosCache.activeSide].z;
-    ext_pos.stdDev = 0.01;
-    estimatorEnqueuePosition(&ext_pos);
-
-    return true;
-  }
-  return false;
-}
-
 void locSrvSendPacket(locsrv_t type, uint8_t *data, uint8_t length)
 {
   CRTPPacket pk;
@@ -230,5 +204,6 @@ LOG_GROUP_START(ext_pos)
 LOG_GROUP_STOP(ext_pos)
 
 PARAM_GROUP_START(locSrv)
-PARAM_ADD(PARAM_UINT8, enRangeStreamFP32, &enableRangeStreamFloat)
+  PARAM_ADD(PARAM_UINT8, enRangeStreamFP32, &enableRangeStreamFloat)
+  PARAM_ADD(PARAM_FLOAT, extPosStdDev, &extPosStdDev)
 PARAM_GROUP_STOP(locSrv)
diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c
index ebcc47f10c..28d42ef260 100644
--- a/src/modules/src/stabilizer.c
+++ b/src/modules/src/stabilizer.c
@@ -174,7 +174,6 @@ static void stabilizerTask(void* param)
         controllerType = getControllerType();
       }
 
-      getExtPosition(&state);
       stateEstimator(&state, &sensorData, &control, tick);
       
       commanderGetSetpoint(&setpoint, &state);