From c1526d6975a3a4711b3a03ebb427060d034552c0 Mon Sep 17 00:00:00 2001
From: Stephan Brown <stephan.brown.07@gmail.com>
Date: Wed, 14 Sep 2016 09:27:27 -0700
Subject: [PATCH 01/14] crtp: Add position port and rename existing commander
 port.

---
 src/modules/interface/crtp.h | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/modules/interface/crtp.h b/src/modules/interface/crtp.h
index 4cf42daee4..fe7ecc8c5a 100644
--- a/src/modules/interface/crtp.h
+++ b/src/modules/interface/crtp.h
@@ -39,9 +39,10 @@
 typedef enum {
   CRTP_PORT_CONSOLE     = 0x00,
   CRTP_PORT_PARAM       = 0x02,
-  CRTP_PORT_COMMANDER   = 0x03,
+  CRTP_PORT_SETPOINT    = 0x03,
   CRTP_PORT_MEM         = 0x04,
   CRTP_PORT_LOG         = 0x05,
+  CRTP_PORT_POSITION    = 0x06,
   CRTP_PORT_PLATFORM    = 0x0D,
   CRTP_PORT_LINK        = 0x0F,
 } CRTPPort;

From 4e76cd82912702e1a9c69b2e87207eede6e359ec Mon Sep 17 00:00:00 2001
From: Stephan Brown <stephan.brown.07@gmail.com>
Date: Wed, 14 Sep 2016 09:28:31 -0700
Subject: [PATCH 02/14] commander: Register callback for the position port and
 add helper for state information.

---
 src/modules/interface/commander.h | 12 ++++++++++
 src/modules/src/commander.c       | 39 ++++++++++++++++++++++++++++++-
 2 files changed, 50 insertions(+), 1 deletion(-)

diff --git a/src/modules/interface/commander.h b/src/modules/interface/commander.h
index b7f4ce7aed..4d5be4702f 100644
--- a/src/modules/interface/commander.h
+++ b/src/modules/interface/commander.h
@@ -50,11 +50,23 @@ struct CommanderCrtpValues
   uint16_t thrust;
 } __attribute__((packed));
 
+struct CommanderCrtpPosition
+{
+  uint8_t valid;
+  float x;
+  float y;
+  float z;
+  float vx;
+  float vy;
+  float vz;
+} __attribute__((packed));
+
 void commanderInit(void);
 bool commanderTest(void);
 uint32_t commanderGetInactivityTime(void);
 void commanderExtrxSet(const struct CommanderCrtpValues* val);
 
 void commanderGetSetpoint(setpoint_t *setpoint, const state_t *state);
+void commanderGetPosition(state_t *state);
 
 #endif /* COMMANDER_H_ */
diff --git a/src/modules/src/commander.c b/src/modules/src/commander.c
index 1d72d962b2..35364481fb 100644
--- a/src/modules/src/commander.c
+++ b/src/modules/src/commander.c
@@ -47,6 +47,16 @@ typedef struct
   uint32_t timestamp; // FreeRTOS ticks
 } CommanderCache;
 
+/**
+ * Commander position data
+ */
+typedef struct
+{
+  struct CommanderCrtpPosition targetVal[2];
+  bool activeSide;
+  uint32_t timestamp; // FreeRTOS ticks
+} CommanderPositionCache;
+
 /**
  * Stabilization modes for Roll, Pitch, Yaw.
  */
@@ -71,6 +81,8 @@ static CommanderCache crtpCache;
 static CommanderCache extrxCache;
 static CommanderCache* activeCache;
 
+static CommanderPositionCache crtpPosCache;
+
 static uint32_t lastUpdate;
 static bool isInactive;
 static bool thrustLocked;
@@ -176,6 +188,13 @@ static void commanderCrtpCB(CRTPPacket* pk)
   }
 }
 
