FlyPi/crazyflie-firmware-2021.06/examples/demos/app_push_demo/.gitignore

+bin/*
+cf2.*

FlyPi/crazyflie-firmware-2021.06/examples/demos/app_push_demo/Makefile

+# enable app support
+APP=1
+APP_STACKSIZE=300
+
+VPATH += src/
+PROJ_OBJ += push.o
+
+CRAZYFLIE_BASE=../../..
+include $(CRAZYFLIE_BASE)/Makefile

FlyPi/crazyflie-firmware-2021.06/examples/demos/app_push_demo/readme.md

+# Push demo for Crazyflie 2.x
+
+This repos contains the push demo for Cazyflie 2.x.
+It uses out-of-tree build functionality of the Crazyflie firmware and is implemented using the app entry-point.
+
+This demo works with a Crazyflie 2.x with Flow deck version 2.0 and Multiranger deck attached. If you want to use the Flow deck 1.0 or any other positioning deck, you will need to change the following line to look at the parameter for the wanted deck:
+```
+  uint16_t idPositioningDeck = paramGetVarId("deck", "bcFlow2");
+
+```
+
+When running, the crazyflie will wait for an object to get close to the top ranging sensor.
+When an oject (ie. an hand) has passed close to the top sensor, the Crazyflie takes off and hover.
+If anything is detected on side sensors, the Crazyflie moves in the oposite direction.
+So, it is possible to push the Crazyflie around.
+
+## Build
+
+You must have the required tools to build the [Crazyflie firmware](https://github.com/bitcraze/crazyflie-firmware).
+
+Clone the repos with ```--recursive```. If you did not do so, pull submodules with:
+```
+git submodule update --init --recursive
+```
+
+Then build and bootload:
+```
+make -j$(nproc)
+make cload
+```
+
+

FlyPi/crazyflie-firmware-2021.06/examples/demos/app_push_demo/src/push.c

+/**
+ * ,---------,       ____  _ __
+ * |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+ * | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * | / ,--´  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *    +------`   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2019 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/>.
+ *
+ *
+ * push.c - App layer application of the onboard push demo. The crazyflie 
+ * has to have the multiranger and the flowdeck version 2.
+ */
+
+#include <string.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "app.h"
+
+#include "commander.h"
+
+#include "FreeRTOS.h"
+#include "task.h"
+
+#include "debug.h"
+
+#include "log.h"
+#include "param.h"
+
+#define DEBUG_MODULE "PUSH"
+
+static void setHoverSetpoint(setpoint_t *setpoint, float vx, float vy, float z, float yawrate)
+{
+  setpoint->mode.z = modeAbs;
+  setpoint->position.z = z;
+
+
+  setpoint->mode.yaw = modeVelocity;
+  setpoint->attitudeRate.yaw = yawrate;
+
+
+  setpoint->mode.x = modeVelocity;
+  setpoint->mode.y = modeVelocity;
+  setpoint->velocity.x = vx;
+  setpoint->velocity.y = vy;
+
+  setpoint->velocity_body = true;
+}
+
+typedef enum {
+    idle,
+    lowUnlock,
+    unlocked,
+    stopping
+} State;
+
+static State state = idle;
+
+static const uint16_t unlockThLow = 100;
+static const uint16_t unlockThHigh = 300;
+static const uint16_t stoppedTh = 500;
+
+static const float velMax = 1.0f;
+static const uint16_t radius = 300;
+
+static const float height_sp = 0.2f;
+
+#define MAX(a,b) ((a>b)?a:b)
+#define MIN(a,b) ((a<b)?a:b)
+
+void appMain()
+{
+  static setpoint_t setpoint;
+
+  vTaskDelay(M2T(3000));
+
+  logVarId_t idUp = logGetVarId("range", "up");
+  logVarId_t idLeft = logGetVarId("range", "left");
+  logVarId_t idRight = logGetVarId("range", "right");
+  logVarId_t idFront = logGetVarId("range", "front");
+  logVarId_t idBack = logGetVarId("range", "back");
+  
+  paramVarId_t idPositioningDeck = paramGetVarId("deck", "bcFlow2");
+  paramVarId_t idMultiranger = paramGetVarId("deck", "bcMultiranger");
+
+
+  float factor = velMax/radius;
+
+  //DEBUG_PRINT("%i", idUp);
+
+  DEBUG_PRINT("Waiting for activation ...\n");
+
+  while(1) {
+    vTaskDelay(M2T(10));
+    //DEBUG_PRINT(".");
+
+    uint8_t positioningInit = paramGetUint(idPositioningDeck);
+    uint8_t multirangerInit = paramGetUint(idMultiranger);
+
+    uint16_t up = logGetUint(idUp);
+
+    if (state == unlocked) {
+      uint16_t left = logGetUint(idLeft);
+      uint16_t right = logGetUint(idRight);
+      uint16_t front = logGetUint(idFront);
+      uint16_t back = logGetUint(idBack);
+
+      uint16_t left_o = radius - MIN(left, radius);
+      uint16_t right_o = radius - MIN(right, radius);
+      float l_comp = (-1) * left_o * factor;
+      float r_comp = right_o * factor;
+      float velSide = r_comp + l_comp;
+
+      uint16_t front_o = radius - MIN(front, radius);
+      uint16_t back_o = radius - MIN(back, radius);
+      float f_comp = (-1) * front_o * factor;
+      float b_comp = back_o * factor;
+      float velFront = b_comp + f_comp;
+
+      uint16_t up_o = radius - MIN(up, radius);
+      float height = height_sp - up_o/1000.0f;
+
+      /*DEBUG_PRINT("l=%i, r=%i, lo=%f, ro=%f, vel=%f\n", left_o, right_o, l_comp, r_comp, velSide);
+      DEBUG_PRINT("f=%i, b=%i, fo=%f, bo=%f, vel=%f\n", front_o, back_o, f_comp, b_comp, velFront);
+      DEBUG_PRINT("u=%i, d=%i, height=%f\n", up_o, height);*/
+
+      if (1) {
+        setHoverSetpoint(&setpoint, velFront, velSide, height, 0);
+        commanderSetSetpoint(&setpoint, 3);
+      }
+
+      if (height < 0.1f) {
+        state = stopping;
+        DEBUG_PRINT("X\n");
+      }
+
+    } else {
+
+      if (state == stopping && up > stoppedTh) {
+        DEBUG_PRINT("%i", up);
+        state = idle;
+        DEBUG_PRINT("S\n");
+      }
+
+      if (up < unlockThLow && state == idle && up > 0.001f) {
+        DEBUG_PRINT("Waiting for hand to be removed!\n");
+        state = lowUnlock;
+      }
+
+      if (up > unlockThHigh && state == lowUnlock && positioningInit && multirangerInit) {
+        DEBUG_PRINT("Unlocked!\n");
+        state = unlocked;
+      }
+
+      if (state == idle || state == stopping) {
+        memset(&setpoint, 0, sizeof(setpoint_t));
+        commanderSetSetpoint(&setpoint, 3);
+      }
+    }
+  }
+}

FlyPi/crazyflie-firmware-2021.06/examples/demos/app_wall_following_demo/Makefile

+# enable app support
+APP=1
+APP_STACKSIZE=300
+
+VPATH += src/
+PROJ_OBJ += wall_following.o
+PROJ_OBJ += wallfollowing_multiranger_onboard.o
+
+CRAZYFLIE_BASE=../../..
+include $(CRAZYFLIE_BASE)/Makefile

FlyPi/crazyflie-firmware-2021.06/examples/demos/app_wall_following_demo/src/wall_following.c

+/**
+ * ,---------,       ____  _ __
+ * |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+ * | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * | / ,--´  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *    +------`   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2021 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/>.
+ *
+ *
+ * wall_follower.c - App layer application of the wall following demo. The crazyflie 
+ * has to have the multiranger and the flowdeck version 2.
+ *
+ * The same wallfollowing strategy was used in the following paper:
+
+ @article{mcguire2019minimal,
+  title={Minimal navigation solution for a swarm of tiny flying robots to explore an unknown environment},
+  author={McGuire, KN and De Wagter, Christophe and Tuyls, Karl and Kappen, HJ and de Croon, Guido CHE},
+  journal={Science Robotics},
+  volume={4},
+  number={35},
+  year={2019},
+  publisher={Science Robotics}
+}
+
+ */
+
+#include <string.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "app.h"
+
+#include "commander.h"
+
+#include "FreeRTOS.h"
+#include "task.h"
+
+#include "debug.h"
+
+#include "log.h"
+#include "param.h"
+#include <math.h>
+#include "usec_time.h"
+
+#include "wallfollowing_multiranger_onboard.h"
+
+#define DEBUG_MODULE "WALLFOLLOWING"
+
+static void setVelocitySetpoint(setpoint_t *setpoint, float vx, float vy, float z, float yawrate)
+{
+  setpoint->mode.z = modeAbs;
+  setpoint->position.z = z;
+  setpoint->mode.yaw = modeVelocity;
+  setpoint->attitudeRate.yaw = yawrate;
+  setpoint->mode.x = modeVelocity;
+  setpoint->mode.y = modeVelocity;
+  setpoint->velocity.x = vx;
+  setpoint->velocity.y = vy;
+
+  setpoint->velocity_body = true;
+}
+
+// States
+typedef enum
+{
+  idle,
+  lowUnlock,
+  unlocked,
+  stopping
+} StateOuterLoop;
+
+StateOuterLoop stateOuterLoop = idle;
+StateWF stateInnerLoop = forward;
+
+// Thresholds for the unlocking procedure of the top sensor of the multiranger
+static const uint16_t unlockThLow = 100;
+static const uint16_t unlockThHigh = 300;
+static const uint16_t stoppedTh = 500;
+
+// Handling the height setpoint
+static const float spHeight = 0.5f;
+static const uint16_t radius = 300;
+
+// Some wallfollowing parameters and logging
+bool goLeft = false;
+float distanceToWall = 0.5f;
+float maxForwardSpeed = 0.5f;
+
+float cmdVelX = 0.0f;
+float cmdVelY = 0.0f;
+float cmdAngWRad = 0.0f;
+float cmdAngWDeg = 0.0f;
+
+#define MAX(a, b) ((a > b) ? a : b)
+#define MIN(a, b) ((a < b) ? a : b)
+
+void appMain()
+{
+  vTaskDelay(M2T(3000));
+  // Getting Logging IDs of the multiranger
+  logVarId_t idUp = logGetVarId("range", "up");
+  logVarId_t idLeft = logGetVarId("range", "left");
+  logVarId_t idRight = logGetVarId("range", "right");
+  logVarId_t idFront = logGetVarId("range", "front");
+
+  // Getting the Logging IDs of the state estimates
+  logVarId_t idStabilizerYaw = logGetVarId("stabilizer", "yaw");
+  logVarId_t idHeightEstimate = logGetVarId("stateEstimate", "z");
+
+  // Getting Param IDs of the deck driver initialization
+  paramVarId_t idPositioningDeck = paramGetVarId("deck", "bcFlow2");
+  paramVarId_t idMultiranger = paramGetVarId("deck", "bcMultiranger");
+
+  // Initialize the wall follower state machine
+  wallFollowerInit(distanceToWall, maxForwardSpeed, stateInnerLoop);
+
+  // Intialize the setpoint structure
+  setpoint_t setpoint;
+
+  DEBUG_PRINT("Waiting for activation ...\n");
+
+  while (1)
+  {
+    vTaskDelay(M2T(10));
+
+    // Check if decks are properly mounted
+    uint8_t positioningInit = paramGetUint(idPositioningDeck);
+    uint8_t multirangerInit = paramGetUint(idMultiranger);
+
+    // Get the upper range sensor value (used for startup and stopping)
+    uint16_t up = logGetUint(idUp);
+
+    // Get Height estimate
+    float heightEstimate = logGetFloat(idHeightEstimate);
+
+    // If the crazyflie is unlocked by the hand, continue with state machine
+    if (stateOuterLoop == unlocked)
+    {
+
+      // Get all multiranger values
+      float frontRange = (float)logGetUint(idFront) / 1000.0f;
+      float sideRange;
+      if (goLeft)
+      {
+        sideRange = (float)logGetUint(idRight) / 1000.0f;
+      }
+      else
+      {
+        sideRange = (float)logGetUint(idLeft) / 1000.0f;
+      }
+
+      // Get the heading and convert it to rad
+      float estYawDeg = logGetFloat(idStabilizerYaw);
+      float estYawRad = estYawDeg * (float)M_PI / 180.0f;
+
+      //Adjust height based on up ranger input
+      uint16_t up_o = radius - MIN(up, radius);
+      float cmdHeight = spHeight - up_o / 1000.0f;
+
+      cmdVelX = 0.0f;
+      cmdVelY = 0.0f;
+      cmdAngWRad = 0.0f;
+      cmdAngWDeg = 0.0f;
+
+      // Only go to the state machine if the crazyflie has reached a certain height
+      if (heightEstimate > spHeight - 0.1f)
+      {
+        // Set the wall following direction
+        int direction;
+        if (goLeft)
+        {
+          direction = 1;
+        }
+        else
+        {
+          direction = -1;
+        }
+
+        // The wall-following state machine which outputs velocity commands
+        float timeNow = usecTimestamp() / 1e6;
+        stateInnerLoop = wallFollower(&cmdVelX, &cmdVelY, &cmdAngWRad, frontRange, sideRange, estYawRad, direction, timeNow);
+        cmdAngWDeg = cmdAngWRad * 180.0f / (float)M_PI;
+      }
+      // Turn velocity commands into setpoints and send it to the commander
+      setVelocitySetpoint(&setpoint, cmdVelX, cmdVelY, cmdHeight, cmdAngWDeg);
+      commanderSetSetpoint(&setpoint, 3);
+
+      // Handling stopping with hand above the crazyflie
+      if (cmdHeight < spHeight - 0.2f)
+      {
+        stateOuterLoop = stopping;
+        DEBUG_PRINT("X\n");
+      }
+    }
+    else
+    {
+
+      // Handeling locking and unlocking
+      if (stateOuterLoop == stopping && up > stoppedTh)
+      {
+        DEBUG_PRINT("%i", up);
+        stateOuterLoop = idle;
+        DEBUG_PRINT("S\n");
+      }
+
+      // If the up multiranger is activated for the first time, prepare to be unlocked
+      if (up < unlockThLow && stateOuterLoop == idle && up > 0.001f)
+      {
+        DEBUG_PRINT("Waiting for hand to be removed!\n");
+        stateOuterLoop = lowUnlock;
+      }
+
+      // Unlock CF if hand above is removed, and if the positioningdeckand multiranger deck is initalized.
+      if (up > unlockThHigh && stateOuterLoop == lowUnlock && positioningInit && multirangerInit)
+      {
+        DEBUG_PRINT("Unlocked!\n");
+        stateOuterLoop = unlocked;
+      }
+
+      // Stop the crazyflie with idle or stopping state
+      if (stateOuterLoop == idle || stateOuterLoop == stopping)
+      {
+        memset(&setpoint, 0, sizeof(setpoint_t));
+        commanderSetSetpoint(&setpoint, 3);
+      }
+    }
+  }
+}
+
+PARAM_GROUP_START(app)
+PARAM_ADD(PARAM_UINT8, goLeft, &goLeft)
+PARAM_ADD(PARAM_FLOAT, distanceWall, &distanceToWall)
+PARAM_ADD(PARAM_FLOAT, maxSpeed, &maxForwardSpeed)
+PARAM_GROUP_STOP(app)
+
+LOG_GROUP_START(app)
+LOG_ADD(LOG_FLOAT, cmdVelX, &cmdVelX)
+LOG_ADD(LOG_FLOAT, cmdVelY, &cmdVelY)
+LOG_ADD(LOG_FLOAT, cmdAngWRad, &cmdAngWRad)
+LOG_ADD(LOG_UINT8, stateInnerLoop, &stateInnerLoop)
+LOG_ADD(LOG_UINT8, stateOuterLoop, &stateOuterLoop)
+LOG_GROUP_STOP(app)

FlyPi/crazyflie-firmware-2021.06/examples/demos/app_wall_following_demo/src/wallfollowing_multiranger_onboard.c

