Merge pull request #147 from stephanbro/master.crtp_position_update

Master.crtp position update

.gitignore

@@ -9,6 +9,7 @@ bin/*
 tools/make/config.mk
 cflie.*
 version.c
+tags
 
 /cf2.*
 /cf1.*

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

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);

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;

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_ */

src/modules/src/commander.c

@@ -31,8 +31,11 @@
 #include "commander.h"
 #include "crtp.h"
 #include "configblock.h"
+#include "log.h"
 #include "param.h"
 #include "num.h"
+#include "debug.h"
+#include "ext_position.h"
 
 #define MIN_THRUST  1000
 #define MAX_THRUST  60000
@@ -47,6 +50,7 @@ typedef struct
   uint32_t timestamp; // FreeRTOS ticks
 } CommanderCache;
 
+
 /**
  * Stabilization modes for Roll, Pitch, Yaw.
  */
@@ -256,7 +260,8 @@ void commanderInit(void)
   }
 
   crtpInit();
-  crtpRegisterPortCB(CRTP_PORT_COMMANDER, commanderCrtpCB);
+  crtpRegisterPortCB(CRTP_PORT_SETPOINT, commanderCrtpCB);
+  extPositionInit(); // Set callback for CRTP_PORT_POSITION
 
   activeCache = &crtpCache;
   lastUpdate = xTaskGetTickCount();

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)
+

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,6 +111,7 @@ static void stabilizerTask(void* param)
   while(1) {
     vTaskDelayUntil(&lastWakeTime, F2T(RATE_MAIN_LOOP));
 
+    getExtPosition(&state);
 #ifdef ESTIMATOR_TYPE_kalman
     stateEstimatorUpdate(&state, &sensorData, &control);
 #else