+static void commanderPositionCrtpCB(CRTPPacket* pk)
+{
+  crtpPosCache.targetVal[!crtpPosCache.activeSide] = *((struct CommanderCrtpPosition*)pk->data);
+  crtpPosCache.activeSide = !crtpPosCache.activeSide;
+  crtpPosCache.timestamp = xTaskGetTickCount();
+}
+
 /**
  * Rotate Yaw so that the Crazyflie will change what is considered front.
  *
@@ -256,7 +275,8 @@ void commanderInit(void)
   }
 
   crtpInit();
-  crtpRegisterPortCB(CRTP_PORT_COMMANDER, commanderCrtpCB);
+  crtpRegisterPortCB(CRTP_PORT_SETPOINT, commanderCrtpCB);
+  crtpRegisterPortCB(CRTP_PORT_POSITION, commanderPositionCrtpCB);
 
   activeCache = &crtpCache;
   lastUpdate = xTaskGetTickCount();
@@ -371,6 +391,23 @@ void commanderGetSetpoint(setpoint_t *setpoint, const state_t *state)
   }
 }
 
+void commanderGetPosition(state_t *state)
+{
+  // Only use position information if it's valid and recent
+  if (crtpPosCache.targetVal[crtpPosCache.activeSide].valid &&
+          (xTaskGetTickCount() - crtpPosCache.timestamp) < M2T(2)) {
+    // Update position information
+    state->position.x = crtpPosCache.targetVal[crtpPosCache.activeSide].x;
+    state->position.y = crtpPosCache.targetVal[crtpPosCache.activeSide].y;
+    state->position.z = crtpPosCache.targetVal[crtpPosCache.activeSide].z;
+
+    // Update velocity information
+    state->velocity.x = crtpPosCache.targetVal[crtpPosCache.activeSide].vx;
+    state->velocity.y = crtpPosCache.targetVal[crtpPosCache.activeSide].vy;
+    state->velocity.z = crtpPosCache.targetVal[crtpPosCache.activeSide].vz;
+  }
+}
+
 // Params for flight modes
 PARAM_GROUP_START(flightmode)
 PARAM_ADD(PARAM_UINT8, althold, &altHoldMode)

From 03e5ae5868bd05174c922284b9d60a94d275ff5e Mon Sep 17 00:00:00 2001
From: Stephan Brown <stephan.brown.07@gmail.com>
Date: Wed, 14 Sep 2016 09:29:00 -0700
Subject: [PATCH 03/14] stabilizer: Get position update from commander.

---
 src/modules/src/stabilizer.c | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c
index 0860146904..3a3b1ce622 100644
--- a/src/modules/src/stabilizer.c
+++ b/src/modules/src/stabilizer.c
@@ -118,6 +118,7 @@ static void stabilizerTask(void* param)
 #endif
 
     commanderGetSetpoint(&setpoint, &state);
+    commanderGetPosition(&state);
 
     sitAwUpdateSetpoint(&setpoint, &sensorData, &state);
 

From ce7d92967476fba7071718b1021ab3b1b055c2b9 Mon Sep 17 00:00:00 2001
From: Stephan Brown <stephan.brown.07@gmail.com>
Date: Wed, 14 Sep 2016 14:58:11 -0700
Subject: [PATCH 04/14] commander: Remove velocity items from the packet.

---
 src/modules/interface/commander.h | 4 ----
 src/modules/src/commander.c       | 8 +-------
 2 files changed, 1 insertion(+), 11 deletions(-)

diff --git a/src/modules/interface/commander.h b/src/modules/interface/commander.h
index 4d5be4702f..22a391e99f 100644
--- a/src/modules/interface/commander.h
+++ b/src/modules/interface/commander.h
@@ -52,13 +52,9 @@ struct CommanderCrtpValues
 
 struct CommanderCrtpPosition
 {
-  uint8_t valid;
   float x;
   float y;
   float z;
-  float vx;
-  float vy;
-  float vz;
 } __attribute__((packed));
 
 void commanderInit(void);
diff --git a/src/modules/src/commander.c b/src/modules/src/commander.c
index 35364481fb..c25e796efc 100644
--- a/src/modules/src/commander.c
+++ b/src/modules/src/commander.c
@@ -394,17 +394,11 @@ void commanderGetSetpoint(setpoint_t *setpoint, const state_t *state)
 void commanderGetPosition(state_t *state)
 {
   // Only use position information if it's valid and recent
-  if (crtpPosCache.targetVal[crtpPosCache.activeSide].valid &&
-          (xTaskGetTickCount() - crtpPosCache.timestamp) < M2T(2)) {
+  if ((xTaskGetTickCount() - crtpPosCache.timestamp) < M2T(2)) {
     // Update position information
     state->position.x = crtpPosCache.targetVal[crtpPosCache.activeSide].x;
     state->position.y = crtpPosCache.targetVal[crtpPosCache.activeSide].y;
     state->position.z = crtpPosCache.targetVal[crtpPosCache.activeSide].z;
-
-    // Update velocity information
-    state->velocity.x = crtpPosCache.targetVal[crtpPosCache.activeSide].vx;
-    state->velocity.y = crtpPosCache.targetVal[crtpPosCache.activeSide].vy;
-    state->velocity.z = crtpPosCache.targetVal[crtpPosCache.activeSide].vz;
   }
 }
 

From d25d891db09682e209d125e8a6607e77ec51eb94 Mon Sep 17 00:00:00 2001
From: Stephan Brown <stephan.brown.07@gmail.com>
Date: Thu, 15 Sep 2016 09:53:49 -0700
Subject: [PATCH 05/14] Merge branch 'master' into master.crtp_position_update

---
 src/modules/src/sensors_task.c | 183 ++++++++++++++++++++++-----------
 1 file changed, 124 insertions(+), 59 deletions(-)

diff --git a/src/modules/src/sensors_task.c b/src/modules/src/sensors_task.c
index 569a8564a3..2480ab58dd 100644
--- a/src/modules/src/sensors_task.c
+++ b/src/modules/src/sensors_task.c
@@ -44,6 +44,7 @@
 #include "task.h"
 
 #include "system.h"
+#include "param.h"
 #include "debug.h"
 #include "imu.h"
 #include "nvicconf.h"
@@ -84,9 +85,13 @@ static bool sensorBiasFound = false;
 static bool isBarometerPresent = false;
 static bool isMagnetometerPresent = false;
 
+static bool isMpu6500TestPassed = true;
+static bool isAK8963TestPassed = true;
+static bool isLPS25HTestPassed = true;
+
 #define MPU6500_BUFF_LEN 14
 #define MAG_BUFF_LEN 8
-#define BARO_BUFF_LEN 5
+#define BARO_BUFF_LEN 6
 
 // This buffer needs to hold data from all sensors
 static uint8_t buffer[MPU6500_BUFF_LEN + MAG_BUFF_LEN + BARO_BUFF_LEN] = {0};
@@ -94,6 +99,7 @@ static uint8_t buffer[MPU6500_BUFF_LEN + MAG_BUFF_LEN + BARO_BUFF_LEN] = {0};
 static void processAccGyroMeasurements(const uint8_t *buffer);
 static void processMagnetometerMeasurements(const uint8_t *buffer);
 static void processBarometerMeasurements(const uint8_t *buffer);
+static void sensorsSetupSlaveRead(void);
 
 bool sensorsReadGyro(Axis3f *gyro) {
   return (pdTRUE == xQueueReceive(gyroDataQueue, gyro, 0));
@@ -128,6 +134,8 @@ static void sensorsTask(void *param)
 {
   systemWaitStart();
 
+  sensorsSetupSlaveRead();
+
   while (1)
   {
     if (pdTRUE == xSemaphoreTake(sensorsDataReady, portMAX_DELAY))
@@ -163,8 +171,14 @@ void processBarometerMeasurements(const uint8_t *buffer)
   static uint32_t rawPressure = 0;
   static int16_t rawTemp = 0;
 
-  rawPressure = ((uint32_t) buffer[2] << 16) | ((uint32_t) buffer[1] << 8) | buffer[0];
-  rawTemp = ((int16_t) buffer[4] << 8) | buffer[3];
+  // Check if there is a new pressure update
+  if (buffer[0] & 0x02) {
+    rawPressure = ((uint32_t) buffer[3] << 16) | ((uint32_t) buffer[2] << 8) | buffer[1];
+  }
+  // Check if there is a new temp update
+  if (buffer[0] & 0x01) {
+    rawTemp = ((int16_t) buffer[5] << 8) | buffer[4];
+  }
 
   sensors.baro.pressure = (float) rawPressure / LPS25H_LSB_PER_MBAR;
   sensors.baro.temperature = LPS25H_TEMP_OFFSET + ((float) rawTemp / LPS25H_LSB_PER_CELSIUS);
@@ -264,7 +278,7 @@ static void sensorsDeviceInit(void) {
   mpu6500SetTempSensorEnabled(true);
   // Disable interrupts
   mpu6500SetIntEnabled(false);
-  // Connect the HMC5883L to the main I2C bus
+  // Connect the MAG and BARO to the main I2C bus
   mpu6500SetI2CBypassEnabled(true);
   // Set gyro full scale range
   mpu6500SetFullScaleGyroRange(IMU_GYRO_FS_CFG);
@@ -285,24 +299,48 @@ static void sensorsDeviceInit(void) {
   mpu6500SetDLPFMode(MPU6500_DLPF_BW_98);
 #endif
 
-  // delay 3 seconds until the quad has stabilized enough to pass the test
-  bool mpu6500SelfTestPassed = false;
-  for (int i=0; i<300; i++)
+
+#ifdef IMU_ENABLE_MAG_AK8963
+  ak8963Init(I2C3_DEV);
+  if (ak8963TestConnection() == true)
   {
-    if(mpu6500SelfTest() == true)
-    {
-      mpu6500SelfTestPassed = true;
-      break;
-    }
-    else
-    {
-      vTaskDelay(M2T(10));
-    }
+    isMagnetometerPresent = true;
+    ak8963SetMode(AK8963_MODE_16BIT | AK8963_MODE_CONT2); // 16bit 100Hz
+    DEBUG_PRINT("AK8963 I2C connection [OK].\n");
   }
-  configASSERT(mpu6500SelfTestPassed);
+  else
+  {
+    DEBUG_PRINT("AK8963 I2C connection [FAIL].\n");
+  }
+#endif
 
+#ifdef IMU_ENABLE_PRESSURE_LPS25H
+  lps25hInit(I2C3_DEV);
+  if (lps25hTestConnection() == true)
+  {
+    lps25hSetEnabled(true);
+    isBarometerPresent = true;
+    DEBUG_PRINT("LPS25H I2C connection [OK].\n");
+  }
+  else
+  {
+    //TODO: Should sensor test fail hard if no connection
+    DEBUG_PRINT("LPS25H I2C connection [FAIL].\n");
+  }
+#endif
+}
+
+
+static void sensorsSetupSlaveRead(void)
+{
   // Now begin to set up the slaves
+#ifdef IMU_MPU6500_DLPF_256HZ
+  // As noted in registersheet 4.4: "Data should be sampled at or above sample rate;
+  // SMPLRT_DIV is only used for 1kHz internal sampling." Slowest update rate is then 500Hz.
+  mpu6500SetSlave4MasterDelay(15); // read slaves at 500Hz = (8000Hz / (1 + 15))
+#else
   mpu6500SetSlave4MasterDelay(4); // read slaves at 100Hz = (500Hz / (1 + 4))
+#endif
 
   mpu6500SetI2CBypassEnabled(false);
   mpu6500SetI2CMasterModeEnabled(true);
@@ -311,63 +349,42 @@ static void sensorsDeviceInit(void) {
   mpu6500SetInterruptDrive(0); // push pull
   mpu6500SetInterruptLatch(0); // latched until clear
   mpu6500SetInterruptLatchClear(1); // cleared on any register read
+  mpu6500SetSlaveReadWriteTransitionEnabled(false); // Send a stop at the end of a slave read
+  mpu6500SetMasterClockSpeed(13); // Set i2c speed to 400kHz
 
 #ifdef IMU_ENABLE_MAG_AK8963
-  // Reset AK8963 through MPU6500 master
-  mpu6500SetSlaveAddress(0, AK8963_ADDRESS_00); // set the magnetometer to Slave 0, enable read
-  mpu6500SetSlaveDataLength(0, 1); // Only modify control register at a time
-  mpu6500SetSlaveRegister(0, AK8963_RA_CNTL2); // reset AK8963
-  mpu6500SetSlaveOutputByte(0, 0x01);
-  mpu6500SetSlaveEnabled(0, true);
-
-  // Read WIA from the AK8963 through the MPU6500 master
-  mpu6500SetSlaveAddress(0, 0x80 | AK8963_ADDRESS_00); // set the magnetometer to Slave 0, enable read
-  mpu6500SetSlaveRegister(0, AK8963_RA_WIA);
-  mpu6500SetSlaveDataLength(0, 1); // Only modify control register at a time
-  mpu6500SetSlaveEnabled(0, true);
-  if (mpu6500GetExternalSensorByte(0) == 0x48) {
-    isMagnetometerPresent = true;
-    DEBUG_PRINT("AK8963 Slave Connection [OK].\n");
-
-    // Set operation mode for AK8963 through MPU6500 master
-    mpu6500SetSlaveAddress(0, AK8963_ADDRESS_00); // set the magnetometer to Slave 0, enable read
-    mpu6500SetSlaveRegister(0, AK8963_RA_CNTL);
-    mpu6500SetSlaveOutputByte(0, AK8963_MODE_16BIT | AK8963_MODE_CONT2); // 16bit 100Hz
-    mpu6500SetSlaveEnabled(0, true);
-
+  if (isMagnetometerPresent) {
     // Set registers for MPU6500 master to read from
     mpu6500SetSlaveAddress(0, 0x80 | AK8963_ADDRESS_00); // set the magnetometer to Slave 0, enable read
     mpu6500SetSlaveRegister(0, AK8963_RA_ST1); // read the magnetometer heading register
-    mpu6500SetSlaveDataLength(0, 8); // read 7 bytes (x, y, z heading, ST2 (overflow check))
-    mpu6500SetSlaveEnabled(0, true);
+    mpu6500SetSlaveDataLength(0, MAG_BUFF_LEN); // read 8 bytes (ST1, x, y, z heading, ST2 (overflow check))
     mpu6500SetSlaveDelayEnabled(0, true);
-
-  } else {
-      DEBUG_PRINT("AK8963 Slave Connection [FAIL].\n");
+    mpu6500SetSlaveEnabled(0, true);
   }
 #endif
 
 #ifdef IMU_ENABLE_PRESSURE_LPS25H
-  lps25hInit(I2C3_DEV);
-  if (lps25hTestConnection() == true)
+  if (isBarometerPresent)
   {
-    lps25hSetEnabled(true);
-    isBarometerPresent = true;
-    configASSERT(lps25hSelfTest());
-    DEBUG_PRINT("LPS25H I2C connection [OK].\n");
-    mpu6500SetSlaveAddress(1, 0x80 | LPS25H_I2C_ADDR); // set the barometer to Slave 1, enable read
-    mpu6500SetSlaveRegister(1, LPS25H_PRESS_OUT_XL | LPS25H_ADDR_AUTO_INC);
-    mpu6500SetSlaveDataLength(1, 5);
+    // Configure the LPS25H as a slave and enable read
+    mpu6500SetSlaveAddress(1, 0x80 | LPS25H_I2C_ADDR);
+    mpu6500SetSlaveRegister(1, LPS25H_STATUS_REG | LPS25H_ADDR_AUTO_INC);
+    mpu6500SetSlaveDataLength(1, BARO_BUFF_LEN);
     mpu6500SetSlaveDelayEnabled(1, true);
     mpu6500SetSlaveEnabled(1, true);
   }
-  else
-  {
-    //TODO: Should sensor test fail hard if no connection
-    DEBUG_PRINT("LPS25H I2C connection [FAIL].\n");
-  }
 #endif
 
+  // Enable sensors after configuration
+//  if (isMagnetometerPresent == true) {
+//    mpu6500SetSlaveEnabled(0, false);
+//    vTaskDelay(M2T(1));
+//  }
+//  if (isBarometerPresent == true) {
+//    mpu6500SetSlaveEnabled(1, true);
+//    vTaskDelay(M2T(1));
+//  }
+
   mpu6500SetIntDataReadyEnabled(true);
 }
 
@@ -426,12 +443,49 @@ void sensorsInit(void)
 
 bool sensorsTest(void)
 {
+  bool mpu6500SelfTestPassed = false;
+  bool testStatus = true;
+
   if (!isInit)
   {
     DEBUG_PRINT("Error while initializing sensor task\r\n");
+    testStatus = false;
   }
 
-  return isInit;
+  // delay 3 seconds until the quad has stabilized enough to pass the test
+  for (int i=0; i<300; i++)
+  {
+    if(mpu6500SelfTest() == true)
+    {
+      mpu6500SelfTestPassed = true;
+      break;
+    }
+    else
+    {
+      vTaskDelay(M2T(10));
+    }
+  }
+  testStatus &= mpu6500SelfTestPassed;
+
+#ifdef IMU_ENABLE_MAG_AK8963
+  testStatus &= isMagnetometerPresent;
+  if (testStatus)
+  {
+    isAK8963TestPassed = ak8963SelfTest();
+    testStatus = isAK8963TestPassed;
+  }
+#endif
+
+#ifdef IMU_ENABLE_PRESSURE_LPS25H
+  testStatus &= isBarometerPresent;
+  if (testStatus)
+  {
+    isLPS25HTestPassed = lps25hSelfTest();
+    testStatus = isLPS25HTestPassed;
+  }
+#endif
+
+  return testStatus;
 }
 
 void __attribute__((used)) EXTI13_Callback(void) {
@@ -443,3 +497,14 @@ void __attribute__((used)) EXTI13_Callback(void) {
     portYIELD();
   }
 }
+
+PARAM_GROUP_START(imu_sensors)
+PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, HMC5883L, &isMagnetometerPresent)
+PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, MS5611, &isBarometerPresent) // TODO: Rename MS5611 to LPS25H. Client needs to be updated at the same time.
+PARAM_GROUP_STOP(imu_sensors)
+
+PARAM_GROUP_START(imu_tests)
+PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, MPU6500, &isMpu6500TestPassed)
+PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, HMC5883L, &isAK8963TestPassed)
+PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, MS5611, &isLPS25HTestPassed) // TODO: Rename MS5611 to LPS25H. Client needs to be updated at the same time.
+PARAM_GROUP_STOP(imu_tests)

From 5824230172266445fe71031e0bdccfa46558f0cf Mon Sep 17 00:00:00 2001
From: Casey Carlin <casey.carlin@us.panasonic.com>
Date: Fri, 16 Sep 2016 14:19:07 -0700
Subject: [PATCH 06/14] commander: Log position info and send it to the EKF.

---
 src/modules/interface/commander.h |  2 +-
 src/modules/src/commander.c       | 32 +++++++++++++++++++++++++------
 2 files changed, 27 insertions(+), 7 deletions(-)

diff --git a/src/modules/interface/commander.h b/src/modules/interface/commander.h
index 22a391e99f..dd42a87fd2 100644
--- a/src/modules/interface/commander.h
+++ b/src/modules/interface/commander.h
@@ -63,6 +63,6 @@ uint32_t commanderGetInactivityTime(void);
 void commanderExtrxSet(const struct CommanderCrtpValues* val);
 
 void commanderGetSetpoint(setpoint_t *setpoint, const state_t *state);
-void commanderGetPosition(state_t *state);
+bool commanderGetPosition(state_t *state);
 
 #endif /* COMMANDER_H_ */