+/*
+ * wall_follower_multi_ranger_onboard.c
+ *
+ *  Created on: Aug 7, 2018
+ *      Author: knmcguire
+
+The same wallfollowing strategy was used in the following paper:
+
+ @article{mcguire2019minimal,
+  title={Minimal navigation solution for a swarm of tiny flying robots to explore an unknown environment},
+  author={McGuire, KN and De Wagter, Christophe and Tuyls, Karl and Kappen, HJ and de Croon, Guido CHE},
+  journal={Science Robotics},
+  volume={4},
+  number={35},
+  year={2019},
+  publisher={Science Robotics}
+
+ */
+
+#include "wallfollowing_multiranger_onboard.h"
+#include <math.h>
+
+// variables
+static float refDistanceFromWall = 0.0f;
+static float maxForwardSpeed = 0.2f;
+static float maxTurnRate = 0.5f;
+static float direction = 1.0f;
+static float firstRun = false;
+static float prevHeading = 0.0f;
+static float wallAngle = 0.0f;
+static bool aroundCornerBackTrack = false;
+static float stateStartTime;
+static float rangerValueBuffer = 0.2f;
+static float angleValueBuffer = 0.1f;
+static float rangerThresholdLost = 0.3f;
+static const float inCornerAngle = 0.8f;
+static const float waitForMeasurementSeconds = 1.0f;
+
+static StateWF stateWF = forward;
+float timeNow = 0.0f;
+
+void wallFollowerInit(float refDistanceFromWallNew, float maxForwardSpeed_ref, StateWF initState)
+{
+  refDistanceFromWall = refDistanceFromWallNew;
+  maxForwardSpeed = maxForwardSpeed_ref;
+  firstRun = true;
+  stateWF = initState;
+}
+
+// Static helper functions
+static bool logicIsCloseTo(float real_value, float checked_value, float margin)
+{
+  if (real_value > checked_value - margin && real_value < checked_value + margin)
+  {
+    return true;
+  }
+  else
+  {
+    return false;
+  }
+}
+
+static float wraptopi(float number)
+{
+  if (number > (float)M_PI)
+  {
+    return (number - (float)(2 * M_PI));
+  }
+  else if (number < (float)(-1 * M_PI))
+  {
+    return (number + (float)(2 * M_PI));
+  }
+  else
+  {
+    return (number);
+  }
+}
+
+// Static command functions
+static void commandTurn(float *cmdVelX, float *cmdAngW, float ref_rate)
+{
+  *cmdVelX = 0.0f;
+  *cmdAngW = direction * ref_rate;
+}
+
+static void commandAlignCorner(float *cmdVelY, float *cmdAngW, float ref_rate, float range,
+                               float wanted_distance_from_corner)
+{
+
+  if (range > wanted_distance_from_corner + 0.3f)
+  {
+    *cmdAngW = direction * ref_rate;
+    *cmdVelY = 0;
+  }
+  else
+  {
+    if (range > wanted_distance_from_corner)
+    {
+      *cmdVelY = direction * (-1.0f * maxForwardSpeed / 3.0f);
+    }
+    else
+    {
+      *cmdVelY = direction * (maxForwardSpeed / 3.0f);
+    }
+    *cmdAngW = 0;
+  }
+}
+
+static void commandHover(float *cmdVelX, float *cmdVelY, float *cmdAngW)
+{
+  *cmdVelX = 0.0f;
+  *cmdVelY = 0.0f;
+  *cmdAngW = 0.0f;
+}
+
+static void commandForwardAlongWall(float *cmdVelX, float *cmdVelY, float range)
+{
+  *cmdVelX = maxForwardSpeed;
+  bool check_distance_wall = logicIsCloseTo(refDistanceFromWall, range, 0.1f);
+  *cmdVelY = 0;
+  if (!check_distance_wall)
+  {
+    if (range > refDistanceFromWall)
+    {
+      *cmdVelY = direction * (-1.0f * maxForwardSpeed / 2.0f);
+    }
+    else
+    {
+      *cmdVelY = direction * (maxForwardSpeed / 2.0f);
+    }
+  }
+}
+
+static void commandTurnAroundCornerAndAdjust(float *cmdVelX, float *cmdVelY, float *cmdAngW, float radius, float range)
+{
+  *cmdVelX = maxForwardSpeed;
+  *cmdAngW = direction * (-1 * (*cmdVelX) / radius);
+  bool check_distance_to_wall = logicIsCloseTo(refDistanceFromWall, range, 0.1f);
+  if (!check_distance_to_wall)
+  {
+    if (range > refDistanceFromWall)
+    {
+      *cmdVelY = direction * (-1.0f * maxForwardSpeed / 3.0f);
+    }
+    else
+    {
+      *cmdVelY = direction * (maxForwardSpeed / 3.0f);
+    }
+  }
+}
+
+static void commandTurnAndAdjust(float *cmdVelY, float *cmdAngW, float rate, float range)
+{
+  *cmdAngW = direction * rate;
+  *cmdVelY = 0.0f;
+}
+
+static StateWF transition(StateWF newState)
+{
+  stateStartTime = timeNow;
+  return newState;
+}
+
+void adjustDistanceWall(float distanceWallNew)
+{
+  refDistanceFromWall = distanceWallNew;
+}
+
+StateWF wallFollower(float *cmdVelX, float *cmdVelY, float *cmdAngW, float frontRange, float sideRange, float currentHeading,
+                     int directionTurn, float timeOuter)
+{
+
+  direction = directionTurn;
+
+  timeNow = timeOuter;
+
+  if (firstRun)
+  {
+    prevHeading = currentHeading;
+    aroundCornerBackTrack = false;
+    firstRun = false;
+  }
+
+  /***********************************************************
+  * Handle state transitions
+  ***********************************************************/
+  switch (stateWF)
+  {
+
+  case forward:
+    if (frontRange < refDistanceFromWall + rangerValueBuffer)
+    {
+      stateWF = transition(turnToFindWall);
+    }
+    break;
+
+  case hover:
+    break;
+
+  case turnToFindWall:;
+    // check if wall is found
+    bool sideRangeCheck = sideRange < (refDistanceFromWall / (float)cos(0.78f) + rangerValueBuffer);
+    bool frontRangeCheck = frontRange < (refDistanceFromWall / (float)cos(0.78f) + rangerValueBuffer);
+
+    if (sideRangeCheck && frontRangeCheck)
+    {
+      prevHeading = currentHeading;
+      wallAngle = direction * (1.57f - (float)atan(frontRange / sideRange) + angleValueBuffer);
+      stateWF = transition(turnToAlignToWall);
+    }
+    // If went too far in heading
+    if (sideRange < refDistanceFromWall + rangerValueBuffer && frontRange > refDistanceFromWall + rangerThresholdLost)
+    {
+      aroundCornerBackTrack = false;
+      prevHeading = currentHeading;
+      stateWF = transition(findCorner);
+    }
+    break;
+
+  case turnToAlignToWall:;
+    bool alignWallCheck = logicIsCloseTo(wraptopi(currentHeading - prevHeading), wallAngle, angleValueBuffer);
+    if (alignWallCheck)
+    {
+      stateWF = transition(forwardAlongWall);
+    }
+    break;
+
+  case forwardAlongWall:
+    // If side range is out of reach,
+    //    end of the wall is reached
+    if (sideRange > refDistanceFromWall + rangerThresholdLost)
+    {
+      stateWF = transition(findCorner);
+    }
+    // If front range is small
+    //    then corner is reached
+    if (frontRange < refDistanceFromWall + rangerValueBuffer)
+    {
+      prevHeading = currentHeading;
+      stateWF = transition(rotateInCorner);
+    }
+    break;
+
+  case rotateAroundWall:
+    if (frontRange < refDistanceFromWall + rangerValueBuffer)
+    {
+      stateWF = transition(turnToFindWall);
+    }
+    break;
+
+  case rotateInCorner:;
+    // Check if heading goes over 0.8 rad
+    bool checkHeadingCorner = logicIsCloseTo(fabs(wraptopi(currentHeading - prevHeading)), inCornerAngle, angleValueBuffer);
+    if (checkHeadingCorner)
+    {
+      stateWF = transition(turnToFindWall);
+    }
+    break;
+
+  case findCorner:
+    if (sideRange <= refDistanceFromWall)
+    {
+      stateWF = transition(rotateAroundWall);
+    }
+    break;
+
+  default:
+    stateWF = transition(hover);
+  }
+
+  /***********************************************************
+   * Handle state actions
+   ***********************************************************/
+
+  float cmdVelXTemp = 0.0f;
+  float cmdVelYTemp = 0.0f;
+  float cmdAngWTemp = 0.0f;
+
+  switch (stateWF)
+  {
+  case forward:
+    cmdVelXTemp = maxForwardSpeed;
+    cmdVelYTemp = 0.0f;
+    cmdAngWTemp = 0.0f;
+    break;
+
+  case hover:
+    commandHover(&cmdVelXTemp, &cmdVelYTemp, &cmdAngWTemp);
+    break;
+
+  case turnToFindWall:
+    commandTurn(&cmdVelXTemp, &cmdAngWTemp, maxTurnRate);
+    cmdVelYTemp = 0.0f;
+    break;
+
+  case turnToAlignToWall:
+    if (timeNow - stateStartTime < waitForMeasurementSeconds)
+    {
+      commandHover(&cmdVelXTemp, &cmdVelYTemp, &cmdAngWTemp);
+    }
+    else
+    { // then turn again
+      commandTurn(&cmdVelXTemp, &cmdAngWTemp, maxTurnRate);
+      cmdVelYTemp = 0.0f;
+    }
+    break;
+
+  case forwardAlongWall:
+    commandForwardAlongWall(&cmdVelXTemp, &cmdVelYTemp, sideRange);
+    cmdAngWTemp = 0.0f;
+    break;
+
+  case rotateAroundWall:
+    // If first time around corner
+    //first try to find the corner again
+
+    // if side range is larger than prefered distance from wall
+    if (sideRange > refDistanceFromWall + rangerThresholdLost)
+    {
+
+      // check if scanning has already occured
+      if (wraptopi(fabs(currentHeading - prevHeading)) > inCornerAngle)
+      {
+        aroundCornerBackTrack = true;
+      }
+      // turn and adjust distnace to corner from that point
+      if (aroundCornerBackTrack)
+      {
+        // go back if it already went into one direction
+        commandTurnAndAdjust(&cmdVelYTemp, &cmdAngWTemp, -1 * maxTurnRate, sideRange);
+        cmdVelXTemp = 0.0f;
+      }
+      else
+      {
+        commandTurnAndAdjust(&cmdVelYTemp, &cmdAngWTemp, maxTurnRate, sideRange);
+        cmdVelXTemp = 0.0f;
+      }
+    }
+    else
+    {
+      // continue to turn around corner
+      prevHeading = currentHeading;
+      aroundCornerBackTrack = false;
+      commandTurnAroundCornerAndAdjust(&cmdVelXTemp, &cmdVelYTemp, &cmdAngWTemp, refDistanceFromWall, sideRange);
+    }
+    break;
+
+  case rotateInCorner:
+    commandTurn(&cmdVelXTemp, &cmdAngWTemp, maxTurnRate);
+    cmdVelYTemp = 0.0f;
+    break;
+
+  case findCorner:
+    commandAlignCorner(&cmdVelYTemp, &cmdAngWTemp, -1 * maxTurnRate, sideRange, refDistanceFromWall);
+    cmdVelXTemp = 0.0f;
+    break;
+
+  default:
+    //State does not exist so hover!!
+    commandHover(&cmdVelXTemp, &cmdVelYTemp, &cmdAngWTemp);
+  }
+
+  *cmdVelX = cmdVelXTemp;
+  *cmdVelY = cmdVelYTemp;
+  *cmdAngW = cmdAngWTemp;
+
+  return stateWF;
+}

FlyPi/crazyflie-firmware-2021.06/examples/demos/app_wall_following_demo/src/wallfollowing_multiranger_onboard.h

+/*
+ * wallfollowing_multirange_onboard.h
+ *
+ *  Created on: Aug 7, 2018
+ *      Author: knmcguire
+ */
+
+#ifndef SRC_WALLFOLLOWING_MULTIRANGER_ONBOARD_H_
+#define SRC_WALLFOLLOWING_MULTIRANGER_ONBOARD_H_
+#include <stdint.h>
+#include <stdbool.h>
+
+typedef enum
+{
+    forward,
+    hover,
+    turnToFindWall,
+    turnToAlignToWall,
+    forwardAlongWall,
+    rotateAroundWall,
+    rotateInCorner,
+    findCorner
+} StateWF;
+
+StateWF wallFollower(float *cmdVelX, float *cmdVelY, float *cmdAngW, float frontRange, float sideRange, float currentHeading,
+                     int directionTurn, float timeOuter);
+
+void adjustDistanceWall(float distanceWallNew);
+
+void wallFollowerInit(float refDistanceFromWallNew, float maxForwardSpeed_ref, StateWF initState);
+#endif /* SRC_WALLFOLLOWING_MULTIRANGER_ONBOARD_H_ */

FlyPi/crazyflie-firmware-2021.06/examples/demos/swarm_demo/.gitignore