diff --git a/src/modules/src/commander.c b/src/modules/src/commander.c
index c25e796efc..c5f8743c8b 100644
--- a/src/modules/src/commander.c
+++ b/src/modules/src/commander.c
@@ -31,8 +31,14 @@
 #include "commander.h"
 #include "crtp.h"
 #include "configblock.h"
+#include "log.h"
 #include "param.h"
 #include "num.h"
+#include "debug.h"
+
+#ifdef ESTIMATOR_TYPE_kalman
+#include "estimator_kalman.h"
+#endif
 
 #define MIN_THRUST  1000
 #define MAX_THRUST  60000
@@ -57,6 +63,8 @@ typedef struct
   uint32_t timestamp; // FreeRTOS ticks
 } CommanderPositionCache;
 
+static positionMeasurement_t pos_mocap;
+
 /**
  * Stabilization modes for Roll, Pitch, Yaw.
  */
@@ -391,17 +399,29 @@ void commanderGetSetpoint(setpoint_t *setpoint, const state_t *state)
   }
 }
 
-void commanderGetPosition(state_t *state)
+bool commanderGetPosition(state_t *state)
 {
   // Only use position information if it's valid and recent
-  if ((xTaskGetTickCount() - crtpPosCache.timestamp) < M2T(2)) {
-    // Update position information
-    state->position.x = crtpPosCache.targetVal[crtpPosCache.activeSide].x;
-    state->position.y = crtpPosCache.targetVal[crtpPosCache.activeSide].y;
-    state->position.z = crtpPosCache.targetVal[crtpPosCache.activeSide].z;
+  if ((xTaskGetTickCount() - crtpPosCache.timestamp) < M2T(5)) {
+    // Get the updated position from the mocap
+    pos_mocap.x = crtpPosCache.targetVal[crtpPosCache.activeSide].x;
+    pos_mocap.y = crtpPosCache.targetVal[crtpPosCache.activeSide].y;
+    pos_mocap.z = crtpPosCache.targetVal[crtpPosCache.activeSide].z;
+    pos_mocap.stdDev = 0.01;
+#ifdef ESTIMATOR_TYPE_kalman
+    stateEstimatorEnqueuePosition(&pos_mocap);
+#endif
+    return true;
   }
+  return false;
 }
 
+LOG_GROUP_START(mocap_pos)
+  LOG_ADD(LOG_FLOAT, X, &pos_mocap.x)
+  LOG_ADD(LOG_FLOAT, Y, &pos_mocap.y)
+  LOG_ADD(LOG_FLOAT, Z, &pos_mocap.z)
+LOG_GROUP_STOP(mocap_pos)
+
 // Params for flight modes
 PARAM_GROUP_START(flightmode)
 PARAM_ADD(PARAM_UINT8, althold, &altHoldMode)

From 3053d0057aaaf8ebe173e4e5c01c18d26e5f3a17 Mon Sep 17 00:00:00 2001
From: Casey Carlin <casey.carlin@us.panasonic.com>
Date: Fri, 16 Sep 2016 14:20:38 -0700
Subject: [PATCH 07/14] commander: Make position variable more generic.

---
 src/modules/src/commander.c | 18 +++++++++---------
 1 file changed, 9 insertions(+), 9 deletions(-)

diff --git a/src/modules/src/commander.c b/src/modules/src/commander.c
index c5f8743c8b..9321a51efb 100644
--- a/src/modules/src/commander.c
+++ b/src/modules/src/commander.c
@@ -63,7 +63,7 @@ typedef struct
   uint32_t timestamp; // FreeRTOS ticks
 } CommanderPositionCache;
 