+bin/*
+cf2.*

FlyPi/crazyflie-firmware-2021.06/examples/demos/swarm_demo/Makefile

+
+# enable app support
+APP=1
+APP_STACKSIZE=300
+
+## Weight of the Crazyflie + Qi + Lighthouse deck
+CFLAGS += -DCF_MASS=0.036f
+
+## Force lighthouse 2
+CFLAGS += -DLIGHTHOUSE_FORCE_TYPE=2
+
+# CLOAD_CMDS = -w radio://0/30/2M
+# DEBUG=1
+
+
+VPATH += src/
+PROJ_OBJ += app.o
+
+CRAZYFLIE_BASE=../../..
+include $(CRAZYFLIE_BASE)/Makefile

FlyPi/crazyflie-firmware-2021.06/examples/demos/swarm_demo/README.md

+# Demo application
+
+This folder contains an app layer application for the Crazyflie that is used as a demo.
+
+## Build
+
+Make sure that you are in the demo folder (not the main folder of the crazyflie firmware). Then type the following to build and flash it while the crazyflie is put into bootloader mode:
+
+```
+make clean
+make
+make cload
+```
+
+## Running the demo
+
+The demo is started by running the control_tower.py script

FlyPi/crazyflie-firmware-2021.06/examples/demos/swarm_demo/app.c

+#include <float.h>
+#include <math.h>
+
+#include "FreeRTOS.h"
+#include "timers.h"
+#include "deck_digital.h"
+#include "deck_constants.h"
+#include "sensors.h"
+#include "estimator_kalman.h"
+#include "crtp_commander_high_level.h"
+#include "commander.h"
+#include "pm.h"
+#include "stabilizer.h"
+#include "ledseq.h"
+#include "log.h"
+#include "param.h"
+#include "sitaw.h"
+#include "controller.h"
+#include "ledseq.h"
+#include "pptraj.h"
+#include "lighthouse_position_est.h"
+#include "lighthouse_core.h"
+
+#define DEBUG_MODULE "APP"
+#include "debug.h"
+
+
+static xTimerHandle timer;
+static bool isInit = false;
+
+static void appTimer(xTimerHandle timer);
+
+#define LED_LOCK         LED_GREEN_R
+#define LOCK_LENGTH 50
+#define LOCK_THRESHOLD 0.001f
+#define MAX_PAD_ERR 0.005
+#define TAKE_OFF_HEIGHT 0.2f
+#define LANDING_HEIGHT 0.12f
+#define SEQUENCE_SPEED 1.0f
+#define DURATION_TO_INITIAL_POSITION 2.0
+
+static uint32_t lockWriteIndex;
+static float lockData[LOCK_LENGTH][3];
+static void resetLockData();
+static bool hasLock();
+
+static bool takeOffWhenReady = false;
+static float goToInitialPositionWhenReady = -1.0f;
+static bool terminateTrajectoryAndLand = false;
+
+static float padX = 0.0;
+static float padY = 0.0;
+static float padZ = 0.0;
+
+static uint32_t landingTimeCheckCharge = 0;
+
+static float stabilizeEndTime;
+
+#define NO_PROGRESS -2000.0f
+static float currentProgressInTrajectory = NO_PROGRESS;
+static uint32_t trajectoryStartTime = 0;
+static uint32_t timeWhenToGoToInitialPosition = 0;
+static float trajectoryDurationMs = 0.0f;
+
+static float trajecory_center_offset_x = 0.0f;
+static float trajecory_center_offset_y = 0.0f;
+static float trajecory_center_offset_z = 0.0f;
+
+static uint32_t now = 0;
+static uint32_t flightTime = 0;
+
+// The nr of trajectories to fly
+static uint8_t trajectoryCount = 255;
+static uint8_t remainingTrajectories = 0;
+
+// Log and param ids
+static logVarId_t logIdStateEstimateX;
+static logVarId_t logIdStateEstimateY;
+static logVarId_t logIdStateEstimateZ;
+static logVarId_t logIdKalmanVarPX;
+static logVarId_t logIdKalmanVarPY;
+static logVarId_t logIdKalmanVarPZ;
+static logVarId_t logIdPmState;
+static logVarId_t logIdlighthouseEstBs0Rt;
+static logVarId_t logIdlighthouseEstBs1Rt;
+
+static paramVarId_t paramIdStabilizerController;
+static paramVarId_t paramIdCommanderEnHighLevel;
+static paramVarId_t paramIdLighthouseMethod;
+
+//#define USE_MELLINGER
+
+#define TRAJ_Y_OFFSET 0.35
+
+enum State {
+  // Initialization
+  STATE_IDLE = 0,
+  STATE_WAIT_FOR_POSITION_LOCK,
+
+  STATE_WAIT_FOR_TAKE_OFF, // Charging
+  STATE_TAKING_OFF,
+  STATE_HOVERING,
+  STATE_WAITING_TO_GO_TO_INITIAL_POSITION,
+  STATE_GOING_TO_INITIAL_POSITION,
+  STATE_RUNNING_TRAJECTORY,
+  STATE_GOING_TO_PAD,
+  STATE_WAITING_AT_PAD,
+  STATE_LANDING,
+  STATE_CHECK_CHARGING,
+  STATE_REPOSITION_ON_PAD,
+  STATE_CRASHED,
+};
+
+static enum State state = STATE_IDLE;
+
+ledseqStep_t seq_lock_def[] = {
+  { true, LEDSEQ_WAITMS(1000)},
+  {    0, LEDSEQ_LOOP},
+};
+
+ledseqContext_t seq_lock = {
+  .sequence = seq_lock_def,
+  .led = LED_LOCK,
+};
+
+const uint8_t trajectoryId = 1;
+
+// duration, x0-x7, y0-y7, z0-z7, yaw0-yaw7
+static struct poly4d sequence[] = {
+  {.duration = 0.55, .p = {{3.850187562676742e-11,-6.126680001583386e-07,0.034011535082562466,-0.0003930116221214229,-0.13617012073728407,-0.013475566231476182,0.13052917793131502,-0.04578980321072286}, {5.13033102278894e-11,-6.894694832263486e-07,2.7736466486255528e-05,0.09665022145136354,0.003058298046984803,-0.14597702960520773,0.025898651835890598,0.031816204956814045}, {0.6948075950058442,0.2683789944542717,0.023245728473115564,-0.010134659315685406,-0.00043890670824819824,0.00011480619184472054,3.331435279368211e-06,-6.413660347948354e-07}, {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,}}},
+  {.duration = 0.55, .p = {{-6.914788682055622e-11,-0.029193709157554027,-0.10559495462082236,-0.05347086572904071,0.1429864490023195,0.13154711964776383,-0.10576667456482508,-0.004894549656606144}, {0.010222252124903127,0.03695902744452226,-0.0088512292032624,-0.1522994840241329,-0.10473818933895811,0.101347754562758,0.10503082012800732,-0.06287119477122378}, {0.8477274186362265,0.2845134091691316,0.005921828500929774,-0.01074393477303553,-0.00011181155704426794,0.00012171542649083367,8.448121214720302e-07,-6.586027744088609e-0}, {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,}}},
+  {.duration = 0.55, .p = {{-0.04019237884462183,-0.07140003156906653,0.1344931065364669,0.2937887095214804,0.009789196771361027,-0.21053548856185011,-0.05387360520003008,0.07164061868443176}, {-7.889581297779964e-11,-0.11478801738672623,-0.20396423865607044,0.07270282032653184,0.2794859692648988,0.07498870791184684,-0.17675426223153057,0.027076140218982695}, {1.0042095323020248,0.28125870522972984,-0.011805634296686286,-0.010621028836870822,0.00022290339530923512,0.00012032988892924413,-1.6993066948952098e-06,-6.309899868949724e-07}, {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,}}},
+  {.duration = 0.55, .p = {{7.988284484732011e-11,0.250949124563498,0.2884318844281135,-0.2732723085219975,-0.3971380460135978,0.019843843315547182,0.23402372804388663,-0.06191711498753417}, {-0.08786796558968513,-0.1009753021910108,0.33435181061014774,0.4152184312151822,-0.14220624001585802,-0.30664667217939623,0.019456187255464174,0.07150045317180133}, {1.153589938555835,0.25883668533089255,-0.028728562626473653,-0.009774317339295326,0.0005424278565983879,0.00011074405151188298,-4.127597844930454e-06,-5.60388917887036e-07}, {-8.314418855166948e-17,2.855993321445286,-4.4522405463652314e-13,3.6027429443266216e-12,-1.4127312536830109e-11,2.8100825191264194e-11,-2.6527583249725554e-11,8.815269909223687e-12,}}},
+  {.duration = 0.55, .p = {{0.1499999999110494,0.1236693334929018,-0.5948073010234445,-0.5083134142475911,0.34088988320791197,0.38313148703735755,-0.10996125300699039,-0.06246025031151826}, {7.204165642107989e-11,0.42839787625064485,0.3532415614668606,-0.5345691196919508,-0.48792488465828615,0.14648785242312232,0.27367225249021054,-0.09725401975942494}, {1.285688609574253,0.21877537307462952,-0.043693686890084134,-0.008261502261561984,0.0008249867228197418,9.361128232250471e-05,-6.274729564937099e-06,-4.5152734395454723e-07}, {1.570796326794896,2.855993321445219,1.1415585581287713e-12,-1.1944049757036972e-11,6.431141607525981e-11,-1.8343792153223745e-10,2.6289169140858755e-10,-1.4907548089075316e-10,}}},
+  {.duration = 0.55, .p = {{-5.590663147591574e-11,-0.6350414332686105,-0.39397659738539126,0.8387863079720367,0.5456595121538056,-0.29631273940020514,-0.29299785427839664,0.130678702950142}, {0.2223542863477553,0.13793556474518312,-0.8981099665280236,-0.5667293893693737,0.5727217708530097,0.43477761941329673,-0.21147382139585272,-0.04513608499171106}, {1.3915032392307496,0.1638048806867086,-0.05568115859886775,-0.006185679463293696,0.001051324141053692,7.009895638961352e-05,-7.994094322078248e-06,-3.1198566063354317e-07}, {-3.1415926535897913,2.855993321445259,2.5087977390525737e-13,-1.1471253057481462e-12,-3.6097331487140355e-12,3.293016202670872e-11,-7.167566690499768e-11,5.132340432324304e-11,}}},
+  {.duration = 0.55, .p = {{-0.2999999998502529,-0.14280177586409412,1.2235902317016245,0.5864854044065786,-0.821902942890023,-0.4580654708546404,0.3170759788277885,0.020708570338654426}, {-3.2577411606298086e-11,-0.8567973787012538,-0.40786096680320616,1.16519197471624,0.5664074090974773,-0.4591081859649081,-0.29068352530931285,0.1599133274637631}, {1.4638227353811781,0.09767135638036385,-0.06387405136662831,-0.0036883128327376896,0.0012060155635279994,4.180948131089076e-05,-9.168649106299251e-06,-1.5119295149245228e-07}, {-1.570796326794896,2.855993321445285,-4.225442457827085e-13,4.4049449702295166e-12,-2.3950348360899724e-11,6.851557251898727e-11,-9.803060051919491e-11,5.52677029888256e-11,}}},
+  {.duration = 0.55, .p = {{3.643713234144152e-12,1.0785534113363267,0.3939484728911548,-1.4915421131692534,-0.5487546404587444,0.6237799510123201,0.266886983994221,-0.18296560239343668}, {-0.3776457133589556,-0.1379363426036038,1.5490671543520358,0.5662351196076164,-1.0714521143729914,-0.4514080123736052,0.41957111221051374,-0.00915759855778746}, {1.497718643876504,0.02488169054632043,-0.06771403309317417,-0.0009395937484483393,0.0012785188596592392,1.0671200625251397e-05,-9.718988081428503e-06,2.0238909278164195e-08}, {-8.314418855166948e-17,2.855993321445286,-4.4522405463652314e-13,3.6027429443266216e-12,-1.4127312536830109e-11,2.8100825191264194e-11,-2.6527583249725554e-11,8.815269909223687e-12,}}},
+  {.duration = 0.55, .p = {{0.44999999981385364,0.12367083619999372,-1.8523600200796242,-0.5073585583818294,1.3043629214703734,0.41525893933383995,-0.5119743484215598,0.04242709212774574}, {-2.89226389768401e-11,1.285197224018999,0.35318722911504113,-1.7955965006790715,-0.4939042134616532,0.7791059265493233,0.223229924284725,-0.19826455269120852}, {1.4908810145684093,-0.049603621379605944,-0.06693941536376884,0.001873157056002599,0.0012638934376494498,-2.1194960501981645e-05,-9.606060589372755e-06,1.8976712320425972e-07}, {1.570796326794896,2.855993321445219,1.1415585581287713e-12,-1.1944049757036972e-11,6.431141607525981e-11,-1.8343792153223745e-10,2.6289169140858755e-10,-1.4907548089075316e-10,}}},
+  {.duration = 0.55, .p = {{6.290210506218638e-11,-1.4626463824098959,-0.28835504688017033,2.056634333247623,0.40559409384743944,-0.914500903393737,-0.16268750359645887,0.20476758072452098}, {0.5121320341641584,0.10097742733972069,-2.11279992129731,-0.41386806111463986,1.504762877791794,0.35208175069725167,-0.5879885586908895,0.07683264882809585}, {1.443775820594523,-0.12070852848241553,-0.06160298710010411,0.004558255313738797,0.0011631356744089595,-5.161639604707585e-05,-8.839022757646207e-06,3.4668809927088064e-07}, {-3.1415926535897913,2.855993321445259,2.5087977390525737e-13,-1.1471253057481462e-12,-3.6097331487140355e-12,3.293016202670872e-11,-7.167566690499768e-11,5.132340432324304e-11,}}},
+  {.duration = 0.55, .p = {{-0.5598076209469373,-0.07140263433406184,2.3126383091368354,0.29213485066461087,-1.6589950573170387,-0.26618186797986115,0.6424335009665487,-0.1100295872652435}, {9.597957855088104e-11,-1.5988080196133752,-0.20387013226277426,2.256866313938279,0.28984247051151446,-1.0207379381606654,-0.08938558706550837,0.20203151543026565}, {1.3596132030791528,-0.18358734884936753,-0.05206841706985553,0.006932716003333422,0.0009831121712365765,-7.852040976178658e-05,-7.4693513883816284e-06,4.798093435152236e-07}, {-1.570796326794896,2.855993321445285,-4.225442457827085e-13,4.4049449702295166e-12,-2.3950348360899724e-11,6.851557251898727e-11,-9.803060051919491e-11,5.52677029888256e-11,}}},
+  {.duration = 0.55, .p = {{-1.2590019835852742e-10,1.6844029450716946,0.10548999257784991,-2.3826469641845596,-0.1545376251943579,1.09057715220263,0.00831957959381703,-0.19024281538328217}, {-0.5897777477105767,-0.0369619304516384,2.4382565277141275,0.1504548441642516,-1.7565487919657432,-0.16341322602586436,0.6715988421120441,-0.13975559077778596}, {1.244128705320668,-0.2339549947846461,-0.038985470464346035,0.008834723564599386,0.0007360911420512199,-0.00010007323169172001,-5.590868353790516e-06,5.803448593771976e-07}, {-8.314418855166948e-17,2.855993321445286,-4.4522405463652314e-13,3.6027429443266216e-12,-1.4127312536830109e-11,2.8100825191264194e-11,-2.6527583249725554e-11,8.815269909223687e-12,}}},
+  {.duration = 0.55, .p = {{0.5999999998441106,2.392746148312848e-06,-2.4810939030286048,0.001516700094742569,1.7907759558714795,0.05077933811777355,-0.6734970125477634,0.1639848814705509}, {-1.506250987376076e-10,1.7135980060672011,-8.092823644568708e-05,-2.4254045405309936,-0.00890035951518397,1.1192591185216,-0.07498600440013853,-0.17020486100199314}, {1.105192404994155,-0.2683789944542932,-0.023245728472413945,0.01013465930675449,0.00043890675937640944,-0.00011480633720814209,-3.331233868784164e-06,6.412574240165199e-07}, {1.570796326794896,2.855993321445219,1.1415585581287713e-12,-1.1944049757036972e-11,6.431141607525981e-11,-1.8343792153223745e-10,2.6289169140858755e-10,-1.4907548089075316e-10,}}},
+  {.duration = 0.55, .p = {{1.684697291033214e-10,-1.6844036074401618,0.10564814639079924,2.382225184807529,-0.1371443875231521,-1.1048292086075555,0.1548540272068101,0.14328320564974964}, {0.5897777477577096,-0.036957247366396044,-2.4382311387421707,0.1534231724906406,1.7593440245030696,-0.06404398275378732,-0.6479986546427713,0.18106627297730243}, {0.9522725813637728,-0.2845134091691431,-0.005921828500469207,0.01074393476605049,0.00011181160327455771,-0.00012171557551975708,-8.445807090820611e-07,6.584637616417346e-07}, {-3.1415926535897913,2.855993321445259,2.5087977390525737e-13,-1.1471253057481462e-12,-3.6097331487140355e-12,3.293016202670872e-11,-7.167566690499768e-11,5.132340432324304e-11,}}},
+  {.duration = 0.55, .p = {{-0.5598076210379905,0.07139825149092185,2.312589261409516,-0.2949123979936194,-1.664395031908084,0.17323171668372522,0.5968414398024987,-0.18983569693480895}, {1.7821782545551847e-10,-1.5988092992110026,0.20401743042640863,2.25605149874844,-0.2736439077696821,-1.0482707969090843,0.22584161491618038,0.11131251575573874}, {0.7957904676979738,-0.28125870522974444,0.011805634297233001,0.010621028829982298,-0.00022290335524226357,-0.00012033000419331912,1.699466435538814e-06,6.309046379744497e-07}, {-1.570796326794896,2.855993321445285,-4.225442457827085e-13,4.4049449702295166e-12,-2.3950348360899724e-11,6.851557251898727e-11,-9.803060051919491e-11,5.52677029888256e-11,}}},
+  {.duration = 0.55, .p = {{-1.7920484419930034e-10,1.462648192034213,-0.2884850761977632,-2.055482010561792,0.39129598457085574,0.9534382455215172,-0.2831110804852299,-0.07647154113270976}, {-0.5121320342929275,0.10097352211288317,2.11273055733514,-0.41634211967852053,-1.5123995951714493,0.2693429004489931,0.5235116471329888,-0.1896955312999179}, {0.6464100614441641,-0.2588366853309645,0.028728562628598252,0.009774317314729044,-0.0005424277188845949,-0.00011074445057684398,4.128172898051939e-06,5.600631449541777e-07}, {-8.314418855166948e-17,2.855993321445286,-4.4522405463652314e-13,3.6027429443266216e-12,-1.4127312536830109e-11,2.8100825191264194e-11,-2.6527583249725554e-11,8.815269909223687e-12,}}},
+  {.duration = 0.55, .p = {{0.44999999997156337,-0.12366755341478729,-1.8522750669218213,0.5094371027127081,1.3137159519637331,-0.34582771525204625,-0.43300658146993454,0.180655328493912}, {-1.713634072390606e-10,1.2851994403470866,-0.3532947532373552,-1.7941851993804268,0.48208282314515644,0.8267942366336414,-0.3227596052703462,-0.041134636155708774}, {0.514311390425746,-0.21877537307467282,0.0436936868909435,0.008261502253779647,-0.0008249866851128808,-9.36113819283429e-05,6.274863968829055e-06,4.51455051613041e-07}, {1.570796326794896,2.855993321445219,1.1415585581287713e-12,-1.1944049757036972e-11,6.431141607525981e-11,-1.8343792153223745e-10,2.6289169140858755e-10,-1.4907548089075316e-10,}}},
+  {.duration = 0.55, .p = {{1.552285277508215e-10,-1.0785558833291304,0.3940297891560065,1.4899680111000886,-0.5398174506435933,-0.6769693496382162,0.34208520701934,0.0077099529941195115}, {0.3776457135348575,-0.13793378466699563,-1.5489724014191015,0.5678530778537859,1.0818840642180365,-0.3974738473501345,-0.33149401346895935,0.16333116338935125}, {0.4084967607692496,-0.16380488068674098,0.05568115859943449,0.00618567945863584,-0.0010513241211192762,-7.009900241094177e-05,7.994148431007077e-06,3.119604503872529e-07}, {-3.1415926535897913,2.855993321445259,2.5087977390525737e-13,-1.1471253057481462e-12,-3.6097331487140355e-12,3.293016202670872e-11,-7.167566690499768e-11,5.132340432324304e-11,}}},
+  {.duration = 0.55, .p = {{-0.30000000003235977,0.14279999578597702,1.2234921362438944,-0.587609092875647,-0.8327028922566038,0.4207616989920525,0.2258918557649712,-0.13890364858850515}, {1.3189926344258986e-10,-0.8567999378964555,0.407914158573091,1.1635623443630034,-0.5605653476226719,-0.5141739029796313,0.3397708779238144,-0.021524671451362908}, {0.3361772646188215,-0.09767135638040796,0.06387405136797168,0.0036883128165148063,-0.001206015469879388,-4.180975783766554e-05,9.169052818128353e-06,1.5096177044506199e-07}, {-1.570796326794896,2.855993321445285,-4.225442457827085e-13,4.4049449702295166e-12,-2.3950348360899724e-11,6.851557251898727e-11,-9.803060051919491e-11,5.52677029888256e-11,}}},
+  {.duration = 0.55, .p = {{-1.029656785148531e-10,0.6350439052614014,-0.39400166466153436,-0.8372122059040767,0.5429125789487638,0.3495021380409436,-0.3159743367761405,0.04457694648242842}, {-0.2223542865236572,0.13793456252546576,0.8980152135936907,-0.5673588080774555,-0.5831537207722597,0.4141042405096757,0.12339672238346343,-0.10903747969310579}, {0.3022813561234955,-0.024881690546276862,0.06771403309177028,0.0009395937627441363,-0.0012785189282877262,-1.0671027269943336e-05,9.71876468535445e-06,-2.0123140548204047e-08}, {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,}}},
+  {.duration = 0.55, .p = {{0.1500000000687584,-0.12366905612177849,-0.594722347868441,0.5084822468775191,0.35024291353796916,-0.3779551670922615,-0.030993486696122376,0.07576798641159813}, {-7.039928298561939e-11,0.42840009257875555,-0.3532404208862144,-0.533157818385911,0.48806215190950436,0.19417616261494053,-0.2723172772132073,0.059875896856828974}, {0.30911898543159044,0.04960362137961011,0.06693941536418026,-0.0018731570659139287,-0.0012638933619877498,2.1194700149092597e-05,9.60648002352551e-06,-1.9002499360993986e-07}, {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,}}},
+  {.duration = 0.55, .p = {{3.6419711080421585e-11,-0.2509509341878004,0.28840823864950105,0.2721199858390575,-0.3997520324183086,-0.05878118540823653,0.21177485599121348,-0.0663789245795264}, {0.08786796571845405,-0.10097564726160337,-0.3342824466477831,0.41499174957660356,0.14984295740004752,-0.3147779789736173,0.045020724305445986,0.04136242930126668}, {0.35622417940547646,0.12070852848236742,0.061602987101403175,-0.004558255327930595,-0.0011631355987503336,5.1616188097919715e-05,8.839307227640228e-06,-3.46842188288027e-07}, {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,}}},
+  {.duration = 0.55, .p = {{-0.040192378935675475,0.07140085425594436,0.13444405880828958,-0.29325853912678074,0.004389222125442104,0.2288780962570675,-0.09946566658297257,-0.008165490863377235}, {3.342506343117202e-12,-0.11478929698434916,0.2039233240328519,0.07188800514013687,-0.28400040903603785,0.047455849220319694,0.1384729396693574,-0.06364285941067112}, {0.4403867969208466,0.1835873488493235,0.05206841707153454,-0.006932716025413217,-0.0009831120388661323,7.85200125196987e-05,7.469934672375333e-06,-4.801436347075494e-07}, {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,}}},
+  {.duration = 0.55, .p = {{2.657819140362752e-11,0.029194371526024346,-0.10554318434787024,0.05389264510610344,0.14869556371716416,-0.11729506325396347,-0.057406932212603616,0.05185415937314559}, {-0.01022225217203592,0.036960150373509054,0.00882584023135473,-0.15157853263098095,0.10194295680128089,0.12610945422193115,-0.12863100760957105,0.021560512581067035}, {0.5558712946793309,0.23395499478462367,0.03898547046555688,-0.008834723582265662,-0.0007360910302886222,0.00010007288325451688,5.591396797932501e-06,-5.806568866668146e-07}, {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,}}},
+};
+
+static float sequenceTime(struct poly4d sequence[], int count) {
+  float totalDuration = 0.0f;
+
+  for (int i = 0; i < count; i++) {
+    totalDuration += sequence[i].duration;
+  }
+
+  return totalDuration;
+}
+
+static float getX() { return logGetFloat(logIdStateEstimateX); }
+static float getY() { return logGetFloat(logIdStateEstimateY); }
+static float getZ() { return logGetFloat(logIdStateEstimateZ); }
+static float getVarPX() { return logGetFloat(logIdKalmanVarPX); }
+static float getVarPY() { return logGetFloat(logIdKalmanVarPY); }
+static float getVarPZ() { return logGetFloat(logIdKalmanVarPZ); }
+static bool isBatLow() { return logGetInt(logIdPmState) == lowPower; }
+static bool isCharging() { return logGetInt(logIdPmState) == charging; }
+static bool isLighthouseAvailable() { return logGetFloat(logIdlighthouseEstBs0Rt) >= 0.0f || logGetFloat(logIdlighthouseEstBs1Rt) >= 0.0f; }
+
+const baseStationGeometry_t lighthouseGeoData[PULSE_PROCESSOR_N_BASE_STATIONS]  = {
+ {.valid = true, .origin = {-2.057947, 0.398319, 3.109704, }, .mat = {{0.807210, 0.002766, 0.590258, }, {0.067095, 0.993078, -0.096409, }, {-0.586439, 0.117426, 0.801437, }, }},
+ {.valid = true, .origin = {0.866244, -2.566829, 3.132632, }, .mat = {{-0.043296, -0.997675, -0.052627, }, {0.766284, -0.066962, 0.639003, }, {-0.641042, -0.012661, 0.767401, }, }},
+};
+
+lighthouseCalibration_t lighthouseCalibrationData[PULSE_PROCESSOR_N_BASE_STATIONS] = {
+  { // Base station 0
+    .sweep = {
+      {.tilt = -0.047353, .phase = 0.000000, .curve = 0.478887, .gibphase = 1.023093, .gibmag = 0.005071, .ogeephase = 1.136886, .ogeemag = -0.520102, },
+      {.tilt = 0.049104, .phase = -0.006642, .curve = 0.675827, .gibphase = 2.367835, .gibmag = 0.004907, .ogeephase = 1.900456, .ogeemag = -0.457289, },
+    },
+    .uid = 0x3C65D22F,
+    .valid = true,
+  },
+  { // Base station 1
+    .sweep = {
+      {.tilt = -0.048959, .phase = 0.000000, .curve = 0.144913, .gibphase = 1.288635, .gibmag = -0.005397, .ogeephase = 2.004001, .ogeemag = 0.033096, },
+      {.tilt = 0.047509, .phase = -0.004676, .curve = 0.374379, .gibphase = 1.727613, .gibmag = -0.005642, .ogeephase = 2.586835, .ogeemag = 0.117884, },
+    },
+    .uid = 0x34C2AD7E,
+    .valid = true,
+  },
+};
+
+#ifdef USE_MELLINGER
+static void enableMellingerController() { paramSetInt(paramIdStabilizerController, ControllerTypeMellinger); }
+#endif
+static void enableHighlevelCommander() { paramSetInt(paramIdCommanderEnHighLevel, 1); }
+static void useCrossingBeamPositioningMethod() { paramSetInt(paramIdLighthouseMethod, 0); }
+
+static void setupLighthouse() {
+  lighthousePositionSetGeometryData(0, &lighthouseGeoData[0]);
+  lighthousePositionSetGeometryData(1, &lighthouseGeoData[1]);
+
+  lighthouseCoreSetCalibrationData(0, &lighthouseCalibrationData[0]);
+  lighthouseCoreSetCalibrationData(1, &lighthouseCalibrationData[1]);
+
+  useCrossingBeamPositioningMethod();
+}
+
+static void defineTrajectory() {
+  const uint32_t polyCount = sizeof(sequence) / sizeof(struct poly4d);
+  trajectoryDurationMs = 1000 * sequenceTime(sequence, polyCount);
+  crtpCommanderHighLevelWriteTrajectory(0, sizeof(sequence), (uint8_t*)sequence);
+  crtpCommanderHighLevelDefineTrajectory(trajectoryId, CRTP_CHL_TRAJECTORY_TYPE_POLY4D, 0, polyCount);
+}
+
+static void defineLedSequence() {
+  ledseqRegisterSequence(&seq_lock);
+}
+
+void appMain() {
+  if (isInit) {
+    return;
+  }
+
+  DEBUG_PRINT("This is a demo app\n");
+
+  // Get log and param ids
+  logIdStateEstimateX = logGetVarId("stateEstimate", "x");
+  logIdStateEstimateY = logGetVarId("stateEstimate", "y");
+  logIdStateEstimateZ = logGetVarId("stateEstimate", "z");
+  logIdKalmanVarPX = logGetVarId("kalman", "varPX");
+  logIdKalmanVarPY = logGetVarId("kalman", "varPY");
+  logIdKalmanVarPZ = logGetVarId("kalman", "varPZ");
+  logIdPmState = logGetVarId("pm", "state");
+  logIdlighthouseEstBs0Rt = logGetVarId("lighthouse", "estBs0Rt");
+  logIdlighthouseEstBs1Rt = logGetVarId("lighthouse", "estBs1Rt");
+
+  paramIdStabilizerController = paramGetVarId("stabilizer", "controller");
+  paramIdCommanderEnHighLevel = paramGetVarId("commander", "enHighLevel");
+  paramIdLighthouseMethod = paramGetVarId("lighthouse", "method");
+
+
+  timer = xTimerCreate("AppTimer", M2T(20), pdTRUE, NULL, appTimer);
+  xTimerStart(timer, 20);
+
+  pinMode(DECK_GPIO_IO3, INPUT_PULLUP);
+
+  #ifdef USE_MELLINGER
+    enableMellingerController();
+  #endif
+
+  setupLighthouse();
+  enableHighlevelCommander();
+  defineTrajectory();
+  defineLedSequence();
+  resetLockData();
+
+  isInit = true;
+}
+
+static void appTimer(xTimerHandle timer) {
+  uint32_t previous = now;
+  now = xTaskGetTickCount();
+  uint32_t delta = now - previous;
+
+  if(sitAwTuDetected()) {
+    state = STATE_CRASHED;
+  }
+
+  if (isBatLow()) {
+    terminateTrajectoryAndLand = true;
+  }
+
+  switch(state) {
+    case STATE_IDLE:
+      DEBUG_PRINT("Let's go! Waiting for position lock...\n");
+      state = STATE_WAIT_FOR_POSITION_LOCK;
+      break;
+    case STATE_WAIT_FOR_POSITION_LOCK:
+      if (hasLock()) {
+        DEBUG_PRINT("Position lock acquired, ready for take off..\n");
+        ledseqRun(&seq_lock);
+        state = STATE_WAIT_FOR_TAKE_OFF;
+      }
+      break;
+    case STATE_WAIT_FOR_TAKE_OFF:
+      trajectoryStartTime = 0;
+      if (takeOffWhenReady) {
+        takeOffWhenReady = false;
+        DEBUG_PRINT("Taking off!\n");
+
+        padX = getX();
+        padY = getY();
+        padZ = getZ();
+        DEBUG_PRINT("Base position: (%f, %f, %f)\n", (double)padX, (double)padY, (double)padZ);
+
+        terminateTrajectoryAndLand = false;
+        crtpCommanderHighLevelTakeoff(padZ + TAKE_OFF_HEIGHT, 1.0);
+        state = STATE_TAKING_OFF;
+      }
+      break;
+    case STATE_TAKING_OFF:
+      if (crtpCommanderHighLevelIsTrajectoryFinished()) {
+        DEBUG_PRINT("Hovering, waiting for command to start\n");
+        ledseqStop(&seq_lock);
+        state = STATE_HOVERING;
+
+      }
+      flightTime += delta;
+      break;
+    case STATE_HOVERING:
+      if (terminateTrajectoryAndLand) {
+          terminateTrajectoryAndLand = false;
+          DEBUG_PRINT("Terminating hovering\n");
+          state = STATE_GOING_TO_PAD;
+      } else {
+        if (goToInitialPositionWhenReady >= 0.0f) {
+          float delayMs = goToInitialPositionWhenReady * trajectoryDurationMs;
+          timeWhenToGoToInitialPosition = now + delayMs;
+          trajectoryStartTime = now + delayMs;
+          goToInitialPositionWhenReady = -1.0f;
+          DEBUG_PRINT("Waiting to go to initial position for %d ms\n", (int)delayMs);
+          state = STATE_WAITING_TO_GO_TO_INITIAL_POSITION;
+        }
+      }
+      flightTime += delta;
+      break;
+    case STATE_WAITING_TO_GO_TO_INITIAL_POSITION:
+      if (now >= timeWhenToGoToInitialPosition) {
+        DEBUG_PRINT("Going to initial position\n");
+        crtpCommanderHighLevelGoTo(sequence[0].p[0][0] + trajecory_center_offset_x, sequence[0].p[1][0] + trajecory_center_offset_y, sequence[0].p[2][0] + trajecory_center_offset_z, sequence[0].p[3][0], DURATION_TO_INITIAL_POSITION, false);
+        state = STATE_GOING_TO_INITIAL_POSITION;
+      }
+      flightTime += delta;
+      break;
+    case STATE_GOING_TO_INITIAL_POSITION:
+      currentProgressInTrajectory = (now - trajectoryStartTime) / trajectoryDurationMs;
+
+      if (crtpCommanderHighLevelIsTrajectoryFinished()) {
+        DEBUG_PRINT("At initial position, starting trajectory...\n");
+        crtpCommanderHighLevelStartTrajectory(trajectoryId, SEQUENCE_SPEED, true, false);
+        remainingTrajectories = trajectoryCount - 1;
+        state = STATE_RUNNING_TRAJECTORY;
+      }
+      flightTime += delta;
+      break;
+    case STATE_RUNNING_TRAJECTORY:
+      currentProgressInTrajectory = (now - trajectoryStartTime) / trajectoryDurationMs;
+
+      if (crtpCommanderHighLevelIsTrajectoryFinished()) {
+        if (terminateTrajectoryAndLand || (remainingTrajectories == 0)) {
+          terminateTrajectoryAndLand = false;
+          DEBUG_PRINT("Terminating trajectory, going to pad...\n");
+          float timeToPadPosition = 2.0;
+          crtpCommanderHighLevelGoTo(padX, padY, padZ + LANDING_HEIGHT, 0.0, timeToPadPosition, false);
+          currentProgressInTrajectory = NO_PROGRESS;
+          state = STATE_GOING_TO_PAD;
+        } else {
+          if (remainingTrajectories > 0) {
+            DEBUG_PRINT("Trajectory finished, restarting...\n");
+            crtpCommanderHighLevelStartTrajectory(trajectoryId, SEQUENCE_SPEED, true, false);
+          }
+          remainingTrajectories--;
+        }
+      }
+      flightTime += delta;
+      break;
+    case STATE_GOING_TO_PAD:
+      if (crtpCommanderHighLevelIsTrajectoryFinished()) {
+        DEBUG_PRINT("Over pad, stabalizing position\n");
+        stabilizeEndTime = now + 5000;
+        state = STATE_WAITING_AT_PAD;
+      }
+      flightTime += delta;
+      break;
+    case STATE_WAITING_AT_PAD:
+      if (now > stabilizeEndTime || ((fabs(padX - getX()) < MAX_PAD_ERR) && (fabs(padY - getY()) < MAX_PAD_ERR))) {
+        if (now > stabilizeEndTime) {
+          DEBUG_PRINT("Warning: timeout!\n");
+        }
+
+        DEBUG_PRINT("Landing...\n");
+        crtpCommanderHighLevelLand(padZ, 1.0);
+        state = STATE_LANDING;
+      }
+      flightTime += delta;
+      break;
+    case STATE_LANDING:
+      if (crtpCommanderHighLevelIsTrajectoryFinished()) {
+        DEBUG_PRINT("Landed. Feed me!\n");
+        crtpCommanderHighLevelStop();
+        landingTimeCheckCharge = now + 3000;
+        state = STATE_CHECK_CHARGING;
+      }
+      flightTime += delta;
+      break;
+    case STATE_CHECK_CHARGING:
+      if (now > landingTimeCheckCharge) {
+        DEBUG_PRINT("isCharging: %d\n", isCharging());
+        if (isCharging()) {
+          ledseqRun(&seq_lock);
+          state = STATE_WAIT_FOR_TAKE_OFF;
+        } else {
+          DEBUG_PRINT("Not charging. Try to reposition on pad.\n");
+          crtpCommanderHighLevelTakeoff(padZ + LANDING_HEIGHT, 1.0);
+          state = STATE_REPOSITION_ON_PAD;
+        }
+      }
+      break;
+    case STATE_REPOSITION_ON_PAD:
+      if (crtpCommanderHighLevelIsTrajectoryFinished()) {
+        DEBUG_PRINT("Over pad, stabalizing position\n");
+        crtpCommanderHighLevelGoTo(padX, padY, padZ + LANDING_HEIGHT, 0.0, 1.5, false);
+        state = STATE_GOING_TO_PAD;
+      }
+      flightTime += delta;
+      break;
+    case STATE_CRASHED:
+      crtpCommanderHighLevelStop();
+      break;
+    default:
+      break;
+  }
+}
+
+
+static bool hasLock() {
+  bool result = false;
+
+  // Store current state
+  lockData[lockWriteIndex][0] = getVarPX();
+  lockData[lockWriteIndex][1] = getVarPY();
+  lockData[lockWriteIndex][2] = getVarPZ();
+
+  lockWriteIndex++;
+  if (lockWriteIndex >= LOCK_LENGTH) {
+    lockWriteIndex = 0;
+  }
+
+  // Check if we have a lock
+  int count = 0;
+
+  float lXMax = FLT_MIN;
+  float lYMax = FLT_MIN;
+  float lZMax = FLT_MIN;
+
+  float lXMin = FLT_MAX;
+  float lYMin = FLT_MAX;
+  float lZMin = FLT_MAX;
+
+  for (int i = 0; i < LOCK_LENGTH; i++) {
+    if (lockData[i][0] != FLT_MAX) {
+      count++;
+
+      lXMax = fmaxf(lXMax, lockData[i][0]);
+      lYMax = fmaxf(lYMax, lockData[i][1]);
+      lZMax = fmaxf(lZMax, lockData[i][2]);
+
+      lXMin = fminf(lXMax, lockData[i][0]);
+      lYMin = fminf(lYMin, lockData[i][1]);
+      lZMin = fminf(lZMin, lockData[i][2]);
+    }
+  }
+
+  result =
+    (count >= LOCK_LENGTH) &&
+    ((lXMax - lXMin) < LOCK_THRESHOLD) &&
+    ((lYMax - lYMin) < LOCK_THRESHOLD) &&
+    ((lZMax - lZMin) < LOCK_THRESHOLD &&
+    isLighthouseAvailable() &&  // Make sure we have a deck and the Lighthouses are powered
+    sensorsAreCalibrated());
+
+  return result;
+}
+
+static void resetLockData() {
+    lockWriteIndex = 0;
+    for (uint32_t i = 0; i < LOCK_LENGTH; i++) {
+      lockData[i][0] = FLT_MAX;
+      lockData[i][1] = FLT_MAX;
+      lockData[i][2] = FLT_MAX;
+    }
+}
+
+PARAM_GROUP_START(app)
+  PARAM_ADD(PARAM_UINT8, takeoff, &takeOffWhenReady)
+  PARAM_ADD(PARAM_FLOAT, start, &goToInitialPositionWhenReady)
+  PARAM_ADD(PARAM_UINT8, stop, &terminateTrajectoryAndLand)
+  PARAM_ADD(PARAM_FLOAT, offsx, &trajecory_center_offset_x)
+  PARAM_ADD(PARAM_FLOAT, offsy, &trajecory_center_offset_y)
+  PARAM_ADD(PARAM_FLOAT, offsz, &trajecory_center_offset_z)
+  PARAM_ADD(PARAM_UINT8, trajcount, &trajectoryCount)
+PARAM_GROUP_STOP(app)
+
+LOG_GROUP_START(app)
+  LOG_ADD(LOG_UINT8, state, &state)
+  LOG_ADD(LOG_FLOAT, prgr, &currentProgressInTrajectory)
+  LOG_ADD(LOG_UINT32, uptime, &now)
+  LOG_ADD(LOG_UINT32, flighttime, &flightTime)
+LOG_GROUP_STOP(app)

FlyPi/crazyflie-firmware-2021.06/examples/demos/swarm_demo/cload-all.sh

+#!/usr/bin/env bash
+echo "Flasing 1"
+python3 -m cfloader flash cf2.bin stm32-fw -w radio://0/10/2M/E7E7E7E701
+echo "Flasing 2"
+python3 -m cfloader flash cf2.bin stm32-fw -w radio://0/10/2M/E7E7E7E702
+echo "Flasing 3"
+python3 -m cfloader flash cf2.bin stm32-fw -w radio://0/10/2M/E7E7E7E703
+echo "Flasing 4"
+python3 -m cfloader flash cf2.bin stm32-fw -w radio://0/10/2M/E7E7E7E704
+echo "Flasing 5"
+python3 -m cfloader flash cf2.bin stm32-fw -w radio://0/10/2M/E7E7E7E705
+echo "Flasing 6"
+python3 -m cfloader flash cf2.bin stm32-fw -w radio://0/10/2M/E7E7E7E706
+echo "Flasing 7"
+python3 -m cfloader flash cf2.bin stm32-fw -w radio://0/10/2M/E7E7E7E707
+echo "Flasing 8"
+python3 -m cfloader flash cf2.bin stm32-fw -w radio://0/10/2M/E7E7E7E708
+echo "Flasing 9"
+python3 -m cfloader flash cf2.bin stm32-fw -w radio://0/10/2M/E7E7E7E709
+

FlyPi/crazyflie-firmware-2021.06/examples/demos/swarm_demo/control_tower/control_tower.py

+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+#
+#     ||          ____  _ __
+#  +------+      / __ )(_) /_______________ _____  ___
+#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+#  Copyright (C) 2019 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; either version 2
+#  of the License, or (at your option) any later version.
+#
+#  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, write to the Free Software
+#  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+#  MA  02110-1301, USA.
+
+import time
+import cflib.crtp  # noqa
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.log import LogConfig
+import statistics
+import sys
+import threading
+import math
+
+import zmq
+
+class TrafficController:
+    CS_DISCONNECTED = 0
+    CS_CONNECTING = 1
+    CS_CONNECTED = 2
+
+    STATE_UNKNOWN = -1
+    STATE_IDLE = 0
+    STATE_WAIT_FOR_POSITION_LOCK = 1
+    STATE_WAIT_FOR_TAKE_OFF = 2  # Charging
+    STATE_TAKING_OFF = 3
+    STATE_HOVERING = 4
+    STATE_WAITING_TO_GO_TO_INITIAL_POSITION = 5
+    STATE_GOING_TO_INITIAL_POSITION = 6
+    STATE_RUNNING_TRAJECTORY = 7
+    STATE_GOING_TO_PAD = 8
+    STATE_WAITING_AT_PAD = 9
+    STATE_LANDING = 10
+    STATE_CHECK_CHARGING = 11
+    STATE_REPOSITION_ON_PAD = 12
+    STATE_CRASHED = 13
+
+    NO_PROGRESS = -1000.0
+
+    PRE_STATE_TIMEOUT = 3
+
+    def __init__(self, uri):
+        self.uri = uri
+        self.stay_alive = True
+        self.reset_internal()
+        self.connection_thread = threading.Thread(target=self.process)
+        self.connection_thread.start()
+
+    def reset_internal(self):
+        self.connection_state = self.CS_DISCONNECTED
+        self._cf = None
+        self._log_conf = None
+        self.copter_state = self.STATE_UNKNOWN
+        self.vbat = -1.0
+        self._time_for_next_connection_attempt = 0
+        self.traj_cycles = None
+        self.est_x = 0.0
+        self.est_y = 0.0
+        self.est_z = 0.0
+        self.up_time_ms = 0
+        self.flight_time_ms = 0
+
+        # Pre states are used to prevent multiple calls to a copter
+        # when waiting for the remote state to change
+        self._pre_state_taking_off_end_time = 0
+        self._pre_state_going_to_initial_position_end_time = 0
+
+    def _pre_state_taking_off(self):
+        return self._pre_state_taking_off_end_time > time.time()
+
+    def _pre_state_going_to_initial_position(self):
+        return self._pre_state_going_to_initial_position_end_time > time.time()
+
+    def is_connected(self):
+        return self.connection_state == self.CS_CONNECTED
+
+    def has_found_position(self):
+        return self.copter_state > self.STATE_WAIT_FOR_POSITION_LOCK
+
+    def is_taking_off(self):
+        return self.copter_state == self.STATE_TAKING_OFF or self._pre_state_taking_off()
+
+    def is_ready_for_flight(self):
+        return self.copter_state == self.STATE_HOVERING and not self._pre_state_going_to_initial_position()
+
+    def is_flying(self):
+        return self.copter_state == self.STATE_RUNNING_TRAJECTORY or \
+               self.copter_state == self.STATE_WAITING_TO_GO_TO_INITIAL_POSITION or \
+               self.copter_state == self.STATE_GOING_TO_INITIAL_POSITION or \
+               self._pre_state_going_to_initial_position()
+
+    def is_landing(self):
+        return self.copter_state == self.STATE_GOING_TO_PAD or \
+               self.copter_state == self.STATE_WAITING_AT_PAD or \
+               self.copter_state == self.STATE_LANDING or \
+               self.copter_state == self.STATE_CHECK_CHARGING or \
+               self.copter_state == self.STATE_REPOSITION_ON_PAD
+
+    def is_charging(self):
+        return self.copter_state == self.STATE_WAIT_FOR_TAKE_OFF and not self._pre_state_taking_off()
+
+    def is_crashed(self):
+        return self.copter_state == self.STATE_CRASHED
+
+    def take_off(self):
+        if self.is_charging():
+            if self._cf:
+                self._pre_state_taking_off_end_time = time.time() + self.PRE_STATE_TIMEOUT
+                self._cf.param.set_value('app.takeoff', 1)
+
+    def start_trajectory(self, trajectory_delay, offset_x=0.0, offset_y=0.0, offset_z=0.0):
+        if self.is_ready_for_flight():
+            if self._cf:
+                self._cf.param.set_value('app.offsx', offset_x)
+                self._cf.param.set_value('app.offsy', offset_y)
+                self._cf.param.set_value('app.offsz', offset_z)
+
+                self._pre_state_going_to_initial_position_end_time = time.time() + self.PRE_STATE_TIMEOUT
+                self._cf.param.set_value('app.start', trajectory_delay)
+
+    def force_land(self):
+        if self.connection_state == self.CS_CONNECTED:
+            self._cf.param.set_value('app.stop', 1)
+
+    def set_trajectory_count(self, count):
+        if self.connection_state == self.CS_CONNECTED:
+            self._cf.param.set_value('app.trajcount', count)
+
+    def get_charge_level(self):
+        return self.vbat
+
+    def is_charged_for_flight(self):
+        return self.vbat > 4.10
+
+    def get_traj_cycles(self):
+        return self.traj_cycles
+
+    def process(self):
+        while self.stay_alive:
+            if self.connection_state == self.CS_DISCONNECTED:
+                if time.time() > self._time_for_next_connection_attempt:
+                    self._connect()
+
+            time.sleep(1)
+        self._cf.close_link()
+
+    def _connected(self, link_uri):
+        self.connection_state = self.CS_CONNECTED
+        print('Connected to %s' % link_uri)
+
+        self.set_trajectory_count(2)
+        self._setup_logging()
+
+    def _connection_failed(self, link_uri, msg):
+        print('Connection to %s failed: %s' % (link_uri, msg))
+        self._set_disconnected(5)
+
+    def _connection_lost(self, link_uri, msg):
+        print('Connection to %s lost: %s' % (link_uri, msg))
+
+    def _disconnected(self, link_uri):
+        print('Disconnected from %s' % link_uri)
+        self._set_disconnected()
+
+    def _set_disconnected(self, hold_back_time=5):
+        self.reset_internal()
+        self._time_for_next_connection_attempt = time.time() + hold_back_time
+
+    def _connect(self):
+        if self.connection_state != self.CS_DISCONNECTED:
+            print("Can only connect when disconnected")
+            return
+
+        self.connection_state = self.CS_CONNECTING
+
+        self._cf = Crazyflie(rw_cache='./cache')
+
+        self._cf.connected.add_callback(self._connected)
+        self._cf.disconnected.add_callback(self._disconnected)
+        self._cf.connection_failed.add_callback(self._connection_failed)
+        self._cf.connection_lost.add_callback(self._connection_lost)
+
+        print("Connecting to " + self.uri)
+        self._cf.open_link(self.uri)
+
+    def _setup_logging(self):
+        # print("Setting up logging")
+        self._log_conf = LogConfig(name='Tower', period_in_ms=100)
+        self._log_conf.add_variable('app.state', 'uint8_t')
+        self._log_conf.add_variable('app.prgr', 'float')
+        self._log_conf.add_variable('app.uptime', 'uint32_t')
+        self._log_conf.add_variable('app.flighttime', 'uint32_t')
+        self._log_conf.add_variable('pm.vbat', 'float')
+        self._log_conf.add_variable('stateEstimate.x', 'float')
+        self._log_conf.add_variable('stateEstimate.y', 'float')
+
+        self._cf.log.add_config(self._log_conf)
+        self._log_conf.data_received_cb.add_callback(self._log_data)
+        self._log_conf.start()
+
+    def _log_data(self, timestamp, data, logconf):
+        self.copter_state = data['app.state']
+
+        if self.copter_state != self.STATE_WAIT_FOR_TAKE_OFF:
+            self._pre_state_taking_off_end_time = 0
+
+        if self.copter_state != self.STATE_HOVERING:
+            self._pre_state_going_to_initial_position_end_time = 0
+
+        self.vbat = data['pm.vbat']
+
+        self.up_time_ms = data['app.uptime']
+        self.flight_time_ms = data['app.flighttime']
+
+        self.traj_cycles = data['app.prgr']
+        if self.traj_cycles <= self.NO_PROGRESS:
+            self.traj_cycles = None
+
+        self.est_x = data['stateEstimate.x']
+        self.est_y = data['stateEstimate.y']
+
+    def dump(self):
+        print("***", self.uri)
+        print("  Connection state:", self.connection_state)
+        print("  Copter state:", self.copter_state)
+        print("  Bat:", self.vbat)
+        print("  Up time:", self.up_time_ms / 1000)
+        print("  Flight time:", self.flight_time_ms / 1000)
+        print("  _pre_state_taking_off:", self._pre_state_taking_off())
+        print("  _pre_state_going_to_initial_position:", self._pre_state_going_to_initial_position())
+
+    def terminate(self):
+        self.stay_alive = False
+
+
+class TowerBase:
+    def __init__(self, uris, report_socket=None):
+        self.controllers = []
+        self._uris = uris
+        for uri in uris:
+            self.controllers.append(TrafficController(uri))
+        self.report_socket = report_socket
+
+    def connected_count(self):
+        count = 0
+        for controller in self.controllers:
+            if controller.is_connected():
+                count += 1
+        return count
+
+    def flying_count(self):
+        count = 0
+        for controller in self.controllers:
+            if controller.is_flying():
+                count += 1
+        return count
+
+    def find_best_controllers(self):
+        too_low_battery = []
+
+        charging_controllers = []
+        for controller in self.controllers:
+            if controller.is_charging():
+                charge = controller.get_charge_level()
+                if controller.is_charged_for_flight():
+                    charging_controllers.append((controller, charge))
+                else:
+                    too_low_battery.append(
+                        "{} ({:.2f}V)".format(controller.uri, charge))
+
+        if len(too_low_battery) > 0:
+            print("Ready but must charge:", too_low_battery)
+
+        charging_controllers.sort(key=lambda d: d[1], reverse=True)
+
+        return list(map(lambda d: d[0], charging_controllers))
+
+    def land_all(self):
+        for controller in self.controllers:
+            controller.force_land()
+
+    def dump_state(self):
+        print('Waiting for connections...')
+        end_time = time.time() + 40
+        while time.time() < end_time and self.connected_count() < len(
+                self._uris):
+            time.sleep(1)
+
+        print("Dumping state")
+        print()
+        for controller in self.controllers:
+            controller.dump()
+            controller.terminate()
+
+    def send_report(self):
+        if self.report_socket is None:
+            return
+
+        for i, controller in enumerate(self.controllers):
+            state = "idle"
+            if not controller.is_connected():
+                state = "disconnected"
+            elif controller.is_crashed():
+                state = "crashed"
+            elif controller.is_flying():
+                state = "flying"
+            elif controller.is_taking_off():
+                state = "hovering"
+            elif controller.is_landing():
+                state = "landing"
+            elif controller.is_charged_for_flight():
+                state = "ready"
+            elif controller.is_charging():
+                state = "charging"
+
+            try:
+                report = {
+                    'id': i,
+                    'state': state,
+                    'battery': controller.get_charge_level(),
+                    'uptime': controller.up_time_ms,
+                    'flighttime': controller.flight_time_ms,
+                }
+                self.report_socket.send_json(report, zmq.NOBLOCK)
+            except Exception:
+                pass
+
+
+class Tower(TowerBase):
+    def __init__(self, uris, report_socket=None):
+        TowerBase.__init__(self, uris, report_socket)
+
+    def fly(self, wanted):
+         # Wait for all CF to connect (to avoid race)
+        time.sleep(10)
+
+        while True:
+            # print()
+            if wanted:
+                currently_flying = self.flying_count()
+                missing = wanted - currently_flying
+                if missing > 0:
+                    print("Want", missing, "more copters")
+                    self.prepare_copters(missing)
+                    self.start_copters(missing, wanted)
+            else:
+                self.land_all()
+
+            self.send_report()
+
+            time.sleep(0.2)
+
+    def prepare_copters(self, count):
+        prepared_count = 0
+        for controller in self.controllers:
+            if controller.is_taking_off() or controller.is_ready_for_flight():
+                prepared_count += 1
+
+        missing = count - prepared_count
+        new_prepared_count = 0
+        if missing > 0:
+            print("Trying to prepare", missing, "copter(s)")
+            best_controllers = self.find_best_controllers()
+            for best_controller in best_controllers[:missing]:
+                if best_controller:
+                    print("Preparing " + best_controller.uri)
+                    new_prepared_count += 1
+                    best_controller.take_off()
+            print("Prepared", new_prepared_count, "copter(s)")
+
+    def start_copters(self, count, total):
+        unused_slot_times = self.find_unused_slot_times(total)
+        # print("Unused slot times:", unused_slot_times)
+
+        slot_index = 0
+        for controller in self.controllers:
+            if controller.is_ready_for_flight():
+                if slot_index < count and slot_index < len(unused_slot_times):
+                    trajectory_delay = 1.0 - unused_slot_times[slot_index]
+                    if trajectory_delay == 1.0:
+                        trajectory_delay = 0.0
+                    print("Starting prepared copter", controller.uri,
+                          'with a delay of', trajectory_delay)
+                    controller.start_trajectory(trajectory_delay, offset_z=0.25)
+                    slot_index += 1
+                else:
+                    return
+
+    def find_unused_slot_times(self, total_slots):
+        # Times are measured in trajectory cycles
+        start_times = []
+        for controller in self.controllers:
+            if controller.is_flying():
+                start_time = controller.get_traj_cycles()
+
+                # If a is flying but has not updated the start time yes we do
+                # not have enough information to calculate empty slots.
+                # Return no unused slots for now
+                if start_time is None:
+                    # print("Start time is unknown, hold back")
+                    return []
+
+                start_times.append(start_time)
+
+        # print("Used start times", start_times)
+        return self.crunch_slot_times(start_times, total_slots)
+
+    def crunch_slot_times(self, start_times, total_slots):
+        # Start times may be multiple cycles ago, remove integer parts
+        start_time_fractions = list(map(lambda t: t % 1.0, start_times))
+
+        # Find the average offset
+        offsets = list(
+            map(lambda t: (t * total_slots) % 1.0, start_time_fractions))
+        offset = 0.0
+        if len(start_times) > 0:
+            offset = statistics.mean(offsets) / total_slots
+
+        adjusted_start_times = list(
+            map(lambda t: t - offset, start_time_fractions))
+
+        closest_slots = list(
+            map(lambda t: round(t * total_slots), adjusted_start_times))
+        unused_slots = list(
+            filter(lambda s: s not in closest_slots, range(total_slots)))
+
+        unsued_slot_times = list(
+            map(lambda s: offset + s / total_slots, unused_slots))
+        return unsued_slot_times
+
+
+class SyncTower(TowerBase):
+    def __init__(self, uris, report_socket=None):
+        TowerBase.__init__(self, uris, report_socket)
+        self.spacing = 0.40
+        self.line_orientation = math.radians(40)
+
+        master_offset = [0, 0]
+        self._start_position = [
+            [   0 + master_offset[0],    0 + master_offset[1], 0],
+            [   0 + master_offset[0],  0.5 + master_offset[1], 0],
+            [   0 + master_offset[0], -0.5 + master_offset[1], 0],
+            [ 0.5 + master_offset[0],    0 + master_offset[1], 0],
+            [-0.5 + master_offset[0],    0 + master_offset[1], 0],
+            [ 0.5 + master_offset[0],  0.5 + master_offset[1], 0],
+            [-0.5 + master_offset[0], -0.5 + master_offset[1], 0],
+            [-0.5 + master_offset[0],  0.5 + master_offset[1], 0],
+            [ 0.5 + master_offset[0], -0.5 + master_offset[1], 0]
+        ]
+
+
+    def fly(self, wanted):
+        while True:
+            if wanted:
+                best = self.find_best_controllers()
+                ready = list(
+                    filter(lambda ctrlr: ctrlr.has_found_position(), best))
+                found_count = len(ready)
+
+                if found_count >= wanted:
+                    self.start_line(wanted, ready)
+                    print("Started, I'm done")
+                    sys.exit(0)
+                else:
+                    print('Can only find ', found_count,
+                          'copter(s) that are charged and ready')
+            else:
+                self.land_all()
+
+            self.send_report()
+
+            time.sleep(0.2)
+
+    def start_line(self, wanted, best):
+        self.prepare_copters(wanted, best)
+
+        while not self.start_copters(wanted, best):
+            time.sleep(1)
+
+    def prepare_copters(self, count, best_controllers):
+        prepared_count = 0
+        for controller in self.controllers:
+            if controller.is_taking_off() or controller.is_ready_for_flight():
+                prepared_count += 1
+
+        missing = count - prepared_count
+        new_prepared_count = 0
+        if missing > 0:
+            print("Trying to prepare", missing, "copter(s)")
+            for best_controller in best_controllers[:missing]:
+                if best_controller:
+                    print("Preparing " + best_controller.uri)
+                    new_prepared_count += 1
+                    best_controller.take_off()
+            print("Prepared", new_prepared_count, "copter(s)")
+
+    def start_copters(self, wanted, best):
+        ready = []
+        for controller in best:
+            if controller.is_ready_for_flight():
+                ready.append(controller)
+
+        if len(ready) >= wanted:
+            ready_positions = []
+            for controller in ready:
+                ready_positions.append([controller.est_x, controller.est_y, controller.est_z])
+
+            offsets = self.get_start_offsets(ready_positions, self._start_position[:wanted])
+
+            index = 0
+            for controller in ready:
+                offset_x = offsets[index][0]
+                offset_y = offsets[index][1]
+                offset_z = offsets[index][2]
+                controller.start_trajectory(0.0, offset_x=offset_x,
+                                            offset_y=offset_y,
+                                            offset_z=offset_z)
+                index += 1
+
+            return True
+        else:
+            return False
+
+    def calculate_distance(self, p1, p2):
+        diff = [0,0,0]
+        diff[0] = p1[0]-p2[0]
+        diff[1] = p1[1]-p2[1]
+        diff[2] = p1[2]-p2[2]
+        return math.sqrt( (diff[0]*diff[0]) + (diff[1]*diff[1]) + (diff[2]*diff[2]) )
+
+    def find_closest_target(self, start_position, targets_position):
+        min_distance = None
+        closest_index = 0
+        for index, target in enumerate(targets_position):
+            dist = self.calculate_distance(start_position, target)
+            if min_distance is None or dist < min_distance:
+                min_distance = dist
+                closest_index = index
+        
+        return targets_position[closest_index]
+
+    def get_start_offsets(self, start_positions, targets_positions):
+        offsets = []
+        target_used = [False,] * len(targets_positions)
+        for start in start_positions:
+            candidate_targets = []
+            for i in range(len(targets_positions)):
+                if not target_used[i]:
+                    candidate_targets.append(targets_positions[i])
+            
+            closest_target = self.find_closest_target(start, candidate_targets)
+            target_used[targets_positions.index(closest_target)] = True
+            offsets.append(closest_target)
+        
+        return offsets
+
+
+uris = [
+    'radio://0/10/2M/E7E7E7E701',
+    'radio://0/10/2M/E7E7E7E702',
+    'radio://0/10/2M/E7E7E7E703',
+    'radio://0/10/2M/E7E7E7E704',
+    'radio://0/10/2M/E7E7E7E705',
+    'radio://0/10/2M/E7E7E7E706',
+    'radio://0/10/2M/E7E7E7E707',
+    'radio://0/10/2M/E7E7E7E708',
+    'radio://0/10/2M/E7E7E7E709'
+]
+
+count = 1
+mode = 'normal'
+
+if len(sys.argv) > 1:
+    if sys.argv[1] == 'd':
+        mode = 'dump'
+    else:
+        count = int(sys.argv[1])
+
+if len(sys.argv) > 2:
+    if sys.argv[2] == 's':
+        mode = 'synch'
+
+context = zmq.Context()
+socket = context.socket(zmq.PUSH)
+socket.bind("tcp://*:5555")
+
+cflib.crtp.init_drivers(enable_debug_driver=False)
+
+print('Starting tower with', count, 'Crazyflie(s)')
+if mode == 'synch':
+    print('Flying with synchronized trajectories')
+    tower = SyncTower(uris, socket)
+else:
+    print('Flying with interleaved trajectories')
+    tower = Tower(uris, socket)
+
+if not mode == 'dump':
+    tower.fly(count)
+else:
+    tower.dump_state()

FlyPi/crazyflie-firmware-2021.06/examples/demos/swarm_demo/control_tower/towergui.py

+#!/usr/bin/env python3
+import tkinter
+from tkinter import *
+import tkinter.ttk as ttk
+import zmq
+import threading
+import time
+
+
+class Crazyflie(ttk.Frame):
+    def __init__(self, parent, ident):
+        ttk.Frame.__init__(self, parent)
+
+        self['padding'] = 10
+
+        self._name = Label(self, text="Crazyflie #{}".format(ident+1))
+        self._name.grid(row=0, column=0)
+
+        self._status = Label(self, text="IDLE", fg='grey', font=("ubuntu", 33), width=10)
+        self._status.grid(row=1, column=0)
+
+        self._battery_label = Label(self, text="Battery:")
+        self._battery_label.grid(row=2, column=0)
+
+        self._battery_frame = ttk.Frame(self)
+        self._battery_frame.grid(row=3, column=0, sticky="ew")
+        self._battery_frame.columnconfigure(1, weight=2)
+
+        self._battery_voltage = ttk.Label(self._battery_frame, text="3.0V", padding=(0,0,10,0))
+        self._battery_voltage.grid(row=0, column=0)
+        
+        self._battery_bar = ttk.Progressbar(self._battery_frame, orient=HORIZONTAL)
+        self._battery_bar['value'] = 50
+        self._battery_bar.grid(row=0, column=1, sticky="ew")
+
+        self._time_frame = ttk.Frame(self)
+        self._time_frame.grid(row=4, column=0)
+
+        up_time = Label(self._time_frame, text="Up time: ", fg="grey", font=("ubuntu", 20))
+        up_time.grid(row=0, column=0)
+
+        self._up_time_label = Label(self._time_frame, text="0:00:00", font=("ubuntu", 20))
+        self._up_time_label.grid(row=0, column=1)
+
+        flight_time = Label(self._time_frame, text="Flight time: ", fg="grey", font=("ubuntu", 20))
+        flight_time.grid(row=1, column=0)
+
+        self._flight_time_label = Label(self._time_frame, text="0:00:00", font=("ubuntu", 20))
+        self._flight_time_label.grid(row=1, column=1)
+
+
+    
+    def set_state(self, state):
+        if state == "idle":
+            self._status.config(text="IDLE", fg="grey")
+        elif state == "disconnected":
+            self._status.config(text="Offline", fg="grey")
+        elif state == "crashed":
+            self._status.config(text="Crashed", fg="purple")
+        elif state == "charging":
+            self._status.config(text="Charging", fg="red")
+        elif state == "ready":
+            self._status.config(text="Ready", fg="orange")
+        elif state == "flying":
+            self._status.config(text="Flying", fg="green")
+        elif state == "hovering":
+            self._status.config(text="Take off", fg="green")
+        elif state == "landing":
+            self._status.config(text="Landing", fg="green")
+        else:
+            self._status.config(text="ERROR", fg="purple")
+            print("Error, state", state, "not handled")
+
+    def set_battery(self, voltage):
+        self._battery_voltage['text'] = "{:.2f}V".format(voltage)
+
+        percent = (voltage - 3.0)*100.0/1.1
+
+        self._battery_bar['value'] = percent
+    
+    def set_uptime(self, ms):
+        seconds = int(ms/1000) % 60
+        minutes = int((ms/1000)/60) % 60
+        hours = int((ms/1000)/3600)
+        self._up_time_label['text'] = "{}:{:02}:{:02}".format(hours, minutes, seconds)
+        if ms == 0:
+            self._up_time_label['fg'] = "grey"
+        else:
+            self._up_time_label['fg'] = "black"
+    
+    def set_flighttime(self, ms):
+        seconds = int(ms/1000) % 60
+        minutes = int((ms/1000)/60) % 60
+        hours = int((ms/1000)/3600)
+        self._flight_time_label['text'] = "{}:{:02}:{:02}".format(hours, minutes, seconds)
+        if ms == 0:
+            self._flight_time_label['fg'] = "grey"
+        else:
+            self._flight_time_label['fg'] = "black"
+
+
+root = tkinter.Tk()
+root.title("ICRA2019 Bitcraze Control Tower")
+
+content = ttk.Frame(root)
+content.grid(column=0, row=0)
+root.columnconfigure(0, weight=1)
+root.rowconfigure(0, weight=1)
+
+cfs = []
+
+for i in range(9):
+    r = int(i/3)
+    c = int(i%3)
+    cf = Crazyflie(content, i)
+    cf.grid(column=c, row=r)
+    cfs.append(cf)
+
+    cf.set_state("error")
+    cf.set_battery(3.0+(i/10.0))
+
+context = zmq.Context()
+
+socket = context.socket(zmq.PULL)
+# socket.connect("tcp://bitcrazeDemo:5555")
+socket.connect("tcp://127.0.0.1:5555")
+socket.setsockopt(zmq.RCVTIMEO, 1000)
+
+
+def receive_thread():
+    last_updated = [0]*len(cfs)
+    while True:
+        try:
+            report = socket.recv_json()
+            print(report)
+
+            cfs[report['id']].set_battery(report['battery'])
+            cfs[report['id']].set_state(report['state'])
+            cfs[report['id']].set_uptime(report['uptime'])
+            cfs[report['id']].set_flighttime(report['flighttime'])
+            last_updated[report['id']] = time.time()
+        except zmq.error.Again:
+            pass
+
+        for i in range(len(cfs)):
+            if last_updated[i] < (time.time()-1):
+                cfs[i].set_state("idle")
+                cfs[i].set_battery(0)
+                cfs[i].set_uptime(0)
+                cfs[i].set_flighttime(0)
+
+
+cfs[0].set_uptime(300000)
+cfs[0].set_flighttime(140000)
+
+threading.Thread(target=receive_thread, daemon=True).start()
+
+root.mainloop()

FlyPi/crazyflie-firmware-2021.06/examples/demos/swarm_demo/visualization-live.py

+from vispy import scene
+from vispy.scene import XYZAxis, LinePlot, Node, Mesh, TurntableCamera, Markers
+import numpy as np
+from vispy.visuals.transforms import MatrixTransform
+import math
+import threading
+import time
+import logging
+import cflib.crtp
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.log import LogConfig
+from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
+from cflib.crazyflie.syncLogger import SyncLogger
+from cflib.crazyflie.mem import MemoryElement
+
+logging.basicConfig(level=logging.ERROR)
+
+
+class AnimCanvas(scene.SceneCanvas):
+    def __init__(self):
+        scene.SceneCanvas.__init__(self, keys='interactive', size=(800, 600),
+                                   show=True)
+
+
+class Visulizer:
+    def __init__(self):
+        self.terminate = False
+
+        self.canvas = AnimCanvas()
+
+        self.view = self.canvas.central_widget.add_view()
+        self.view.bgcolor = '#ffffff'
+        self.view.camera = TurntableCamera(fov=10.0, distance=40.0, up='+z',
+                                           center=(0.0, 1.0, 0.0))
+
+        self.scene = self.view.scene
+
+        XYZAxis(parent=self.scene)
+
+        self.base_stations = [
+            {
+                'origin': [-1.9584829807281494, 0.5422989726066589, 3.152726888656616],
+                'mat': [
+                    [0.7972149848937988, -0.004273999948054552, 0.6036810278892517],
+                    [0.0, 0.9999750256538391, 0.007079999893903732],
+                    [-0.6036959886550903, -0.005644999910145998, 0.7971950173377991]]
+            },
+            {
+                'origin': [1.0623979568481445, -2.563488006591797, 3.1123669147491455],
+                'mat': [
+                    [0.018067000433802605, -0.9993360042572021, 0.03164700046181679],
+                    [0.7612509727478027, 0.034269001334905624, 0.6475520133972168],
+                    [-0.6482059955596924, 0.012392000295221806, 0.7613639831542969]]
+            },
+        ]
+
+        self.angles = [
+            [0.19170784950256348, 0.01734159141778946],
+            [-0.12198804318904877, -0.3505938947200775],
+        ]
+
+        self.objs = [{}, {}]
+        self.generate_scene()
+
+    def generate_scene(self):
+        p = [0, 0, 0]
+
+        for i in range(2):
+            objs = self.objs[i]
+
+            if i == 0:
+                objs['bs'] = self.marker(p, color='green')
+            else:
+                objs['bs'] = self.marker(p, color='red')
+
+            objs['tripod'] = self.line(p, p, color='blue')
+            objs['center_line'] = self.line(p, p, color="green")
+            objs['beam'] = self.line(p, p, color="red")
+
+    def update_scene(self):
+        normal = [1, 0, 0]
+        for i in range(2):
+            objs = self.objs[i]
+
+            bs = self.base_stations[i]
+            origin = bs['origin']
+            self.update_marker(objs['bs'], origin)
+            self.update_line(objs['tripod'], [origin[0], origin[1], 0], origin)
+
+            rot_bs = np.array(bs['mat'])
+            center_line = np.dot(rot_bs, normal)
+            self.update_line(objs['center_line'], origin,
+                             np.add(center_line, origin))
+
+            ################
+
+            a = self.angles[i]
+
+            a_x = a[0]
+            a_y = a[1]
+
+            beam_direction = np.array([1, math.tan(a_x), math.tan(a_y)])
+            beam_line_local = 4 * beam_direction / np.linalg.norm(beam_direction)
+            beam_line = np.dot(rot_bs, beam_line_local)
+
+            self.update_line(objs['beam'], origin, np.add(beam_line, origin))
+
+    def marker(self, pos, color='black'):
+        return Markers(pos=np.array(pos, ndmin=2), face_color=color,
+                       parent=self.scene)
+
+    def update_marker(self, marker, pos):
+        marker.set_data(np.array(pos, ndmin=2))
+
+    def line(self, p1, p2, color='black'):
+        return LinePlot([p1, p2], color=color, parent=self.scene,
+                        marker_size=0)
+
+    def update_line(self, line, p1, p2):
+        line.set_data(data=[p1, p2], marker_size=0)
+
+    def run(self):
+        print('starting CF thread')
+        self.t = threading.Thread(target=self.cf_handler)
+        self.t.start()
+
+        self.canvas.app.run()
+        self.terminate = True
+
+    def cf_handler(self):
+        uri = "radio://0/30"
+
+        lg_block = LogConfig(name='LH', period_in_ms=50)
+        lg_block.add_variable('lighthouse.angle0x', 'float')
+        lg_block.add_variable('lighthouse.angle0y', 'float')
+        lg_block.add_variable('lighthouse.angle1x', 'float')
+        lg_block.add_variable('lighthouse.angle1y', 'float')
+
+        cf = Crazyflie(rw_cache='./cache')
+        with SyncCrazyflie(uri, cf=cf) as scf:
+            print("Getting LH geometry")
+            mems = scf.cf.mem.get_mems(MemoryElement.TYPE_LH)
+            mems[0].update(self._update_geometry)
+
+            with SyncLogger(scf, lg_block) as logger:
+                for log_entry in logger:
+                    data = log_entry[1]
+
+                    self.angles[0][0] = data['lighthouse.angle0x']
+                    self.angles[0][1] = data['lighthouse.angle0y']
+                    self.angles[1][0] = data['lighthouse.angle1x']
+                    self.angles[1][1] = data['lighthouse.angle1y']
+
+                    if self.terminate:
+                        break
+
+                    self.update_scene()
+
+    def _update_geometry(self, mem):
+        print("Received LH geometry")
+        for i in range(2):
+            self.base_stations[i]['origin'] = mem.geometry_data[i].origin
+            self.base_stations[i]['mat'] = mem.geometry_data[i].rotation_matrix
+            print(self.base_stations[i])
+
+        self.update_scene()
+
+
+# Initialize the low-level drivers (don't list the debug drivers)
+cflib.crtp.init_drivers(enable_debug_driver=False)
+
+viz = Visulizer()
+viz.run()

FlyPi/crazyflie-firmware-2021.06/module.json

+{
+  "version": "2.0",
+  "environmentReqs": {
+    "build": ["arm-none-eabi"],
+    "build-docs": ["doxygen"]
+  },
+  "artifacts": {
+    "file": [
+      {
+        "source": "*.bin",
+        "insertTag": true,
+        "destination": "currDir"
+      },
+      {
+        "source": "*.dfu",
+        "insertTag": true,
+        "destination": "currDir"
+      }
+    ]
+  },
+  "targets": {
+    "cf2": [
+      "PLATFORM=cf2"
+    ],
+    "tag": [
+      "PLATFORM=tag"
+    ]
+  }
+}

FlyPi/crazyflie-firmware-2021.06/src/config/FreeRTOSConfig.h

+/*
+    FreeRTOS V6.0.0 - Copyright (C) 2009 Real Time Engineers Ltd.
+
+    ***************************************************************************
+    *                                                                         *
+    * If you are:                                                             *
+    *                                                                         *
+    *    + New to FreeRTOS,                                                   *
+    *    + Wanting to learn FreeRTOS or multitasking in general quickly       *
+    *    + Looking for basic training,                                        *
+    *    + Wanting to improve your FreeRTOS skills and productivity           *
+    *                                                                         *
+    * then take a look at the FreeRTOS eBook                                  *
+    *                                                                         *
+    *        "Using the FreeRTOS Real Time Kernel - a Practical Guide"        *
+    *                  http://www.FreeRTOS.org/Documentation                  *
+    *                                                                         *
+    * A pdf reference manual is also available.  Both are usually delivered   *
+    * to your inbox within 20 minutes to two hours when purchased between 8am *
+    * and 8pm GMT (although please allow up to 24 hours in case of            *
+    * exceptional circumstances).  Thank you for your support!                *
+    *                                                                         *
+    ***************************************************************************
+
+    This file is part of the FreeRTOS distribution.
+
+    FreeRTOS is free software; you can redistribute it and/or modify it under
+    the terms of the GNU General Public License (version 2) as published by the
+    Free Software Foundation AND MODIFIED BY the FreeRTOS exception.
+    ***NOTE*** The exception to the GPL is included to allow you to distribute
+    a combined work that includes FreeRTOS without being obliged to provide the
+    source code for proprietary components outside of the FreeRTOS kernel.
+    FreeRTOS 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 and the FreeRTOS license exception along with FreeRTOS; if not it
+    can be viewed here: http://www.freertos.org/a00114.html and also obtained
+    by writing to Richard Barry, contact details for whom are available on the
+    FreeRTOS WEB site.
+
+    1 tab == 4 spaces!
+
+    http://www.FreeRTOS.org - Documentation, latest information, license and
+    contact details.
+
+    http://www.SafeRTOS.com - A version that is certified for use in safety
+    critical systems.
+
+    http://www.OpenRTOS.com - Commercial support, development, porting,
+    licensing and training services.
+*/
+
+#ifndef FREERTOS_CONFIG_H
+#define FREERTOS_CONFIG_H
+
+#include <stdint.h>
+
+#include "config.h"
+#include "cfassert.h"
+/*-----------------------------------------------------------
+ * Application specific definitions.
+ *
+ * These definitions should be adjusted for your particular hardware and
+ * application requirements.
+ *
+ * THESE PARAMETERS ARE DESCRIBED WITHIN THE 'CONFIGURATION' SECTION OF THE
+ * FreeRTOS API DOCUMENTATION AVAILABLE ON THE FreeRTOS.org WEB SITE.
+ *
+ * See http://www.freertos.org/a00110.html.
+ *----------------------------------------------------------*/
+
+#define configUSE_PREEMPTION		1
+#define configUSE_IDLE_HOOK			1
+#define configUSE_TICK_HOOK			0
+#define configCPU_CLOCK_HZ			( ( unsigned long ) FREERTOS_MCU_CLOCK_HZ )
+#define configTICK_RATE_HZ_RAW  1000
+#define configTICK_RATE_HZ			( ( portTickType ) configTICK_RATE_HZ_RAW )
+#define configMINIMAL_STACK_SIZE	( ( unsigned short ) FREERTOS_MIN_STACK_SIZE )
+#define configTOTAL_HEAP_SIZE		( ( size_t ) ( FREERTOS_HEAP_SIZE ) )
+#define configMAX_TASK_NAME_LEN		( 10 )
+#define configUSE_16_BIT_TICKS		0
+#define configIDLE_SHOULD_YIELD		0
+#define configUSE_CO_ROUTINES 		0
+#ifdef DEBUG
+  #define configCHECK_FOR_STACK_OVERFLOW      1
+#else
+  #define configCHECK_FOR_STACK_OVERFLOW      0
+#endif
+#define configUSE_TIMERS          1
+#define configTIMER_TASK_PRIORITY 1
+#define configTIMER_QUEUE_LENGTH  20
+#define configUSE_MALLOC_FAILED_HOOK 1
+#define configTIMER_TASK_STACK_DEPTH (configMINIMAL_STACK_SIZE * 4)
+
+#define configMAX_PRIORITIES		( 6 )
+#define configMAX_CO_ROUTINE_PRIORITIES ( 2 )
+
+/* Set the following definitions to 1 to include the API function, or zero
+to exclude the API function. */
+
+#define INCLUDE_vTaskPrioritySet		1
+#define INCLUDE_uxTaskPriorityGet		1
+#define INCLUDE_vTaskDelete				1
+#define INCLUDE_vTaskCleanUpResources	1
+#define INCLUDE_vTaskSuspend			1
+#define INCLUDE_vTaskDelayUntil			1
+#define INCLUDE_vTaskDelay				1
+#define INCLUDE_uxTaskGetStackHighWaterMark 1
+#define INCLUDE_xTaskGetIdleTaskHandle 1
+
+#define configUSE_MUTEXES 1
+
+#define configKERNEL_INTERRUPT_PRIORITY     255
+//#define configMAX_SYSCALL_INTERRUPT_PRIORITY 1
+#define configMAX_SYSCALL_INTERRUPT_PRIORITY 0x5F /* equivalent to 0x05, or priority 5. */
+
+//Map the port handler to the crt0 interruptions handlers
+#define xPortPendSVHandler PendSV_Handler
+#define xPortSysTickHandler tickFreeRTOS
+#define vPortSVCHandler SVC_Handler
+
+//Milliseconds to OS Ticks
+#if configTICK_RATE_HZ_RAW != 1000
+  #error "Please review the use of M2T and T2M if there is not a 1 to 1 mapping between ticks and milliseconds"
+#endif
+#define M2T(X) ((unsigned int)(X))
+#define F2T(X) ((unsigned int)((configTICK_RATE_HZ/(X))))
+#define T2M(X) ((unsigned int)(X))
+
+// Seconds to OS ticks
+#define S2T(X) ((portTickType)((X) * configTICK_RATE_HZ))
+#define T2S(X) ((X) / (float)configTICK_RATE_HZ)
+
+
+// DEBUG SECTION
+#define configUSE_APPLICATION_TASK_TAG  1
+#define configQUEUE_REGISTRY_SIZE       10
+
+#ifdef DEBUG
+#define configRECORD_STACK_HIGH_ADDRESS 1
+#endif
+
+#define TASK_LED_ID_NBR         1
+#define TASK_RADIO_ID_NBR       2
+#define TASK_STABILIZER_ID_NBR  3
+#define TASK_ADC_ID_NBR         4
+#define TASK_PM_ID_NBR          5
+#define TASK_PROXIMITY_ID_NBR   6
+
+#define configASSERT( x )  if( ( x ) == 0 ) assertFail(#x, __FILE__, __LINE__ )
+
+/*
+#define traceTASK_SWITCHED_IN() \
+  { \
+    extern void debugSendTraceInfo(unsigned int taskNbr); \
+    debugSendTraceInfo((int)pxCurrentTCB->pxTaskTag); \
+  }
+*/
+
+#define configSUPPORT_STATIC_ALLOCATION 1
+
+// Queue monitoring
+#ifdef DEBUG_QUEUE_MONITOR
+    #undef traceQUEUE_SEND
+    #undef traceQUEUE_SEND_FAILED
+    #define traceQUEUE_SEND(xQueue) qm_traceQUEUE_SEND(xQueue)
+    void qm_traceQUEUE_SEND(void* xQueue);
+    #define traceQUEUE_SEND_FAILED(xQueue) qm_traceQUEUE_SEND_FAILED(xQueue)
+    void qm_traceQUEUE_SEND_FAILED(void* xQueue);
+#endif // DEBUG_QUEUE_MONITOR
+
+#endif /* FREERTOS_CONFIG_H */

FlyPi/crazyflie-firmware-2021.06/src/config/config.h

+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 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/>.
+ *
+ * config.h - Main configuration file
+ *
+ * This file define the default configuration of the copter
+ * It contains two types of parameters:
+ * - The global parameters are globally defined and independent of any
+ *   compilation profile. An example of such define could be some pinout.
+ * - The profiled defines, they are parameter that can be specific to each
+ *   dev build. The vanilla build is intended to be a "customer" build without
+ *   fancy spinning debugging stuff. The developers build are anything the
+ *   developer could need to debug and run his code/crazy stuff.
+ *
+ * The golden rule for the profile is NEVER BREAK ANOTHER PROFILE. When adding a
+ * new parameter, one shall take care to modified everything necessary to
+ * preserve the behavior of the other profiles.
+ *
+ * For the flag. T_ means task. H_ means HAL module. U_ would means utils.
+ */
+
+#ifndef CONFIG_H_
+#define CONFIG_H_
+#include "nrf24l01.h"
+
+#include "trace.h"
+#include "usec_time.h"
+
+#define PROTOCOL_VERSION 4
+
+#ifdef STM32F4XX
+  #define QUAD_FORMATION_X
+
+  #define CONFIG_BLOCK_ADDRESS    (2048 * (64-1))
+  #define MCU_ID_ADDRESS          0x1FFF7A10
+  #define MCU_FLASH_SIZE_ADDRESS  0x1FFF7A22
+  #ifndef FREERTOS_HEAP_SIZE
+    #define FREERTOS_HEAP_SIZE      30000
+  #endif
+  #define FREERTOS_MIN_STACK_SIZE 150       // M4-FPU register setup is bigger so stack needs to be bigger
+  #define FREERTOS_MCU_CLOCK_HZ   168000000
+
+  #define configGENERATE_RUN_TIME_STATS 1
+  #define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS() initUsecTimer()
+  #define portGET_RUN_TIME_COUNTER_VALUE() usecTimestamp()
+#endif
+
+
+// Task priorities. Higher number higher priority
+#define STABILIZER_TASK_PRI     5
+#define SENSORS_TASK_PRI        4
+#define ADC_TASK_PRI            3
+#define FLOW_TASK_PRI           3
+#define MULTIRANGER_TASK_PRI    3
+#define SYSTEM_TASK_PRI         2
+#define CRTP_TX_TASK_PRI        2
+#define CRTP_RX_TASK_PRI        2
+#define EXTRX_TASK_PRI          2
+#define ZRANGER_TASK_PRI        2
+#define ZRANGER2_TASK_PRI       2
+#define LOG_TASK_PRI            1
+#define MEM_TASK_PRI            1
+#define PARAM_TASK_PRI          1
+#define PROXIMITY_TASK_PRI      0
+#define PM_TASK_PRI             0
+#define USDLOG_TASK_PRI         1
+#define USDWRITE_TASK_PRI       0
+#define PCA9685_TASK_PRI        2
+#define CMD_HIGH_LEVEL_TASK_PRI 2
+#define BQ_OSD_TASK_PRI         1
+#define GTGPS_DECK_TASK_PRI     1
+#define LIGHTHOUSE_TASK_PRI     3
+#define LPS_DECK_TASK_PRI       3
+#define OA_DECK_TASK_PRI        3
+#define UART1_TEST_TASK_PRI     1
+#define UART2_TEST_TASK_PRI     1
+#define KALMAN_TASK_PRI         2
+#define LEDSEQCMD_TASK_PRI      1
+
+#define SYSLINK_TASK_PRI        3
+#define USBLINK_TASK_PRI        3
+#define ACTIVE_MARKER_TASK_PRI  3
+#define AI_DECK_TASK_PRI        3
+#define UART2_TASK_PRI          3
+#define CRTP_SRV_TASK_PRI       0
+#define PLATFORM_SRV_TASK_PRI   0
+
+// Not compiled
+#if 0
+  #define INFO_TASK_PRI           2
+  #define PID_CTRL_TASK_PRI       2
+#endif
+
+
+// Task names
+#define SYSTEM_TASK_NAME        "SYSTEM"
+#define LEDSEQCMD_TASK_NAME     "LEDSEQCMD"
+#define ADC_TASK_NAME           "ADC"
+#define PM_TASK_NAME            "PWRMGNT"
+#define CRTP_TX_TASK_NAME       "CRTP-TX"
+#define CRTP_RX_TASK_NAME       "CRTP-RX"
+#define CRTP_RXTX_TASK_NAME     "CRTP-RXTX"
+#define LOG_TASK_NAME           "LOG"
+#define MEM_TASK_NAME           "MEM"
+#define PARAM_TASK_NAME         "PARAM"
+#define SENSORS_TASK_NAME       "SENSORS"
+#define STABILIZER_TASK_NAME    "STABILIZER"
+#define NRF24LINK_TASK_NAME     "NRF24LINK"
+#define ESKYLINK_TASK_NAME      "ESKYLINK"
+#define SYSLINK_TASK_NAME       "SYSLINK"
+#define USBLINK_TASK_NAME       "USBLINK"
+#define PROXIMITY_TASK_NAME     "PROXIMITY"
+#define EXTRX_TASK_NAME         "EXTRX"
+#define UART_RX_TASK_NAME       "UART"
+#define ZRANGER_TASK_NAME       "ZRANGER"
+#define ZRANGER2_TASK_NAME      "ZRANGER2"
+#define FLOW_TASK_NAME          "FLOW"
+#define USDLOG_TASK_NAME        "USDLOG"
+#define USDWRITE_TASK_NAME      "USDWRITE"
+#define PCA9685_TASK_NAME       "PCA9685"
+#define CMD_HIGH_LEVEL_TASK_NAME "CMDHL"
+#define MULTIRANGER_TASK_NAME   "MR"
+#define BQ_OSD_TASK_NAME        "BQ_OSDTASK"
+#define GTGPS_DECK_TASK_NAME    "GTGPS"
+#define LIGHTHOUSE_TASK_NAME    "LH"
+#define LPS_DECK_TASK_NAME      "LPS"
+#define OA_DECK_TASK_NAME       "OA"
+#define UART1_TEST_TASK_NAME    "UART1TEST"
+#define UART2_TEST_TASK_NAME    "UART2TEST"
+#define KALMAN_TASK_NAME        "KALMAN"
+#define ACTIVE_MARKER_TASK_NAME "ACTIVEMARKER-DECK"
+#define AI_DECK_GAP_TASK_NAME   "AI-DECK-GAP"
+#define AI_DECK_NINA_TASK_NAME  "AI-DECK-NINA"
+#define UART2_TASK_NAME         "UART2"
+#define CRTP_SRV_TASK_NAME      "CRTP-SRV"
+#define PLATFORM_SRV_TASK_NAME  "PLATFORM-SRV"
+
+//Task stack sizes
+#define SYSTEM_TASK_STACKSIZE         (2* configMINIMAL_STACK_SIZE)
+#define LEDSEQCMD_TASK_STACKSIZE      configMINIMAL_STACK_SIZE
+#define ADC_TASK_STACKSIZE            configMINIMAL_STACK_SIZE
+#define PM_TASK_STACKSIZE             configMINIMAL_STACK_SIZE
+#define CRTP_TX_TASK_STACKSIZE        configMINIMAL_STACK_SIZE
+#define CRTP_RX_TASK_STACKSIZE        (2* configMINIMAL_STACK_SIZE)
+#define CRTP_RXTX_TASK_STACKSIZE      configMINIMAL_STACK_SIZE
+#define LOG_TASK_STACKSIZE            (2 * configMINIMAL_STACK_SIZE)
+#define MEM_TASK_STACKSIZE            (2 * configMINIMAL_STACK_SIZE)
+#define PARAM_TASK_STACKSIZE          configMINIMAL_STACK_SIZE
+#define SENSORS_TASK_STACKSIZE        (2 * configMINIMAL_STACK_SIZE)
+#define STABILIZER_TASK_STACKSIZE     (3 * configMINIMAL_STACK_SIZE)
+#define NRF24LINK_TASK_STACKSIZE      configMINIMAL_STACK_SIZE
+#define ESKYLINK_TASK_STACKSIZE       configMINIMAL_STACK_SIZE
+#define SYSLINK_TASK_STACKSIZE        (2 * configMINIMAL_STACK_SIZE)
+#define USBLINK_TASK_STACKSIZE        configMINIMAL_STACK_SIZE
+#define PROXIMITY_TASK_STACKSIZE      configMINIMAL_STACK_SIZE
+#define EXTRX_TASK_STACKSIZE          configMINIMAL_STACK_SIZE
+#define UART_RX_TASK_STACKSIZE        configMINIMAL_STACK_SIZE
+#define ZRANGER_TASK_STACKSIZE        (2 * configMINIMAL_STACK_SIZE)
+#define ZRANGER2_TASK_STACKSIZE       (2 * configMINIMAL_STACK_SIZE)
+#define FLOW_TASK_STACKSIZE           (2 * configMINIMAL_STACK_SIZE)
+#define USDLOG_TASK_STACKSIZE         (2 * configMINIMAL_STACK_SIZE)
+#define USDWRITE_TASK_STACKSIZE       (3 * configMINIMAL_STACK_SIZE)
+#define PCA9685_TASK_STACKSIZE        (2 * configMINIMAL_STACK_SIZE)
+#define CMD_HIGH_LEVEL_TASK_STACKSIZE (2 * configMINIMAL_STACK_SIZE)
+#define MULTIRANGER_TASK_STACKSIZE    (2 * configMINIMAL_STACK_SIZE)
+#define ACTIVEMARKER_TASK_STACKSIZE   configMINIMAL_STACK_SIZE
+#define AI_DECK_TASK_STACKSIZE        configMINIMAL_STACK_SIZE
+#define UART2_TASK_STACKSIZE          configMINIMAL_STACK_SIZE
+#define CRTP_SRV_TASK_STACKSIZE       configMINIMAL_STACK_SIZE
+#define PLATFORM_SRV_TASK_STACKSIZE   configMINIMAL_STACK_SIZE
+
+//The radio channel. From 0 to 125
+#define RADIO_CHANNEL 80
+#define RADIO_DATARATE RADIO_RATE_2M
+#define RADIO_ADDRESS 0xE7E7E7E7E7ULL
+
+/**
+ * \def PROPELLER_BALANCE_TEST_THRESHOLD
+ * This is the threshold for a propeller/motor to pass. It calculates the variance of the accelerometer X+Y
+ * when the propeller is spinning.
+ */
+#define PROPELLER_BALANCE_TEST_THRESHOLD  2.5f
+
+/**
+ * \def BAT_LOADING_SAG_THESHOLD
+ * This is the threshold for a battery and connector to pass. It loads the power path by spinning all 4 motors
+ * and measure the voltage sag. The threshold is very experimental and dependent on stock configuration. It is
+ * fairly constant over the battery voltage range but testing with fully changed battery is best.
+ */
+#define BAT_LOADING_SAG_THRESHOLD  0.95f
+
+/**
+ * \def ACTIVATE_AUTO_SHUTDOWN
+ * Will automatically shot of system if no radio activity
+ */
+//#define ACTIVATE_AUTO_SHUTDOWN
+
+/**
+ * \def ACTIVATE_STARTUP_SOUND
+ * Playes a startup melody using the motors and PWM modulation
+ */
+#define ACTIVATE_STARTUP_SOUND
+
+// Define to force initialization of expansion board drivers. For test-rig and programming.
+//#define FORCE_EXP_DETECT
+
+/**
+ * \def PRINT_OS_DEBUG_INFO
+ * Print with an interval information about freertos mem/stack usage to console.
+ */
+//#define PRINT_OS_DEBUG_INFO
+
+
+//Debug defines
+//#define BRUSHLESS_MOTORCONTROLLER
+//#define ADC_OUTPUT_RAW_DATA
+//#define UART_OUTPUT_TRACE_DATA
+//#define UART_OUTPUT_RAW_DATA_ONLY
+//#define IMU_OUTPUT_RAW_DATA_ON_UART
+//#define T_LAUCH_MOTORS
+//#define T_LAUCH_MOTOR_TEST
+//#define MOTOR_RAMPUP_TEST
+/**
+ * \def ADC_OUTPUT_RAW_DATA
+ * When defined the gyro data will be written to the UART channel.
+ * The UART must be configured to run really fast, e.g. in 2Mb/s.
+ */
+//#define ADC_OUTPUT_RAW_DATA
+
+#if defined(UART_OUTPUT_TRACE_DATA) && defined(ADC_OUTPUT_RAW_DATA)
+#  error "Can't define UART_OUTPUT_TRACE_DATA and ADC_OUTPUT_RAW_DATA at the same time"
+#endif
+
+#if defined(UART_OUTPUT_TRACE_DATA) || defined(ADC_OUTPUT_RAW_DATA) || defined(IMU_OUTPUT_RAW_DATA_ON_UART)
+#define UART_OUTPUT_RAW_DATA_ONLY
+#endif
+
+#if defined(UART_OUTPUT_TRACE_DATA) && defined(T_LAUNCH_ACC)
+#  error "UART_OUTPUT_TRACE_DATA and T_LAUNCH_ACC doesn't work at the same time yet due to dma sharing..."
+#endif
+
+#endif /* CONFIG_H_ */