-static positionMeasurement_t pos_mocap;
+static positionMeasurement_t pos_ext;
 
 /**
  * Stabilization modes for Roll, Pitch, Yaw.
@@ -404,12 +404,12 @@ bool commanderGetPosition(state_t *state)
   // Only use position information if it's valid and recent
   if ((xTaskGetTickCount() - crtpPosCache.timestamp) < M2T(5)) {
     // Get the updated position from the mocap
-    pos_mocap.x = crtpPosCache.targetVal[crtpPosCache.activeSide].x;
-    pos_mocap.y = crtpPosCache.targetVal[crtpPosCache.activeSide].y;
-    pos_mocap.z = crtpPosCache.targetVal[crtpPosCache.activeSide].z;
-    pos_mocap.stdDev = 0.01;
+    pos_ext.x = crtpPosCache.targetVal[crtpPosCache.activeSide].x;
+    pos_ext.y = crtpPosCache.targetVal[crtpPosCache.activeSide].y;
+    pos_ext.z = crtpPosCache.targetVal[crtpPosCache.activeSide].z;
+    pos_ext.stdDev = 0.01;
 #ifdef ESTIMATOR_TYPE_kalman
-    stateEstimatorEnqueuePosition(&pos_mocap);
+    stateEstimatorEnqueuePosition(&pos_ext);
 #endif
     return true;
   }
@@ -417,9 +417,9 @@ bool commanderGetPosition(state_t *state)
 }
 
 LOG_GROUP_START(mocap_pos)
-  LOG_ADD(LOG_FLOAT, X, &pos_mocap.x)
-  LOG_ADD(LOG_FLOAT, Y, &pos_mocap.y)
-  LOG_ADD(LOG_FLOAT, Z, &pos_mocap.z)
+  LOG_ADD(LOG_FLOAT, X, &pos_ext.x)
+  LOG_ADD(LOG_FLOAT, Y, &pos_ext.y)
+  LOG_ADD(LOG_FLOAT, Z, &pos_ext.z)
 LOG_GROUP_STOP(mocap_pos)
 
 // Params for flight modes

From 7e8bebe1919e8cfe9c14ed27663bc1ac7ebfafe9 Mon Sep 17 00:00:00 2001
From: Stephan Brown <stephan.brown.07@gmail.com>
Date: Fri, 16 Sep 2016 14:46:37 -0700
Subject: [PATCH 08/14] stabilzer: Get the position information before the
 state estimation.

---
 src/modules/src/commander.c  | 4 ++--
 src/modules/src/stabilizer.c | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/modules/src/commander.c b/src/modules/src/commander.c
index 9321a51efb..e1e4459cf3 100644
--- a/src/modules/src/commander.c
+++ b/src/modules/src/commander.c
@@ -416,11 +416,11 @@ bool commanderGetPosition(state_t *state)
   return false;
 }
 
-LOG_GROUP_START(mocap_pos)
+LOG_GROUP_START(pos_ext)
   LOG_ADD(LOG_FLOAT, X, &pos_ext.x)
   LOG_ADD(LOG_FLOAT, Y, &pos_ext.y)
   LOG_ADD(LOG_FLOAT, Z, &pos_ext.z)
-LOG_GROUP_STOP(mocap_pos)
+LOG_GROUP_STOP(pos_ext)
 
 // Params for flight modes
 PARAM_GROUP_START(flightmode)
diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c
index 3a3b1ce622..94edce0648 100644
--- a/src/modules/src/stabilizer.c
+++ b/src/modules/src/stabilizer.c
@@ -110,6 +110,7 @@ static void stabilizerTask(void* param)
   while(1) {
     vTaskDelayUntil(&lastWakeTime, F2T(RATE_MAIN_LOOP));
 
+    commanderGetPosition(&state);
 #ifdef ESTIMATOR_TYPE_kalman
     stateEstimatorUpdate(&state, &sensorData, &control);
 #else
@@ -118,7 +119,6 @@ static void stabilizerTask(void* param)
 #endif
 
     commanderGetSetpoint(&setpoint, &state);
-    commanderGetPosition(&state);
 
     sitAwUpdateSetpoint(&setpoint, &sensorData, &state);
 

From 3d884e304784bd004c5b20537b32d42808a35bb5 Mon Sep 17 00:00:00 2001
From: Stephan Brown <stephan.brown.07@gmail.com>
Date: Fri, 16 Sep 2016 15:19:02 -0700
Subject: [PATCH 09/14] eskylink: Change to port name to CRTP_PORT_SETPOINT.

---
 src/hal/src/eskylink.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/hal/src/eskylink.c b/src/hal/src/eskylink.c
index 2a29b2f6a8..c3c6d08f66 100644
--- a/src/hal/src/eskylink.c
+++ b/src/hal/src/eskylink.c
@@ -205,7 +205,7 @@ static void eskylinkDecode(char* packet)
   if (thrust>(2*PPM_RANGE)) thrust = 2*PPM_RANGE;
   thrust *= 55000/(2*PPM_RANGE);
 
-  crtpPacket.port = CRTP_PORT_COMMANDER;
+  crtpPacket.port = CRTP_PORT_SETPOINT;
   memcpy(&crtpPacket.data[0],  (char*)&roll,   4);
   memcpy(&crtpPacket.data[4],  (char*)&pitch,  4);
   memcpy(&crtpPacket.data[8],  (char*)&yaw,    4);

From 1f5c625ed6894497ba41891f083af2b862fead5e Mon Sep 17 00:00:00 2001
From: Stephan Brown <stephan.brown.07@gmail.com>
Date: Fri, 16 Sep 2016 17:25:19 -0700
Subject: [PATCH 10/14] Makefile: Add missing sensor task define.

---
 Makefile | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Makefile b/Makefile
index d8fe46691d..8992feef20 100644
--- a/Makefile
+++ b/Makefile
@@ -260,7 +260,7 @@ ifeq ($(USE_ESKYLINK), 1)
   CFLAGS += -DUSE_ESKYLINK
 endif
 
-CFLAGS += -DBOARD_REV_$(REV) -DESTIMATOR_TYPE_$(ESTIMATOR) -DCONTROLLER_TYPE_$(CONTROLLER) -DPOWER_DISTRIBUTION_TYPE_$(POWER_DISTRIBUTION)
+CFLAGS += -DBOARD_REV_$(REV) -DESTIMATOR_TYPE_$(ESTIMATOR) -DCONTROLLER_TYPE_$(CONTROLLER) -DPOWER_DISTRIBUTION_TYPE_$(POWER_DISTRIBUTION) -DSENSORS_TYPE_$(SENSORS)
 
 CFLAGS += $(PROCESSOR) $(INCLUDES) $(STFLAGS)
 ifeq ($(PLATFORM), CF1)

From 3aab01f4288257bbdbac7862b4bf8e94279d844b Mon Sep 17 00:00:00 2001
From: Stephan Brown <stephan.brown.07@gmail.com>
Date: Fri, 16 Sep 2016 17:43:11 -0700
Subject: [PATCH 11/14] gitignore: Ignore exuberant ctags output.

---
 .gitignore | 1 +
 1 file changed, 1 insertion(+)

diff --git a/.gitignore b/.gitignore
index 920b1b7f6e..74746ca094 100644
--- a/.gitignore
+++ b/.gitignore
@@ -9,6 +9,7 @@ bin/*
 tools/make/config.mk
 cflie.*
 version.c
+tags
 
 /cf2.*
 /cf1.*

From 2746b6c4ad648f99463a45c79a9fc3bb88cdc0c1 Mon Sep 17 00:00:00 2001
From: Stephan Brown <stephan.brown.07@gmail.com>
Date: Wed, 21 Sep 2016 10:55:35 -0700
Subject: [PATCH 12/14] ext_position: Move handling of external position
 information to it's own file.

---
 Makefile                             |  3 +-
 src/modules/interface/commander.h    |  8 ---
 src/modules/interface/ext_position.h | 47 ++++++++++++++
 src/modules/src/commander.c          | 50 +--------------
 src/modules/src/ext_position.c       | 95 ++++++++++++++++++++++++++++
 src/modules/src/stabilizer.c         |  3 +-
 6 files changed, 148 insertions(+), 58 deletions(-)
 create mode 100644 src/modules/interface/ext_position.h
 create mode 100644 src/modules/src/ext_position.c

diff --git a/Makefile b/Makefile
index 8992feef20..8e930b054f 100644
--- a/Makefile
+++ b/Makefile
@@ -154,7 +154,8 @@ PROJ_OBJ_CF1 += sound_cf1.o sensors_stock.o
 PROJ_OBJ_CF2 += platformservice.o sound_cf2.o extrx.o
 
 # Stabilizer modules
-PROJ_OBJ += commander.o attitude_pid_controller.o sensfusion6.o stabilizer.o
+PROJ_OBJ += commander.o ext_position.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
 PROJ_OBJ += power_distribution_$(POWER_DISTRIBUTION).o
diff --git a/src/modules/interface/commander.h b/src/modules/interface/commander.h
index dd42a87fd2..b7f4ce7aed 100644
--- a/src/modules/interface/commander.h
+++ b/src/modules/interface/commander.h
@@ -50,19 +50,11 @@ struct CommanderCrtpValues
   uint16_t thrust;
 } __attribute__((packed));
 
-struct CommanderCrtpPosition
-{
-  float x;
-  float y;
-  float z;
-} __attribute__((packed));
-
 void commanderInit(void);
 bool commanderTest(void);
 uint32_t commanderGetInactivityTime(void);
 void commanderExtrxSet(const struct CommanderCrtpValues* val);
 
 void commanderGetSetpoint(setpoint_t *setpoint, const state_t *state);
-bool commanderGetPosition(state_t *state);
 
 #endif /* COMMANDER_H_ */
diff --git a/src/modules/interface/ext_position.h b/src/modules/interface/ext_position.h
new file mode 100644
index 0000000000..23c8f59390
--- /dev/null
+++ b/src/modules/interface/ext_position.h
@@ -0,0 +1,47 @@
+/**
+ *    ||          ____  _ __  ______
+ * +------+      / __ )(_) /_/ ____/_________ _____  ___
+ * | 0xBC |     / __  / / __/ /    / ___/ __ `/_  / / _	\
+ * +------+    / /_/ / / /_/ /___ / /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\____//_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2012 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#ifndef _EXT_POSITION_H_
+#define _EXT_POSITION_H_
+
+#include "stabilizer_types.h"
+
+/**
+ * CRTP external position data struct
+ */
+struct CrtpExtPosition
+{
+  float x; // in m
+  float y; // in m
+  float z; // in m
+} __attribute__((packed));
+
+// Set up the callback for the CRTP_PORT_POSITION
+void extPositionInit(void);
+
+// Get the current position from the cache
+bool getExtPosition(state_t *state);
+
+#endif /* _EXT_POSITION_H_ */
diff --git a/src/modules/src/commander.c b/src/modules/src/commander.c
index e1e4459cf3..2cce73280b 100644
--- a/src/modules/src/commander.c
+++ b/src/modules/src/commander.c
@@ -35,10 +35,7 @@
 #include "param.h"
 #include "num.h"
 #include "debug.h"