FlyPi/crazyflie-firmware-2021.06/src/config/ffconf.h

+/*---------------------------------------------------------------------------/
+/  FatFs Functional Configurations
+/---------------------------------------------------------------------------*/
+
+#define FFCONF_DEF	80196	/* Revision ID */
+
+/*---------------------------------------------------------------------------/
+/ Function Configurations
+/---------------------------------------------------------------------------*/
+
+#define FF_FS_READONLY	0
+/* This option switches read-only configuration. (0:Read/Write or 1:Read-only)
+/  Read-only configuration removes writing API functions, f_write(), f_sync(),
+/  f_unlink(), f_mkdir(), f_chmod(), f_rename(), f_truncate(), f_getfree()
+/  and optional writing functions as well. */
+
+
+#define FF_FS_MINIMIZE	0
+/* This option defines minimization level to remove some basic API functions.
+/
+/   0: Basic functions are fully enabled.
+/   1: f_stat(), f_getfree(), f_unlink(), f_mkdir(), f_truncate() and f_rename()
+/      are removed.
+/   2: f_opendir(), f_readdir() and f_closedir() are removed in addition to 1.
+/   3: f_lseek() function is removed in addition to 2. */
+
+
+#define FF_USE_STRFUNC	2
+/* This option switches string functions, f_gets(), f_putc(), f_puts() and f_printf().
+/
+/  0: Disable string functions.
+/  1: Enable without LF-CRLF conversion.
+/  2: Enable with LF-CRLF conversion. */
+
+
+#define FF_USE_FIND		1
+/* This option switches filtered directory read functions, f_findfirst() and
+/  f_findnext(). (0:Disable, 1:Enable 2:Enable with matching altname[] too) */
+
+
+#define FF_USE_MKFS		1
+/* This option switches f_mkfs() function. (0:Disable or 1:Enable) */
+
+
+#define FF_USE_FASTSEEK	0
+/* This option switches fast seek function. (0:Disable or 1:Enable) */
+
+
+#define FF_USE_EXPAND	0
+/* This option switches f_expand function. (0:Disable or 1:Enable) */
+
+
+#define FF_USE_CHMOD	0
+/* This option switches attribute manipulation functions, f_chmod() and f_utime().
+/  (0:Disable or 1:Enable) Also FF_FS_READONLY needs to be 0 to enable this option. */
+
+
+#define FF_USE_LABEL	1
+/* This option switches volume label functions, f_getlabel() and f_setlabel().
+/  (0:Disable or 1:Enable) */
+
+
+#define FF_USE_FORWARD	1
+/* This option switches f_forward() function. (0:Disable or 1:Enable) */
+
+
+/*---------------------------------------------------------------------------/
+/ Locale and Namespace Configurations
+/---------------------------------------------------------------------------*/
+
+#define FF_CODE_PAGE	850
+/* This option specifies the OEM code page to be used on the target system.
+/  Incorrect code page setting can cause a file open failure.
+/
+/   437 - U.S.
+/   720 - Arabic
+/   737 - Greek
+/   771 - KBL
+/   775 - Baltic
+/   850 - Latin 1
+/   852 - Latin 2
+/   855 - Cyrillic
+/   857 - Turkish
+/   860 - Portuguese
+/   861 - Icelandic
+/   862 - Hebrew
+/   863 - Canadian French
+/   864 - Arabic
+/   865 - Nordic
+/   866 - Russian
+/   869 - Greek 2
+/   932 - Japanese (DBCS)
+/   936 - Simplified Chinese (DBCS)
+/   949 - Korean (DBCS)
+/   950 - Traditional Chinese (DBCS)
+/     0 - Include all code pages above and configured by f_setcp()
+*/
+
+
+#define FF_USE_LFN		1
+#define FF_MAX_LFN		255
+/* The FF_USE_LFN switches the support for LFN (long file name).
+/
+/   0: Disable LFN. FF_MAX_LFN has no effect.
+/   1: Enable LFN with static  working buffer on the BSS. Always NOT thread-safe.
+/   2: Enable LFN with dynamic working buffer on the STACK.
+/   3: Enable LFN with dynamic working buffer on the HEAP.
+/
+/  To enable the LFN, ffunicode.c needs to be added to the project. The LFN function
+/  requiers certain internal working buffer occupies (FF_MAX_LFN + 1) * 2 bytes and
+/  additional (FF_MAX_LFN + 44) / 15 * 32 bytes when exFAT is enabled.
+/  The FF_MAX_LFN defines size of the working buffer in UTF-16 code unit and it can
+/  be in range of 12 to 255. It is recommended to be set it 255 to fully support LFN
+/  specification.
+/  When use stack for the working buffer, take care on stack overflow. When use heap
+/  memory for the working buffer, memory management functions, ff_memalloc() and
+/  ff_memfree() exemplified in ffsystem.c, need to be added to the project. */
+
+
+#define FF_LFN_UNICODE	0
+/* This option switches the character encoding on the API when LFN is enabled.
+/
+/   0: ANSI/OEM in current CP (TCHAR = char)
+/   1: Unicode in UTF-16 (TCHAR = WCHAR)
+/   2: Unicode in UTF-8 (TCHAR = char)
+/   3: Unicode in UTF-32 (TCHAR = DWORD)
+/
+/  Also behavior of string I/O functions will be affected by this option.
+/  When LFN is not enabled, this option has no effect. */
+
+
+#define FF_LFN_BUF		255
+#define FF_SFN_BUF		12
+/* This set of options defines size of file name members in the FILINFO structure
+/  which is used to read out directory items. These values should be suffcient for
+/  the file names to read. The maximum possible length of the read file name depends
+/  on character encoding. When LFN is not enabled, these options have no effect. */
+
+
+#define FF_STRF_ENCODE	3
+/* When FF_LFN_UNICODE >= 1 with LFN enabled, string I/O functions, f_gets(),
+/  f_putc(), f_puts and f_printf() convert the character encoding in it.
+/  This option selects assumption of character encoding ON THE FILE to be
+/  read/written via those functions.
+/
+/   0: ANSI/OEM in current CP
+/   1: Unicode in UTF-16LE
+/   2: Unicode in UTF-16BE
+/   3: Unicode in UTF-8
+*/
+
+
+#define FF_FS_RPATH		2
+/* This option configures support for relative path.
+/
+/   0: Disable relative path and remove related functions.
+/   1: Enable relative path. f_chdir() and f_chdrive() are available.
+/   2: f_getcwd() function is available in addition to 1.
+*/
+
+
+/*---------------------------------------------------------------------------/
+/ Drive/Volume Configurations
+/---------------------------------------------------------------------------*/
+
+#define FF_VOLUMES		1
+/* Number of volumes (logical drives) to be used. (1-10) */
+
+
+#define FF_STR_VOLUME_ID	1
+#define FF_VOLUME_STRS		"SD"
+/* FF_STR_VOLUME_ID switches support for volume ID in arbitrary strings.
+/  When FF_STR_VOLUME_ID is set to 1 or 2, arbitrary strings can be used as drive
+/  number in the path name. FF_VOLUME_STRS defines the volume ID strings for each
+/  logical drives. Number of items must not be less than FF_VOLUMES. Valid
+/  characters for the volume ID strings are A-Z, a-z and 0-9, however, they are
+/  compared in case-insensitive. If FF_STR_VOLUME_ID >= 1 and FF_VOLUME_STRS is
+/  not defined, a user defined volume string table needs to be defined as:
+/
+/  const char* VolumeStr[FF_VOLUMES] = {"ram","flash","sd","usb",...
+*/
+
+
+#define FF_MULTI_PARTITION	0
+/* This option switches support for multiple volumes on the physical drive.
+/  By default (0), each logical drive number is bound to the same physical drive
+/  number and only an FAT volume found on the physical drive will be mounted.
+/  When this function is enabled (1), each logical drive number can be bound to
+/  arbitrary physical drive and partition listed in the VolToPart[]. Also f_fdisk()
+/  funciton will be available. */
+
+
+#define FF_MIN_SS		512
+#define FF_MAX_SS		512
+/* This set of options configures the range of sector size to be supported. (512,
+/  1024, 2048 or 4096) Always set both 512 for most systems, generic memory card and
+/  harddisk. But a larger value may be required for on-board flash memory and some
+/  type of optical media. When FF_MAX_SS is larger than FF_MIN_SS, FatFs is configured
+/  for variable sector size mode and disk_ioctl() function needs to implement
+/  GET_SECTOR_SIZE command. */
+
+
+#define FF_LBA64		0
+/* This option switches support for 64-bit LBA. (0:Disable or 1:Enable)
+/  To enable the 64-bit LBA, also exFAT needs to be enabled. (FF_FS_EXFAT == 1) */
+
+
+#define FF_MIN_GPT		0x100000000
+/* Minimum number of sectors to switch GPT as partitioning format in f_mkfs and
+/  f_fdisk function. 0x100000000 max. This option has no effect when FF_LBA64 == 0. */
+
+
+#define FF_USE_TRIM		0
+/* This option switches support for ATA-TRIM. (0:Disable or 1:Enable)
+/  To enable Trim function, also CTRL_TRIM command should be implemented to the
+/  disk_ioctl() function. */
+
+
+
+/*---------------------------------------------------------------------------/
+/ System Configurations
+/---------------------------------------------------------------------------*/
+
+#define FF_FS_TINY		0
+/* This option switches tiny buffer configuration. (0:Normal or 1:Tiny)
+/  At the tiny configuration, size of file object (FIL) is shrinked FF_MAX_SS bytes.
+/  Instead of private sector buffer eliminated from the file object, common sector
+/  buffer in the filesystem object (FATFS) is used for the file data transfer. */
+
+
+#define FF_FS_EXFAT		0
+/* This option switches support for exFAT filesystem. (0:Disable or 1:Enable)
+/  To enable exFAT, also LFN needs to be enabled. (FF_USE_LFN >= 1)
+/  Note that enabling exFAT discards ANSI C (C89) compatibility. */
+
+
+#define FF_FS_NORTC		0
+#define FF_NORTC_MON	1
+#define FF_NORTC_MDAY	1
+#define FF_NORTC_YEAR	2020
+/* The option FF_FS_NORTC switches timestamp functiton. If the system does not have
+/  any RTC function or valid timestamp is not needed, set FF_FS_NORTC = 1 to disable
+/  the timestamp function. Every object modified by FatFs will have a fixed timestamp
+/  defined by FF_NORTC_MON, FF_NORTC_MDAY and FF_NORTC_YEAR in local time.
+/  To enable timestamp function (FF_FS_NORTC = 0), get_fattime() function need to be
+/  added to the project to read current time form real-time clock. FF_NORTC_MON,
+/  FF_NORTC_MDAY and FF_NORTC_YEAR have no effect.
+/  These options have no effect in read-only configuration (FF_FS_READONLY = 1). */
+
+
+#define FF_FS_NOFSINFO	0
+/* If you need to know correct free space on the FAT32 volume, set bit 0 of this
+/  option, and f_getfree() function at first time after volume mount will force
+/  a full FAT scan. Bit 1 controls the use of last allocated cluster number.
+/
+/  bit0=0: Use free cluster count in the FSINFO if available.
+/  bit0=1: Do not trust free cluster count in the FSINFO.
+/  bit1=0: Use last allocated cluster number in the FSINFO if available.
+/  bit1=1: Do not trust last allocated cluster number in the FSINFO.
+*/
+
+
+#define FF_FS_LOCK		0
+/* The option FF_FS_LOCK switches file lock function to control duplicated file open
+/  and illegal operation to open objects. This option must be 0 when FF_FS_READONLY
+/  is 1.
+/
+/  0:  Disable file lock function. To avoid volume corruption, application program
+/      should avoid illegal open, remove and rename to the open objects.
+/  >0: Enable file lock function. The value defines how many files/sub-directories
+/      can be opened simultaneously under file lock control. Note that the file
+/      lock control is independent of re-entrancy. */
+
+
+/* #include <somertos.h>	// O/S definitions */
+#define FF_FS_REENTRANT	0
+#define FF_FS_TIMEOUT	1000
+#define FF_SYNC_t		HANDLE
+/* The option FF_FS_REENTRANT switches the re-entrancy (thread safe) of the FatFs
+/  module itself. Note that regardless of this option, file access to different
+/  volume is always re-entrant and volume control functions, f_mount(), f_mkfs()
+/  and f_fdisk() function, are always not re-entrant. Only file/directory access
+/  to the same volume is under control of this function.
+/
+/   0: Disable re-entrancy. FF_FS_TIMEOUT and FF_SYNC_t have no effect.
+/   1: Enable re-entrancy. Also user provided synchronization handlers,
+/      ff_req_grant(), ff_rel_grant(), ff_del_syncobj() and ff_cre_syncobj()
+/      function, must be added to the project. Samples are available in
+/      option/syscall.c.
+/
+/  The FF_FS_TIMEOUT defines timeout period in unit of time tick.
+/  The FF_SYNC_t defines O/S dependent sync object type. e.g. HANDLE, ID, OS_EVENT*,
+/  SemaphoreHandle_t and etc. A header file for O/S definitions needs to be
+/  included somewhere in the scope of ff.h. */
+
+
+
+/*--- End of configuration options ---*/