-
-#ifdef ESTIMATOR_TYPE_kalman
-#include "estimator_kalman.h"
-#endif
+#include "ext_position.h"
 
 #define MIN_THRUST  1000
 #define MAX_THRUST  60000
@@ -53,17 +50,6 @@ typedef struct
   uint32_t timestamp; // FreeRTOS ticks
 } CommanderCache;
 
-/**
- * Commander position data
- */
-typedef struct
-{
-  struct CommanderCrtpPosition targetVal[2];
-  bool activeSide;
-  uint32_t timestamp; // FreeRTOS ticks
-} CommanderPositionCache;
-
-static positionMeasurement_t pos_ext;
 
 /**
  * Stabilization modes for Roll, Pitch, Yaw.
@@ -89,8 +75,6 @@ static CommanderCache crtpCache;
 static CommanderCache extrxCache;
 static CommanderCache* activeCache;
 
-static CommanderPositionCache crtpPosCache;
-
 static uint32_t lastUpdate;
 static bool isInactive;
 static bool thrustLocked;
@@ -196,13 +180,6 @@ static void commanderCrtpCB(CRTPPacket* pk)
   }
 }
 
-static void commanderPositionCrtpCB(CRTPPacket* pk)
-{
-  crtpPosCache.targetVal[!crtpPosCache.activeSide] = *((struct CommanderCrtpPosition*)pk->data);
-  crtpPosCache.activeSide = !crtpPosCache.activeSide;
-  crtpPosCache.timestamp = xTaskGetTickCount();
-}
-
 /**
  * Rotate Yaw so that the Crazyflie will change what is considered front.
  *
@@ -284,7 +261,7 @@ void commanderInit(void)
 
   crtpInit();
   crtpRegisterPortCB(CRTP_PORT_SETPOINT, commanderCrtpCB);
-  crtpRegisterPortCB(CRTP_PORT_POSITION, commanderPositionCrtpCB);
+  extPositionInit(); // Set callback for CRTP_PORT_POSITION
 
   activeCache = &crtpCache;
   lastUpdate = xTaskGetTickCount();
@@ -399,29 +376,6 @@ void commanderGetSetpoint(setpoint_t *setpoint, const state_t *state)
   }
 }
 
-bool commanderGetPosition(state_t *state)
-{
-  // Only use position information if it's valid and recent
-  if ((xTaskGetTickCount() - crtpPosCache.timestamp) < M2T(5)) {
-    // Get the updated position from the mocap
-    pos_ext.x = crtpPosCache.targetVal[crtpPosCache.activeSide].x;
-    pos_ext.y = crtpPosCache.targetVal[crtpPosCache.activeSide].y;
-    pos_ext.z = crtpPosCache.targetVal[crtpPosCache.activeSide].z;
-    pos_ext.stdDev = 0.01;
-#ifdef ESTIMATOR_TYPE_kalman
-    stateEstimatorEnqueuePosition(&pos_ext);
-#endif
-    return true;
-  }
-  return false;
-}
-
-LOG_GROUP_START(pos_ext)
-  LOG_ADD(LOG_FLOAT, X, &pos_ext.x)
-  LOG_ADD(LOG_FLOAT, Y, &pos_ext.y)
-  LOG_ADD(LOG_FLOAT, Z, &pos_ext.z)
-LOG_GROUP_STOP(pos_ext)
-
 // Params for flight modes
 PARAM_GROUP_START(flightmode)
 PARAM_ADD(PARAM_UINT8, althold, &altHoldMode)
diff --git a/src/modules/src/ext_position.c b/src/modules/src/ext_position.c
new file mode 100644
index 0000000000..7677075624
--- /dev/null
+++ b/src/modules/src/ext_position.c
@@ -0,0 +1,95 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie Firmware
+ *
+ * Copyright (C) 2011-2012 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ *
+ */
+
+#include "FreeRTOS.h"
+#include "task.h"
+
+#include "ext_position.h"
+#include "crtp.h"
+#include "log.h"
+
+#ifdef ESTIMATOR_TYPE_kalman
+#include "estimator_kalman.h"
+#endif
+
+/**
+ * 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 void extPositionCrtpCB(CRTPPacket* pk);
+
+static bool isInit = false;
+
+void extPositionInit()
+{
+  if (isInit) {
+    return;
+  }
+
+  crtpRegisterPortCB(CRTP_PORT_POSITION, extPositionCrtpCB);
+  isInit = true;
+}
+
+static void extPositionCrtpCB(CRTPPacket* pk)
+{
+  crtpExtPosCache.targetVal[!crtpExtPosCache.activeSide] = *((struct CrtpExtPosition*)pk->data);
+  crtpExtPosCache.activeSide = !crtpExtPosCache.activeSide;
+  crtpExtPosCache.timestamp = xTaskGetTickCount();
+}
+
+
+bool getExtPosition(state_t *state)
+{
+  // Only use position information if it's valid and recent
+  if ((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;
+#ifdef ESTIMATOR_TYPE_kalman
+    stateEstimatorEnqueuePosition(&ext_pos);
+#endif
+    return true;
+  }
+  return false;
+}
+
+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)
+
diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c
index 94edce0648..c392f98ae0 100644
--- a/src/modules/src/stabilizer.c
+++ b/src/modules/src/stabilizer.c
@@ -36,6 +36,7 @@
 
 #include "sensors.h"
 #include "commander.h"
+#include "ext_position.h"
 #include "sitaw.h"
 #include "controller.h"
 #include "power_distribution.h"
@@ -110,7 +111,7 @@ static void stabilizerTask(void* param)
   while(1) {
     vTaskDelayUntil(&lastWakeTime, F2T(RATE_MAIN_LOOP));
 
-    commanderGetPosition(&state);
+    getExtPosition(&state);
 #ifdef ESTIMATOR_TYPE_kalman
     stateEstimatorUpdate(&state, &sensorData, &control);
 #else

From 0e2af452c87bec552dfccb2eaa6ad8ce82856eed Mon Sep 17 00:00:00 2001
From: Stephan Brown <stephan.brown.07@gmail.com>
Date: Wed, 21 Sep 2016 10:57:25 -0700
Subject: [PATCH 13/14] Makefile: Remove unnecessary define.

---
 Makefile | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Makefile b/Makefile
index 8e930b054f..0f3f6b736c 100644
--- a/Makefile
+++ b/Makefile
@@ -261,7 +261,7 @@ ifeq ($(USE_ESKYLINK), 1)
   CFLAGS += -DUSE_ESKYLINK
 endif
 
-CFLAGS += -DBOARD_REV_$(REV) -DESTIMATOR_TYPE_$(ESTIMATOR) -DCONTROLLER_TYPE_$(CONTROLLER) -DPOWER_DISTRIBUTION_TYPE_$(POWER_DISTRIBUTION) -DSENSORS_TYPE_$(SENSORS)
+CFLAGS += -DBOARD_REV_$(REV) -DESTIMATOR_TYPE_$(ESTIMATOR) -DCONTROLLER_TYPE_$(CONTROLLER) -DPOWER_DISTRIBUTION_TYPE_$(POWER_DISTRIBUTION)
 
 CFLAGS += $(PROCESSOR) $(INCLUDES) $(STFLAGS)
 ifeq ($(PLATFORM), CF1)

From b5ca42c30d32ff6fce216e948822d4cfec013345 Mon Sep 17 00:00:00 2001
From: Stephan Brown <stephan.brown.07@gmail.com>
Date: Wed, 21 Sep 2016 10:57:47 -0700
Subject: [PATCH 14/14] libdw1000: Point submodule back to master.

---
 vendor/libdw1000 | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/vendor/libdw1000 b/vendor/libdw1000
index 4f68d78075..55f86179c7 160000
--- a/vendor/libdw1000
+++ b/vendor/libdw1000
@@ -1 +1 @@
-Subproject commit 4f68d780757b7345ffb9ae3f3784a7ce07ebcb32
+Subproject commit 55f86179c7f4ea821130603f4186f80346779700