FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/interface/mac.h

+#ifndef __MAC_H__
+#define __MAC_H__
+
+#include <stdint.h>
+
+#include "locodeck.h"
+
+// Packet format with compressed PAN and 64Bit addresses
+// Maximum 64 bytes payload
+typedef struct packet_s {
+    union {
+      uint16_t fcf;
+      struct {
+        uint16_t type:3;
+        uint16_t security:1;
+        uint16_t framePending:1;
+        uint16_t ack:1;
+        uint16_t ipan:1;
+        uint16_t reserved:3;
+        uint16_t destAddrMode:2;
+        uint16_t version:2;
+        uint16_t srcAddrMode:2;
+      } fcf_s;
+    };
+
+    uint8_t seq;
+    uint16_t pan;
+    locoAddress_t destAddress;
+    locoAddress_t sourceAddress;
+
+    uint8_t payload[128];
+} __attribute__((packed)) packet_t;
+
+#define MAC80215_PACKET_INIT(packet, TYPE) packet.fcf_s.type = (TYPE); \
+  packet.fcf_s.security = 0; \
+  packet.fcf_s.framePending = 0; \
+  packet.fcf_s.ack = 0; \
+  packet.fcf_s.ipan = 1; \
+  packet.fcf_s.destAddrMode = 3; \
+  packet.fcf_s.version = 1; \
+  packet.fcf_s.srcAddrMode = 3;
+
+
+#define MAC802154_TYPE_BEACON 0
+#define MAC802154_TYPE_DATA 1
+#define MAC802154_TYPE_ACK 2
+#define MAC802154_TYPE_CMD 3
+
+#define MAC802154_HEADER_LENGTH 21
+
+#endif

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/interface/usddeck.h

+#ifndef __USDDECK_H__
+#define __USDDECK_H__
+
+#include <stdint.h>
+#include <stdbool.h>
+
+enum usddeckLoggingMode_e
+{
+  usddeckLoggingMode_Disabled = 0,
+  usddeckLoggingMode_SynchronousStabilizer,
+  usddeckLoggingMode_Asynchronous,
+};
+
+// returns true if logging is enabled
+bool usddeckLoggingEnabled(void);
+
+// returns the current logging mode
+enum usddeckLoggingMode_e usddeckLoggingMode(void);
+
+// returns the desired logging frequency
+int usddeckFrequency(void);
+
+// For synchronous logging: add a new log entry
+void usddeckTriggerLogging(void);
+
+// returns size of current file if logging is stopped (0 otherwise)
+uint32_t usddeckFileSize(void);
+
+// Read "length" number of bytes at "offset" into "buffer" of current file
+// Only works if logging is stopped
+bool usddeckRead(uint32_t offset, uint8_t* buffer, uint16_t length);
+
+#endif //__USDDECK_H__

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/interface/zranger.h

+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 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/>.
+ *
+ * zranger.h: Z-Ranger deck driver
+ */
+
+#ifndef _ZRANGER_H_
+#define _ZRANGER_H_
+
+#include "stabilizer_types.h"
+#include "deck_core.h"
+
+void zRangerInit(DeckInfo* info);
+
+bool zRangerTest(void);
+void zRangerTask(void* arg);
+
+#endif /* _ZRANGER_H_ */

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/interface/zranger2.h

+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 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/>.
+ *
+ * zranger.h: Z-Ranger deck driver
+ */
+
+#ifndef _ZRANGER2_H_
+#define _ZRANGER2_H_
+
+#include "stabilizer_types.h"
+#include "deck_core.h"
+
+void zRanger2Init(DeckInfo* info);
+
+bool zRanger2Test(void);
+void zRanger2Task(void* arg);
+
+#endif /* _ZRANGER2_H_ */

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/activeMarkerDeck.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/>.
+ *
+ *
+ * activeMarkerDeck.c - Deck driver for the Active marker deck
+ */
+
+#include "stm32fxxx.h"
+#include "FreeRTOS.h"
+#include "task.h"
+
+#include "system.h"
+#include "deck.h"
+#include "log.h"
+#include "param.h"
+#include "eventtrigger.h"
+#include "i2cdev.h"
+
+#define DEBUG_MODULE "ACTIVE_MARKER"
+#include "debug.h"
+
+#define LED_COUNT 4
+
+#define MEM_ADR_LED 0x00
+#define MEM_ADR_MODE 0x01
+#define MEM_ADR_BUTTON_SENSOR 0x02
+#define MEM_ADR_VER 0x10
+
+#define DEFAULT_UPDATE_PERIOD_MS 1000
+#define POLL_UPDATE_PERIOD_MS 10
+
+static bool isInit = false;
+static bool isVerified = false;
+
+// currentId != requestedID at startup to make sure all IDs are initialized in the deck
+static uint8_t currentId[LED_COUNT] = {0xff, 0xff, 0xff, 0xff};
+static uint8_t requestedId[LED_COUNT] = {1, 3, 4, 2}; // 1 to 4, clockwise
+
+#define MODE_OFF 0
+#define MODE_PWM 1
+#define MODE_MODULATED 2
+#define MODE_QUALISYS 3
+#define MODE_UART_TEST 0xff
+#define MODE_BUTTON_RESET 0xff
+
+static uint8_t currentDeckMode = MODE_QUALISYS;
+static uint8_t requestedDeckMode = MODE_QUALISYS;
+
+// The deck button and sensor data is polled when doPollDeckButtonSensor > 0. Used for production test.
+static uint8_t doPollDeckButtonSensor = 0;
+static uint8_t deckButtonSensorValue = 0;
+static uint32_t nextPollTime = 0;
+static const uint32_t pollIntervall = M2T(100);
+static bool i2cOk = false;
+
+// defines eventTrigger_activeMarkerModeChanged
+EVENTTRIGGER(activeMarkerModeChanged, uint8, mode)
+
+#ifdef ACTIVE_MARKER_DECK_TEST
+static bool activeMarkerDeckCanStart = false;
+#endif
+
+#define DECK_I2C_ADDRESS 0x2E
+#define VERSION_STRING_LEN 12
+
+enum version_e {
+  versionUndefined = 0,
+  version_0_A,
+  version_1_0,
+};
+
+enum version_e deckFwVersion = versionUndefined;
+
+static char versionString[VERSION_STRING_LEN + 1];
+
+static void task(void* param);
+
+static void activeMarkerDeckInit(DeckInfo *info) {
+  if (isInit) {
+    return;
+  }
+
+  xTaskCreate(task, "activeMarkerDeck",
+              configMINIMAL_STACK_SIZE, NULL, 3, NULL);
+
+#ifndef ACTIVE_MARKER_DECK_TEST
+  memset(versionString, 0, VERSION_STRING_LEN + 1);
+  i2cOk = i2cdevReadReg8(I2C1_DEV, DECK_I2C_ADDRESS, MEM_ADR_VER, VERSION_STRING_LEN, (uint8_t*)versionString);
+  DEBUG_PRINT("Deck FW %s\n", versionString);
+#endif
+
+  isInit = true;
+}
+
+static bool activeMarkerDeckTest() {
+  if (!isInit) {
+    return false;
+  }
+
+#ifndef ACTIVE_MARKER_DECK_TEST
+  if (0 == strcmp("Qualisys0.A", versionString)) {
+    deckFwVersion = version_0_A;
+  } else if (0 == strcmp("Qualisys1.0", versionString)) {
+    deckFwVersion = version_1_0;
+  }
+
+  isVerified = (versionUndefined != deckFwVersion);
+  if (! isVerified) {
+    DEBUG_PRINT("Incompatible deck FW\n");
+  }
+#else
+  isVerified = true;
+  deckFwVersion = version_1_0;
+#endif
+
+  return isVerified;
+}
+
+static void handleIdUpdate() {
+  bool isDifferent = false;
+  for (int led = 0; led < LED_COUNT; led++) {
+    if (currentId[led] != requestedId[led]) {
+      isDifferent = true;
+      currentId[led] = requestedId[led];
+    }
+  }
+
+  if (isDifferent) {
+      i2cdevWriteReg8(I2C1_DEV, DECK_I2C_ADDRESS, MEM_ADR_LED, LED_COUNT, currentId);
+  }
+}
+
+static void handleModeUpdate() {
+  if (currentDeckMode != requestedDeckMode) {
+    currentDeckMode = requestedDeckMode;
+    i2cdevWriteReg8(I2C1_DEV, DECK_I2C_ADDRESS, MEM_ADR_MODE, 1, &currentDeckMode);
+
+    eventTrigger_activeMarkerModeChanged_payload.mode = currentDeckMode;
+    eventTrigger(&eventTrigger_activeMarkerModeChanged);
+  }
+}
+
+static void handleButtonSensorRead() {
+  if (doPollDeckButtonSensor) {
+    uint32_t now = xTaskGetTickCount();
+    if (now > nextPollTime) {
+      i2cdevReadReg8(I2C1_DEV, DECK_I2C_ADDRESS, MEM_ADR_BUTTON_SENSOR, 1, &deckButtonSensorValue);
+      nextPollTime = now + pollIntervall;
+    }
+  }
+}
+
+static void task(void *param) {
+  systemWaitStart();
+
+#ifdef ACTIVE_MARKER_DECK_TEST
+  while (!activeMarkerDeckCanStart) {
+    vTaskDelay(100);
+  }
+  i2cOk = i2cdevReadReg8(I2C1_DEV, DECK_I2C_ADDRESS, MEM_ADR_VER, VERSION_STRING_LEN, (uint8_t*)versionString);
+#endif
+
+  while (1) {
+    if (isVerified) {
+      handleIdUpdate();
+
+      if (deckFwVersion >= version_1_0) {
+        handleModeUpdate();
+        handleButtonSensorRead();
+      }
+    }
+
+    int delay = DEFAULT_UPDATE_PERIOD_MS;
+    if (doPollDeckButtonSensor) {
+      delay = POLL_UPDATE_PERIOD_MS;
+    }
+
+    vTaskDelay(M2T(delay));
+  }
+
+}
+
+static const DeckDriver deck_info = {
+  .vid = 0xBC,
+  .pid = 0x11,
+  .name = "bcActiveM",
+
+  .init = activeMarkerDeckInit,
+  .test = activeMarkerDeckTest,
+};
+
+DECK_DRIVER(deck_info);
+
+/**
+ *
+ * The Active Marker deck is mainly designed for Qualisys mocap systems and
+ * supports Qualisys Active markers, but it can also be used with other systems
+ * in a simplified mode. The deck has 4 arms with one IR LED on the tip of each
+ * arm and a light sensor in the center of the deck.
+
+ * The deck is configured using the parameter sub system, for details on which
+ * parameter to use, see below.
+ */
+PARAM_GROUP_START(activeMarker)
+
+/**
+ * @brief Qualisys id of marker for front (default: 1)
+ *
+ * In Qualisys mode the front LED act as an Active marker with IDs that are
+ * in the range 0 - 170.
+ */
+PARAM_ADD_CORE(PARAM_UINT8, front, &requestedId[0])
+
+/**
+ * @brief Qualisys id of marker for back (default: 3)
+ *
+ * In Qualisys mode the back LED act as an Active marker with IDs that are
+ * in the range 0 - 170.
+ */
+PARAM_ADD_CORE(PARAM_UINT8, back, &requestedId[1])
+
+/**
+ * @brief Qualisys id of marker for left (default: 4)
+ *
+ * In Qualisys mode the left LED act as an Active marker with IDs that are
+ * in the range 0 - 170.
+ */
+PARAM_ADD_CORE(PARAM_UINT8, left, &requestedId[2])
+
+/**
+ * @brief Qualisys id of marker for right (default: 2)
+ *
+ * In Qualisys mode the right LED act as an Active marker with IDs that are
+ * in the range 0 - 170.
+ */
+PARAM_ADD_CORE(PARAM_UINT8, right, &requestedId[3])
+
+/**
+ * @brief Off(0), pwm(1), modulated(2) or qualisys(3)
+ *
+ * |    Mode    |    Value      |         Comment                | \n
+ * | -          | -             | -                              | \n
+ * | OFF        |   0           |    Always off                  | \n
+ * | PWM        |   1           |    Always on, PWM modulated    | \n
+ * | MODULATED  |   2           |    Switching                   | \n
+ * | QUALISYS   |   3 (default) |    Qualisys Active Marker mode | \n
+ *
+ * ### Off mode
+ *
+ * All marker LEDs are turned off.
+ *
+ * ### PWM mode
+ *
+ * The marker LEDs are turned on and PWM modulated. The brightness of each LED
+ * is controlled by the marker parameters below, in the range 0 - 255.
+ *
+ * ### Modulated mode
+ *
+ * The LEDs are switched on and off at around 42 kHz (24 micro seconds cycle).
+ * The brightness of the LEDs during the “on” part of the cycle is controlled
+ * by the marker parameters below, in the range 0 - 255.
+ *
+ * ### Qualisys mode
+ *
+ * In this mode the LEDs act as Active markers with IDs that are identified by
+ * the Qualisys system and used for better 6-dof identification and tracking.
+ * The IDs are controlled by the marker parameters. The Qualisys systems
+ * and the deck currently supports IDs in the range [0 - 170]
+ */
+PARAM_ADD_CORE(PARAM_UINT8, mode, &requestedDeckMode)
+
+PARAM_ADD(PARAM_UINT8, poll, &doPollDeckButtonSensor)
+
+#ifdef ACTIVE_MARKER_DECK_TEST
+PARAM_ADD(PARAM_UINT8, canStart, &activeMarkerDeckCanStart)
+#endif
+
+PARAM_GROUP_STOP(activeMarker)
+
+/**
+ * The deck parameter group tells us which decks are connected.
+ * There is one parameter per official deck and the parameter is nonzero if the
+ * deck is connected.
+ */
+PARAM_GROUP_START(deck)
+
+/**
+ * @brief Nonzero if [Active Marker deck](%https://www.bitcraze.io/products/active-marker-deck) is attached
+ */
+PARAM_ADD_CORE(PARAM_UINT8 | PARAM_RONLY, bcActiveMarker, &isInit)
+
+PARAM_GROUP_STOP(deck)
+
+LOG_GROUP_START(activeMarker)
+LOG_ADD(LOG_UINT8, btSns, &deckButtonSensorValue)
+LOG_ADD(LOG_UINT8, i2cOk, &i2cOk)
+LOG_GROUP_STOP(activeMarker)

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/aideck.c

+/*
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 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/>.
+ *
+ * aideck.c - Deck driver for the AIdeck
+ */
+#define DEBUG_MODULE "AIDECK"
+
+#include <stdint.h>
+#include <string.h>
+#include <stdio.h>
+
+#include "stm32fxxx.h"
+#include "config.h"
+#include "console.h"
+#include "uart1.h"
+#include "debug.h"
+#include "deck.h"
+#include "FreeRTOS.h"
+#include "task.h"
+#include "queue.h"
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+#include "log.h"
+#include "param.h"
+#include "system.h"
+#include "uart1.h"
+#include "uart2.h"
+
+static bool isInit = false;
+static uint8_t byte;
+
+//Uncomment when NINA printout read is desired from console
+//#define DEBUG_NINA_PRINT
+
+#ifdef DEBUG_NINA_PRINT
+static void NinaTask(void *param)
+{
+    systemWaitStart();
+    vTaskDelay(M2T(1000));
+    DEBUG_PRINT("Starting reading out NINA debugging messages:\n");
+    vTaskDelay(M2T(2000));
+
+    // Pull the reset button to get a clean read out of the data
+    pinMode(DECK_GPIO_IO4, OUTPUT);
+    digitalWrite(DECK_GPIO_IO4, LOW);
+    vTaskDelay(10);
+    digitalWrite(DECK_GPIO_IO4, HIGH);
+    pinMode(DECK_GPIO_IO4, INPUT_PULLUP);
+
+    // Read out the byte the NINA sends and immediately send it to the console.
+    uint8_t byte;
+    while (1)
+    {
+        if (uart2GetDataWithDefaultTimeout(&byte) == true)
+        {
+            consolePutchar(byte);
+        }
+    }
+}
+#endif
+
+static void Gap8Task(void *param)
+{
+    systemWaitStart();
+    vTaskDelay(M2T(1000));
+
+    // Pull the reset button to get a clean read out of the data
+    pinMode(DECK_GPIO_IO4, OUTPUT);
+    digitalWrite(DECK_GPIO_IO4, LOW);
+    vTaskDelay(10);
+    digitalWrite(DECK_GPIO_IO4, HIGH);
+    pinMode(DECK_GPIO_IO4, INPUT_PULLUP);
+
+    // Read out the byte the Gap8 sends and immediately send it to the console.
+    while (1)
+    {
+        uart1GetDataWithDefaultTimeout(&byte);
+    }
+}
+
+static void aideckInit(DeckInfo *info)
+{
+
+    if (isInit)
+        return;
+
+    // Intialize the UART for the GAP8
+    uart1Init(115200);
+    // Initialize task for the GAP8
+    xTaskCreate(Gap8Task, AI_DECK_GAP_TASK_NAME, AI_DECK_TASK_STACKSIZE, NULL,
+                AI_DECK_TASK_PRI, NULL);
+
+#ifdef DEBUG_NINA_PRINT
+    // Initialize the UART for the NINA
+    uart2Init(115200);
+    // Initialize task for the NINA
+    xTaskCreate(NinaTask, AI_DECK_NINA_TASK_NAME, AI_DECK_TASK_STACKSIZE, NULL,
+                AI_DECK_TASK_PRI, NULL);
+
+#endif
+
+    isInit = true;
+}
+
+static bool aideckTest()
+{
+
+    return true;
+}
+
+static const DeckDriver aideck_deck = {
+    .vid = 0xBC,
+    .pid = 0x12,
+    .name = "bcAI",
+
+    .usedPeriph = 0,
+    .usedGpio = 0, // FIXME: Edit the used GPIOs
+
+    .init = aideckInit,
+    .test = aideckTest,
+};
+
+LOG_GROUP_START(aideck)
+LOG_ADD(LOG_UINT8, receivebyte, &byte)
+LOG_GROUP_STOP(aideck)
+
+/** @addtogroup deck
+*/
+PARAM_GROUP_START(deck)
+
+/**
+ * @brief Nonzero if [AI deck](%https://store.bitcraze.io/collections/decks/products/ai-deck-1-1) is attached
+ */
+PARAM_ADD_CORE(PARAM_UINT8 | PARAM_RONLY, bcAIDeck, &isInit)
+
+PARAM_GROUP_STOP(deck)
+
+DECK_DRIVER(aideck_deck);

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/bigquad.c

+/*
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 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/>.
+ *
+ * exptest.c - Testing of expansion port.
+ */
+#define DEBUG_MODULE "BIGQUAD"
+
+#include <stdint.h>
+#include <string.h>
+
+#include "stm32fxxx.h"
+#include "config.h"
+#include "motors.h"
+#include "debug.h"
+#include "deck.h"
+#include "param.h"
+#include "extrx.h"
+#include "pm.h"
+#include "uart1.h"
+#include "msp.h"
+
+#include "FreeRTOS.h"
+#include "task.h"
+
+#define BIGQUAD_BAT_VOLT_PIN       DECK_GPIO_MISO
+#define BIGQUAD_BAT_VOLT_MULT      7.8f
+#define BIGQUAD_BAT_CURR_PIN       DECK_GPIO_SCK
+#define BIGQUAD_BAT_AMP_PER_VOLT   1.0f
+
+#ifdef ENABLE_BQ_DECK
+
+//Hardware configuration
+static bool isInit;
+
+#ifdef BQ_DECK_ENABLE_OSD
+static MspObject s_MspObject;
+
+static void osdTask(void *param)
+{
+  while(1)
+  {
+    char ch;
+    uart1Getchar(&ch);
+
+    mspProcessByte(&s_MspObject, (uint8_t)ch);
+  }
+}
+
+static void osdResponseCallback(uint8_t* pBuffer, uint32_t bufferLen)
+{
+  uart1SendData(bufferLen, pBuffer);
+}
+#endif // BQ_DECK_ENABLE_OSD
+
+
+static void bigquadInit(DeckInfo *info)
+{
+  if(isInit) {
+    return;
+  }
+
+  DEBUG_PRINT("Switching to brushless.\n");
+  motorsInit(motorMapBigQuadDeck);
+  extRxInit();
+#ifdef BQ_DECK_ENABLE_PM
+  pmEnableExtBatteryVoltMeasuring(BIGQUAD_BAT_VOLT_PIN, BIGQUAD_BAT_VOLT_MULT);
+  pmEnableExtBatteryCurrMeasuring(BIGQUAD_BAT_CURR_PIN, BIGQUAD_BAT_AMP_PER_VOLT);
+#endif
+
+#ifdef BQ_DECK_ENABLE_OSD
+  uart1Init(115200);
+  mspInit(&s_MspObject, osdResponseCallback);
+  xTaskCreate(osdTask, BQ_OSD_TASK_NAME,
+              configMINIMAL_STACK_SIZE, NULL, BQ_OSD_TASK_PRI, NULL);
+#endif
+
+  isInit = true;
+}
+
+static bool bigquadTest()
+{
+  bool status = true;
+
+  if(!isInit)
+    return false;
+
+  status = motorsTest();
+
+  return status;
+}
+
+static const DeckDriver bigquad_deck = {
+  .vid = 0xBC,
+  .pid = 0x05,
+  .name = "bcBigQuad",
+
+  .usedPeriph = DECK_USING_TIMER3 | DECK_USING_TIMER14,
+  .usedGpio = DECK_USING_PA2 | DECK_USING_PA3 | DECK_USING_PB4 | DECK_USING_PB5 | DECK_USING_PA7,
+  .init = bigquadInit,
+  .test = bigquadTest,
+};
+
+DECK_DRIVER(bigquad_deck);
+
+PARAM_GROUP_START(deck)
+
+/**
+ * @brief Nonzero if [BigQuad deck](%https://www.bitcraze.io/products/bigquad-deck) is attached
+ */
+PARAM_ADD_CORE(PARAM_UINT8 | PARAM_RONLY, bcBigQuad, &isInit)
+
+PARAM_GROUP_STOP(deck)
+#endif // ENABLE_BQ_DECK

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/buzzdeck.c

+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2015 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/>.
+ *
+ * buzzdeck.c - Deck driver for the buzzer deck
+ */
+
+#include <stdint.h>
+#include <stdlib.h>
+#include "stm32fxxx.h"
+
+#include "deck.h"
+#include "param.h"
+
+#include "buzzer.h"
+#include "piezo.h"
+
+static bool isInit;
+
+static void buzzDeckOn(uint32_t freq)
+{
+  piezoSetRatio(128);
+  piezoSetFreq(freq);
+}
+
+static void buzzDeckOff()
+{
+  piezoSetRatio(0);
+}
+
+static struct buzzerControl buzzDeckCtrl = {
+  .on         = buzzDeckOn,
+  .off        = buzzDeckOff
+};
+
+static void buzzDeckInit(DeckInfo *info)
+{
+  if (isInit) {
+    return;
+  }
+
+  piezoInit();
+  buzzerSetControl(&buzzDeckCtrl);
+
+  isInit = true;
+}
+
+static const DeckDriver buzzer_deck = {
+  .vid = 0xBC,
+  .pid = 0x04,
+  .name = "bcBuzzer",
+
+  .usedPeriph = DECK_USING_TIMER5,
+  .usedGpio = DECK_USING_TX2 | DECK_USING_RX2,
+
+  .init = buzzDeckInit,
+};
+
+DECK_DRIVER(buzzer_deck);
+
+PARAM_GROUP_START(deck)
+
+/**
+ * @brief Nonzero if [Buzzer deck](%https://store.bitcraze.io/collections/decks/products/buzzer-deck) is attached
+ */
+PARAM_ADD_CORE(PARAM_UINT8 | PARAM_RONLY, bcBuzzer, &isInit)
+
+PARAM_GROUP_STOP(deck)

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/cppmdeck.c

+/*
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 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/>.
+ *
+ * exptest.c - Testing of expansion port.
+ */
+#define DEBUG_MODULE "CPPM"
+
+#include <stdint.h>
+#include <string.h>
+
+#include "stm32fxxx.h"
+#include "config.h"
+#include "debug.h"
+#include "deck.h"
+#include "extrx.h"
+#include "param.h"
+
+//Hardware configuration
+static bool isInit;
+
+static void cppmdeckInit(DeckInfo *info)
+{
+  if(isInit)
+    return;
+
+  extRxInit();
+
+  isInit = true;
+}
+
+static bool cppmdeckTest()
+{
+  bool status = true;
+
+  if(!isInit)
+    return false;
+
+  return status;
+}
+
+static const DeckDriver cppm_deck = {
+  .vid = 0,
+  .pid = 0,
+  .name = "bcCPPM",
+
+  .usedPeriph = DECK_USING_TIMER14,
+  .usedGpio = DECK_USING_PA7,
+  .init = cppmdeckInit,
+  .test = cppmdeckTest,
+};
+
+DECK_DRIVER(cppm_deck);
+
+PARAM_GROUP_START(deck)
+
+PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, bcCPPM, &isInit)
+PARAM_GROUP_STOP(deck)

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/flowdeck_v1v2.c

+/*
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * LPS node firmware.
+ *
+ * Copyright 2017, Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Foobar 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 Foobar.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/* flowdeck.c: Flow deck driver */
+#include "FreeRTOS.h"
+#include "task.h"
+
+#include "deck.h"
+#include "debug.h"
+#include "system.h"
+#include "log.h"
+#include "param.h"
+#include "pmw3901.h"
+#include "sleepus.h"
+
+#include "stabilizer_types.h"
+#include "estimator.h"
+#include "estimator.h"
+
+#include "cf_math.h"
+
+#include "usec_time.h"
+#include <stdlib.h>
+
+#define AVERAGE_HISTORY_LENGTH 4
+#define OULIER_LIMIT 100
+#define LP_CONSTANT 0.8f
+// #define USE_LP_FILTER
+// #define USE_MA_SMOOTHING
+
+#if defined(USE_MA_SMOOTHING)
+static struct {
+  float32_t averageX[AVERAGE_HISTORY_LENGTH];
+  float32_t averageY[AVERAGE_HISTORY_LENGTH];
+  size_t ptr;
+} pixelAverages;
+#endif
+
+float dpixelx_previous = 0;
+float dpixely_previous = 0;
+
+static uint8_t outlierCount = 0;
+static float stdFlow = 2.0f;
+
+static bool isInit1 = false;
+static bool isInit2 = false;
+
+motionBurst_t currentMotion;
+
+// Disables pushing the flow measurement in the EKF
+static bool useFlowDisabled = false;
+
+// Turn on adaptive standard deviation for the kalman filter
+static bool useAdaptiveStd = false;
+
+// Set standard deviation flow
+// (will not work if useAdaptiveStd is on)
+static float flowStdFixed = 2.0f;
+
+#define NCS_PIN DECK_GPIO_IO3
+
+
+static void flowdeckTask(void *param)
+{
+  systemWaitStart();
+
+  uint64_t lastTime  = usecTimestamp();
+  while(1) {
+    vTaskDelay(10);
+
+    pmw3901ReadMotion(NCS_PIN, &currentMotion);
+
+    // Flip motion information to comply with sensor mounting
+    // (might need to be changed if mounted differently)
+    int16_t accpx = -currentMotion.deltaY;
+    int16_t accpy = -currentMotion.deltaX;
+
+    // Outlier removal
+    if (abs(accpx) < OULIER_LIMIT && abs(accpy) < OULIER_LIMIT) {
+
+    if (useAdaptiveStd)
+    {
+      // The standard deviation is fitted by measurements flying over low and high texture
+      //   and looking at the shutter time
+      float shutter_f = (float)currentMotion.shutter;
+      stdFlow=0.0007984f *shutter_f + 0.4335f;
+
+
+      // The formula with the amount of features instead
+      /*float squal_f = (float)currentMotion.squal;
+      stdFlow =  -0.01257f * squal_f + 4.406f; */
+      if (stdFlow < 0.1f) stdFlow=0.1f;
+    } else {
+      stdFlow = flowStdFixed;
+    }
+
+
+    // Form flow measurement struct and push into the EKF
+    flowMeasurement_t flowData;
+    flowData.stdDevX = stdFlow;
+    flowData.stdDevY = stdFlow;
+    flowData.dt = 0.01;
+
+#if defined(USE_MA_SMOOTHING)
+      // Use MA Smoothing
+      pixelAverages.averageX[pixelAverages.ptr] = (float32_t)accpx;
+      pixelAverages.averageY[pixelAverages.ptr] = (float32_t)accpy;
+
+      float32_t meanX;
+      float32_t meanY;
+
+      arm_mean_f32(pixelAverages.averageX, AVERAGE_HISTORY_LENGTH, &meanX);
+      arm_mean_f32(pixelAverages.averageY, AVERAGE_HISTORY_LENGTH, &meanY);
+
+      pixelAverages.ptr = (pixelAverages.ptr + 1) % AVERAGE_HISTORY_LENGTH;
+
+      flowData.dpixelx = (float)meanX;   // [pixels]
+      flowData.dpixely = (float)meanY;   // [pixels]
+#elif defined(USE_LP_FILTER)
+      // Use LP filter measurements
+      flowData.dpixelx = LP_CONSTANT * dpixelx_previous + (1.0f - LP_CONSTANT) * (float)accpx;
+      flowData.dpixely = LP_CONSTANT * dpixely_previous + (1.0f - LP_CONSTANT) * (float)accpy;
+      dpixelx_previous = flowData.dpixelx;
+      dpixely_previous = flowData.dpixely;
+#else
+      // Use raw measurements
+      flowData.dpixelx = (float)accpx;
+      flowData.dpixely = (float)accpy;
+#endif
+      // Push measurements into the estimator if flow is not disabled
+      //    and the PMW flow sensor indicates motion detection
+      if (!useFlowDisabled && currentMotion.motion == 0xB0) {
+        flowData.dt = (float)(usecTimestamp()-lastTime)/1000000.0f;
+        lastTime = usecTimestamp();
+        estimatorEnqueueFlow(&flowData);
+      }
+    } else {
+      outlierCount++;
+    }
+  }
+}
+
+static void flowdeck1Init()
+{
+  if (isInit1 || isInit2) {
+    return;
+  }
+
+  // Initialize the VL53L0 sensor using the zRanger deck driver
+  const DeckDriver *zRanger = deckFindDriverByName("bcZRanger");
+  zRanger->init(NULL);
+
+  if (pmw3901Init(NCS_PIN))
+  {
+    xTaskCreate(flowdeckTask, FLOW_TASK_NAME, FLOW_TASK_STACKSIZE, NULL,
+                FLOW_TASK_PRI, NULL);
+
+    isInit1 = true;
+  }
+}
+
+static bool flowdeck1Test()
+{
+  if (!isInit1) {
+    DEBUG_PRINT("Error while initializing the PMW3901 sensor\n");
+    return false;
+  }
+
+  // Test the VL53L0 driver
+  const DeckDriver *zRanger = deckFindDriverByName("bcZRanger");
+
+  return zRanger->test();
+}
+
+static const DeckDriver flowdeck1_deck = {
+  .vid = 0xBC,
+  .pid = 0x0A,
+  .name = "bcFlow",
+
+  .usedGpio = 0,  // FIXME: set the used pins
+  .requiredEstimator = kalmanEstimator,
+
+  .init = flowdeck1Init,
+  .test = flowdeck1Test,
+};
+
+DECK_DRIVER(flowdeck1_deck);
+
+static void flowdeck2Init()
+{
+  if (isInit1 || isInit2) {
+    return;
+  }
+
+  // Initialize the VL53L1 sensor using the zRanger deck driver
+  const DeckDriver *zRanger = deckFindDriverByName("bcZRanger2");
+  zRanger->init(NULL);
+
+  if (pmw3901Init(NCS_PIN))
+  {
+    xTaskCreate(flowdeckTask, FLOW_TASK_NAME, FLOW_TASK_STACKSIZE, NULL,
+                FLOW_TASK_PRI, NULL);
+
+    isInit2 = true;
+  }
+}
+
+static bool flowdeck2Test()
+{
+  if (!isInit2) {
+    DEBUG_PRINT("Error while initializing the PMW3901 sensor\n");
+    return false;
+  }
+
+  // Test the VL53L1 driver
+  const DeckDriver *zRanger = deckFindDriverByName("bcZRanger2");
+
+  return zRanger->test();
+}
+
+static const DeckDriver flowdeck2_deck = {
+  .vid = 0xBC,
+  .pid = 0x0F,
+  .name = "bcFlow2",
+
+  .usedGpio = 0,  // FIXME: set the used pins
+  .requiredEstimator = kalmanEstimator,
+
+  .init = flowdeck2Init,
+  .test = flowdeck2Test,
+};
+
+DECK_DRIVER(flowdeck2_deck);
+
+/**
+ * Logging variables of the motion sensor of the flowdeck
+ */
+LOG_GROUP_START(motion)
+/**
+ * @brief True if motion occured since the last measurement
+ */
+LOG_ADD(LOG_UINT8, motion, &currentMotion.motion)
+/**
+ * @brief Flow X  measurment  [flow/fr]
+ */
+LOG_ADD(LOG_INT16, deltaX, &currentMotion.deltaX)
+/**
+ * @brief Flow Y measurement [flow/fr]
+ */
+LOG_ADD(LOG_INT16, deltaY, &currentMotion.deltaY)
+/**
+ * @brief Shutter time [clock cycles]
+ */
+LOG_ADD(LOG_UINT16, shutter, &currentMotion.shutter)
+/**
+ * @brief Maximum raw data value in frame
+ */
+LOG_ADD(LOG_UINT8, maxRaw, &currentMotion.maxRawData)
+/**
+ * @brief Minimum raw data value in frame
+ */
+LOG_ADD(LOG_UINT8, minRaw, &currentMotion.minRawData)
+/**
+ * @brief Avarage raw data value
+ */
+LOG_ADD(LOG_UINT8, Rawsum, &currentMotion.rawDataSum)
+/**
+ * @brief Counted flow outliers exluded from the estimator
+ */
+LOG_ADD(LOG_UINT8, outlierCount, &outlierCount)
+/**
+ * @brief Count of surface feature
+ */
+LOG_ADD(LOG_UINT8, squal, &currentMotion.squal)
+/**
+ * @brief Standard deviation of flow measurement
+ */
+LOG_ADD(LOG_FLOAT, std, &stdFlow)
+LOG_GROUP_STOP(motion)
+
+/**
+ * Settings and parameters for handling of the flowdecks
+ * measurments
+ */
+PARAM_GROUP_START(motion)
+/**
+ * @brief Nonzero to not push the flow measurement in the EKF (default: 0)
+ */
+PARAM_ADD(PARAM_UINT8, disable, &useFlowDisabled)
+/**
+ * @brief Nonzero to turn on adaptive standard devivation estimation (default: 0)
+ */
+PARAM_ADD(PARAM_UINT8, adaptive, &useAdaptiveStd)
+/**
+ * @brief Set standard devivation flow measurement (default: 2.0f)
+ */
+PARAM_ADD_CORE(PARAM_FLOAT, flowStdFixed, &flowStdFixed)
+PARAM_GROUP_STOP(motion)
+
+PARAM_GROUP_START(deck)
+
+/**
+ * @brief Nonzero if Flow deck v1 is attached
+ */
+PARAM_ADD_CORE(PARAM_UINT8 | PARAM_RONLY, bcFlow, &isInit1)
+
+/**
+ * @brief Nonzero if [Flow deck v2](%https://store.bitcraze.io/collections/decks/products/flow-deck-v2) is attached
+ */
+PARAM_ADD_CORE(PARAM_UINT8 | PARAM_RONLY, bcFlow2, &isInit2)
+
+PARAM_GROUP_STOP(deck)

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/gtgps.c

+/*
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 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/>.
+ *
+ * exptest.c - Testing of expansion port.
+ */
+#define DEBUG_MODULE "GTGPS"
+
+#include <stdint.h>
+#include <string.h>
+#include <stdio.h>
+
+#include "stm32fxxx.h"
+#include "config.h"
+#include "console.h"
+#include "uart1.h"
+#include "debug.h"
+#include "deck.h"
+#include "FreeRTOS.h"
+#include "task.h"
+#include "queue.h"
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+#include "log.h"
+#include "param.h"
+
+static bool isInit;
+
+#define LEN_TOKEN 5
+#define MAX_LEN_SENTANCE 100
+char buff[MAX_LEN_SENTANCE];
+uint8_t bi;
+
+typedef bool (*SentanceParser)(char * buff);
+
+typedef struct {
+  const char * token;
+  SentanceParser parser;
+} ParserConfig;
+
+typedef enum {
+  FixNone = 1,
+  Fix2D = 2,
+  Fix3D = 3
+} FixQuality;
+
+typedef enum {
+  NoFix = 1,
+  GPSFix = 2
+} FixType;
+
+typedef enum {FIELD_COORD, FIELD_FLOAT, FIELD_INT} FieldType;
+
+typedef struct {
+  FixQuality fix;
+  uint32_t locks[12];
+  float pdop;
+  float hdop;
+  float vdop;
+} Basic;
+
+typedef struct {
+  uint32_t fixtime;
+  int32_t latitude;
+  int32_t longitude;
+  FixType fixtype;
+  uint32_t nsat;
+  float hdop;
+  float alt;
+  float height;
+} MeasData;
+
+static Basic b;
+static MeasData m;
+
+// Only use on 0-terminated strings!
+static int skip_to_next(char ** sp, const char ch) {
+  int steps=0;
+  while (ch != 0 && (**sp) != ch) {
+    (*sp)++;
+    steps++;
+  }
+  if (ch != 0)
+    (*sp)++;
+  return (ch != 0 ? steps : -1);
+}
+
+static int32_t parse_coordinate(char ** sp) {
+  int32_t dm;
+  int32_t degree;
+  int32_t minute;
+  int32_t second;
+  int32_t ret;
+  char * i;
+  char * j;
+
+  // Format as DDDMM.SSSS converted by long or lat = DDD + MM / 100 + SSSS/3600
+  // To avoid inaccuracy caused by float representation save this value as
+  // a large number * 10 M
+
+  // 32 18.0489 N = 32 degrees + 18.0489 / 60 = 32.300815 N
+  dm = strtol(*sp, &i, 10);
+  degree = (dm / 100) * 10000000;
+  minute = ((dm % 100) * 10000000) / 60;
+  second = (strtol(i+1, &j, 10) * 1000) / 60;
+  ret = degree + minute + second;
+  skip_to_next(sp, ',');
+  if (**sp == 'S' || **sp == 'W')
+    ret *= -1;
+  return ret;
+}
+
+static float parse_float(char * sp) {
+  float ret = 0;
+  int major = 0;
+  int minor = 0;
+  int deci_nbr = 0;
+  char * i;
+  char * j;
+
+  major = strtol(sp, &i, 10);
+  // Do decimals
+  if (strncmp(i, ".", 1) == 0) {
+    minor = strtol(i+1, &j, 10);
+    deci_nbr = j - i - 1;
+  }
+  ret = (major * pow(10, deci_nbr) + minor) / pow(10, deci_nbr);
+  //printf("%i.%i == %f (%i) (%c)\n", major, minor, ret, deci_nbr, (int) *i);
+  return ret;
+}
+
+static void parse_next(char ** sp, FieldType t, void * value) {
+  skip_to_next(sp, ',');
+  //DEBUG_PRINT("[%s]\n", (*sp));
+  switch (t) {
+    case FIELD_INT:
+      *((uint32_t*) value) = strtol(*sp, 0, 10);
+      break;
+    case FIELD_FLOAT:
+      *((float*) value) = parse_float(*sp);
+      break;
+    case FIELD_COORD:
+      *((int32_t*) value) = parse_coordinate(sp);
+  }
+}
+
+static bool gpgsaParser(char * buff) {
+  int i = 0;
+  char * sp = buff;
+
+  // Skip leading A/M
+  skip_to_next(&sp, ',');
+
+  parse_next(&sp, FIELD_INT, &b.fix);
+  for (i = 0; i < 12; i++) {
+    parse_next(&sp, FIELD_INT, &b.locks[i]);
+  }
+  parse_next(&sp, FIELD_FLOAT, &b.pdop);
+  parse_next(&sp, FIELD_FLOAT, &b.hdop);
+  parse_next(&sp, FIELD_FLOAT, &b.vdop);
+
+  //dbg_print_basic(&b);
+  return false;
+}
+
+static bool gpggaParser(char * buff) {
+  char * sp = buff;
+
+  parse_next(&sp, FIELD_INT, &m.fixtime);
+  parse_next(&sp, FIELD_COORD, &m.latitude);
+  parse_next(&sp, FIELD_COORD, &m.longitude);
+  parse_next(&sp, FIELD_INT, &m.fixtype);
+  parse_next(&sp, FIELD_INT, &m.nsat);
+  parse_next(&sp, FIELD_FLOAT, &m.hdop);
+  parse_next(&sp, FIELD_FLOAT, &m.alt);
+  skip_to_next(&sp, ',');
+  // Unit for altitude (not used yet)
+  parse_next(&sp, FIELD_FLOAT, &m.height);
+  skip_to_next(&sp, ',');
+  // Unit for height (not used yet)
+  skip_to_next(&sp, ',');
+  //consolePutchar('.');
+  //consoleFlush();
+  return false;
+}
+
+static ParserConfig parsers[] = {
+  {.token = "GPGSA", .parser = gpgsaParser},
+  {.token = "GPGGA", .parser = gpggaParser}
+};
+
+static bool verifyChecksum(const char * buff) {
+  uint8_t test_chksum = 0;
+  uint32_t ref_chksum = 0;
+  uint8_t i = 0;
+  while (buff[i] != '*' && i < MAX_LEN_SENTANCE-3) {
+    test_chksum ^= buff[i++];
+  }
+  ref_chksum = strtol(&buff[i+1], 0, 16);
+
+  return (test_chksum == ref_chksum);
+}
+
+static uint8_t baudcmd[] = "$PMTK251,115200*1F\r\n";
+
+// 5 Hz
+static uint8_t updaterate[] = "$PMTK220,200*2C\r\n";
+static uint8_t updaterate2[] = "$PMTK300,200,0,0,0,0*2F\r\n";
+
+// 10 Hz
+//static uint8_t updaterate3[] = "$PMTK220,100*2F\r\n";
+//static uint8_t updaterate4[] = "$PMTK300,100,0,0,0,0*2C\r\n";
+
+
+void gtgpsTask(void *param)
+{
+  char ch;
+  int j;
+
+  uart1SendData(sizeof(baudcmd), baudcmd);
+
+  vTaskDelay(500);
+  uart1Init(115200);
+  vTaskDelay(500);
+
+  uart1SendData(sizeof(updaterate), updaterate);
+  uart1SendData(sizeof(updaterate2), updaterate2);
+
+//  uart1SendData(sizeof(updaterate3), updaterate3);
+//  uart1SendData(sizeof(updaterate4), updaterate4);
+
+
+  while(1)
+  {
+    uart1Getchar(&ch);
+    consolePutchar(ch);
+
+    if (ch == '$') {
+      bi = 0;
+    } else if (ch == '\n') {
+      buff[bi] = 0; // Terminate with null
+      if (verifyChecksum(buff)) {
+        //DEBUG_PRINT("O");
+        for (j = 0; j < sizeof(parsers)/sizeof(parsers[0]); j++) {
+          if (strncmp(parsers[j].token, buff, LEN_TOKEN) == 0) {
+            parsers[j].parser(&buff[LEN_TOKEN]);
+          }
+        }
+      }
+    } else if (bi < MAX_LEN_SENTANCE) {
+      buff[bi++] = ch;
+    }
+  }
+}
+
+
+static void gtgpsInit(DeckInfo *info)
+{
+  if(isInit)
+    return;
+
+  DEBUG_PRINT("Enabling reading from GlobalTop GPS\n");
+  uart1Init(9600);
+
+  xTaskCreate(gtgpsTask, GTGPS_DECK_TASK_NAME,
+              configMINIMAL_STACK_SIZE, NULL, GTGPS_DECK_TASK_PRI, NULL);
+
+  isInit = true;
+}
+
+static bool gtgpsTest()
+{
+  bool status = true;
+
+  if(!isInit)
+    return false;
+
+  return status;
+}
+
+static const DeckDriver gtgps_deck = {
+  .vid = 0xBC,
+  .pid = 0x07,
+  .name = "bcGTGPS",
+
+  .usedPeriph = 0,
+  .usedGpio = 0,               // FIXME: Edit the used GPIOs
+
+  .init = gtgpsInit,
+  .test = gtgpsTest,
+};
+
+DECK_DRIVER(gtgps_deck);
+
+PARAM_GROUP_START(deck)
+PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, bcGTGPS, &isInit)
+PARAM_GROUP_STOP(deck)
+
+LOG_GROUP_START(gps)
+LOG_ADD(LOG_INT32, lat, &m.latitude)
+LOG_ADD(LOG_INT32, lon, &m.longitude)
+LOG_ADD(LOG_FLOAT, hMSL, &m.height)
+LOG_ADD(LOG_FLOAT, hAcc, &b.pdop)
+LOG_ADD(LOG_INT32, nsat, &m.nsat)
+LOG_ADD(LOG_INT32, fix, &b.fix)
+LOG_GROUP_STOP(gps)

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/ledring12.c

+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 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/>.
+ *
+ * ledring12.c: RGB Ring 12 Leds effects/driver
+ */
+
+#include <stdint.h>
+#include <math.h>
+#include <string.h>
+
+#include "stm32fxxx.h"
+
+#include "deck.h"
+
+#include "FreeRTOS.h"
+#include "timers.h"
+
+#include "ws2812.h"
+#include "worker.h"
+#include "param.h"
+#include "pm.h"
+#include "log.h"
+#include "pulse_processor.h"
+#include "mem.h"
+
+#define DEBUG_MODULE "LED"
+#include "debug.h"
+
+#ifdef LED_RING_NBR_LEDS
+#define NBR_LEDS  LED_RING_NBR_LEDS
+#else
+#define NBR_LEDS  12
+#endif
+
+#ifndef LEDRING_TIME_MEM_SIZE
+#define LEDRING_TIME_MEM_SIZE 10
+#endif
+
+typedef struct __attribute__((packed)) timing {
+  uint8_t duration;       // How long this color should be show in parts of 1/25s. So 25 will show the color 1s, before going to the next color.
+  uint8_t color[2];       // What color should be shown in RGB565 format.
+  unsigned leds:4;        // What led should be show, 0 equals all leds.
+  bool fade:1;            // Fade from previous colour to this colour during the full duration.
+  unsigned rotate:3;      // Speed of the rotation, number of seconds per revolution
+} ledtiming;
+
+typedef struct timings {
+  uint32_t hash;          // Hash will later be used to check if the contents is already uploaded, so we don't reupload
+  ledtiming timings[LEDRING_TIME_MEM_SIZE];
+} ledtimings;
+
+static uint8_t ledringmem[NBR_LEDS * 2];
+static ledtimings ledringtimingsmem;
+
+static bool isInit = false;
+
+// Memory handler for ledringmem
+static uint32_t handleLedringmemGetSize(void) { return sizeof(ledringmem); }
+static bool handleLedringmemRead(const uint32_t memAddr, const uint8_t readLen, uint8_t* buffer);
+static bool handleLedringmemWrite(const uint32_t memAddr, const uint8_t writeLen, const uint8_t* buffer);
+static const MemoryHandlerDef_t ledringmemDef = {
+  .type = MEM_TYPE_LED12,
+  .getSize = handleLedringmemGetSize,
+  .read = handleLedringmemRead,
+  .write = handleLedringmemWrite,
+};
+
+// Memory handler for timingmem
+static uint32_t handleTimingmemGetSize(void) { return sizeof(ledringtimingsmem); }
+static bool handleTimingmemRead(const uint32_t memAddr, const uint8_t readLen, uint8_t* buffer);
+static bool handleTimingmemWrite(const uint32_t memAddr, const uint8_t writeLen, const uint8_t* buffer);
+static const MemoryHandlerDef_t timingmemDef = {
+  .type = MEM_TYPE_LEDMEM,
+  .getSize = handleTimingmemGetSize,
+  .read = handleTimingmemRead,
+  .write = handleTimingmemWrite,
+};
+
+
+/*
+ * To add a new effect just add it as a static function with the prototype
+ * void effect(uint8_t buffer[][3], bool reset)
+ *
+ * Then add it to the effectsFct[] list bellow. It will automatically be
+ * activated using the ring.effect parameter.
+ *
+ * The ring color needs to be written in the buffer argument. The buffer is not
+ * modified in memory as long as reset is not 'true', see the spin effects for
+ * and example.
+ *
+ * The log subsystem can be used to get the value of any log variable of the
+ * system. See tiltEffect for an example.
+ */
+
+typedef void (*Ledring12Effect)(uint8_t buffer[][3], bool reset);
+
+/**************** Some useful macros ***************/
+
+#define RED {0x10, 0x00, 0x00}
+#define GREEN {0x00, 0x10, 0x00}
+#define BLUE {0x00, 0x00, 0x10}
+#define WHITE {0xff, 0xff, 0xff}
+#define BLACK {0x00, 0x00, 0x00}
+
+#define COPY_COLOR(dest, orig) dest[0]=orig[0]; dest[1]=orig[1]; dest[2]=orig[2]
+#define ADD_COLOR(dest, o1, o2) dest[0]=(o1[0]>>1)+(o2[0]>>1);dest[1]=(o1[1]>>1)+(o2[1]>>1);dest[2]=(o1[2]>>1)+(o2[2]>>1);
+#define LIMIT(a) ((a>255)?255:(a<0)?0:a)
+#define SIGN(a) ((a>=0)?1:-1)
+#define DEADBAND(a, b) ((a<b) ? 0:a)
+#define LINSCALE(domain_low, domain_high, codomain_low, codomain_high, value) ((codomain_high - codomain_low) / (domain_high - domain_low)) * (value - domain_low) + codomain_low
+#define SET_WHITE(dest, intensity) dest[0] = intensity; dest[1] = intensity; dest[2] = intensity;
+#define RGB565_TO_RGB888(dest, orig)                                           \
+  uint8_t R5, G6, B5;                                                          \
+  R5 = orig[0] >> 3;                                                           \
+  G6 = ((orig[0] & 0x07) << 3) | (orig[1] >> 5);                               \
+  B5 = orig[1] & 0x1F;                                                         \
+  dest[0] = ((uint16_t)R5 * 527 + 23) >> 6;                                    \
+  dest[1] = ((uint16_t)G6 * 259 + 33) >> 6;                                    \
+  dest[2] = ((uint16_t)B5 * 527 + 23) >> 6;
+
+#ifndef LEDRING_DEFAULT_EFFECT
+#define LEDRING_DEFAULT_EFFECT 6
+#endif
+
+#define LEDRING_TIME_MEM_SEC 1000 / 25
+
+static uint32_t effect = LEDRING_DEFAULT_EFFECT;
+static uint32_t neffect;
+static uint8_t headlightEnable = 0;
+static uint8_t black[][3] = {BLACK, BLACK, BLACK,
+                             BLACK, BLACK, BLACK,
+                             BLACK, BLACK, BLACK,
+                             BLACK, BLACK, BLACK,
+                            };
+
+static const uint8_t green[] = {0x00, 0xFF, 0x00};
+static const uint8_t red[] = {0xFF, 0x00, 0x00};
+static const uint8_t blue[] = {0x00, 0x00, 0xFF};
+static const uint8_t white[] = WHITE;
+static const uint8_t part_black[] = BLACK;
+
+/**************** Black (LEDs OFF) ***************/
+
+static void blackEffect(uint8_t buffer[][3], bool reset)
+{
+  int i;
+
+  if (reset)
+  {
+    for (i=0; i<NBR_LEDS; i++) {
+      buffer[i][0] = 0;
+      buffer[i][1] = 0;
+      buffer[i][2] = 0;
+    }
+  }
+}
+
+/**************** White spin ***************/
+#if NBR_LEDS > 12
+static const uint8_t whiteRing[NBR_LEDS][3] = {{32, 32, 32}, {8,8,8}, {2,2,2},
+                                       BLACK, BLACK, BLACK,
+                                       BLACK, BLACK, BLACK,
+                                       BLACK, BLACK, BLACK,
+                                      };
+#else
+static const uint8_t whiteRing[][3] = {{32, 32, 32}, {8,8,8}, {2,2,2},
+                                       BLACK, BLACK, BLACK,
+                                       BLACK, BLACK, BLACK,
+                                       BLACK, BLACK, BLACK,
+                                      };
+#endif
+
+#if NBR_LEDS > 12
+static const uint8_t blueRing[NBR_LEDS][3] = {{64, 64, 255}, {32,32,64}, {8,8,16},
+                                       BLACK, BLACK, BLACK,
+                                       BLACK, BLACK, BLACK,
+                                       BLACK, BLACK, BLACK,
+                                      };
+#else
+static const uint8_t blueRing[][3] = {{64, 64, 255}, {32,32,64}, {8,8,16},
+                                       BLACK, BLACK, BLACK,
+                                       BLACK, BLACK, BLACK,
+                                       BLACK, BLACK, BLACK,
+                                      };
+#endif
+
+// #if NBR_LEDS > 12
+// static const uint8_t greenRing[NBR_LEDS][3] = {{64, 255, 64}, {32,64,32}, {8,16,8},
+//                                       BLACK, BLACK, BLACK,
+//                                       BLACK, BLACK, BLACK,
+//                                       BLACK, BLACK, BLACK,
+//                                      };
+// #else
+// static const uint8_t greenRing[][3] = {{64, 255, 64}, {32,64,32}, {8,16,8},
+//                                       BLACK, BLACK, BLACK,
+//                                       BLACK, BLACK, BLACK,
+//                                       BLACK, BLACK, BLACK,
+//                                      };
+// #endif
+
+// #if NBR_LEDS > 12
+// static const uint8_t redRing[NBR_LEDS][3] = {{64, 0, 0}, {16,0,0}, {8,0,0},
+//                                       {4,0,0}, {2,0,0}, {1,0,0},
+//                                       BLACK, BLACK, BLACK,
+//                                       BLACK, BLACK, BLACK,
+//                                      };
+// #else
+// static const uint8_t redRing[][3] = {{64, 0, 0}, {16,0,0}, {8,0,0},
+//                                       {4,0,0}, {2,0,0}, {1,0,0},
+//                                       BLACK, BLACK, BLACK,
+//                                       BLACK, BLACK, BLACK,
+//                                      };
+// #endif
+
+static void whiteSpinEffect(uint8_t buffer[][3], bool reset)
+{
+  int i;
+  uint8_t temp[3];
+
+  if (reset)
+  {
+    for (i=0; i<NBR_LEDS; i++) {
+      COPY_COLOR(buffer[i], whiteRing[i]);
+    }
+  }
+
+  COPY_COLOR(temp, buffer[0]);
+  for (i=0; i<(NBR_LEDS-1); i++) {
+    COPY_COLOR(buffer[i], buffer[i+1]);
+  }
+  COPY_COLOR(buffer[(NBR_LEDS-1)], temp);
+}
+
+static uint8_t solidRed=20, solidGreen=20, solidBlue=20;
+static void solidColorEffect(uint8_t buffer[][3], bool reset)
+{
+  int i;
+  static float brightness=0;
+
+  if (reset) brightness = 0;
+
+  if (brightness<1) brightness += 0.05f;
+  else brightness = 1;
+
+  for (i=0; i<NBR_LEDS; i++)
+  {
+    buffer[i][0] = solidRed*brightness;
+    buffer[i][1] = solidGreen*brightness;
+    buffer[i][2] = solidBlue*brightness;
+  }
+}
+
+static void virtualMemEffect(uint8_t buffer[][3], bool reset)
+{
+  int i;
+
+  if (reset)
+  {
+    for (i=0; i<NBR_LEDS; i++) {
+      COPY_COLOR(buffer[i], part_black);
+    }
+  }
+
+  for (i = 0; i < NBR_LEDS; i++)
+  {
+    uint8_t R5, G6, B5;
+    uint8_t (*led)[2] = (uint8_t (*)[2])ledringmem;
+    // Convert from RGB565 to RGB888
+    R5 = led[i][0] >> 3;
+    G6 = ((led[i][0] & 0x07) << 3) | (led[i][1] >> 5);
+    B5 = led[i][1] & 0x1F;
+    buffer[i][0] = ((uint16_t)R5 * 527 + 23 ) >> 6;
+    buffer[i][1] = ((uint16_t)G6 * 259 + 33 ) >> 6;
+    buffer[i][2] = ((uint16_t)B5 * 527 + 23 ) >> 6;
+  }
+}
+
+static void boatEffect(uint8_t buffer[][3], bool reset)
+{
+  int i;
+
+  uint8_t reds[] = {1,2,3,4,5};
+  uint8_t greens[] = {7,8,9,10,11};
+  uint8_t whites[] = {0};
+  uint8_t blacks[] = {6};
+
+
+  for (i=0; i<sizeof(reds); i++)
+  {
+    COPY_COLOR(buffer[reds[i]], red);
+  }
+
+  for (i=0; i<sizeof(greens); i++)
+  {
+    COPY_COLOR(buffer[greens[i]], green);
+  }
+
+  for (i=0; i<sizeof(whites); i++)
+  {
+    COPY_COLOR(buffer[whites[i]], white);
+  }
+
+  for (i=0; i<sizeof(blacks); i++)
+  {
+    COPY_COLOR(buffer[blacks[i]], part_black);
+  }
+
+
+}
+
+/**************** Color spin ***************/
+
+#if NBR_LEDS > 12
+static const uint8_t colorRing[NBR_LEDS][3] = {{0,0,32}, {0,0,16}, {0,0,8},
+                                       {0,0,4}, {16,16,16}, {8,8,8},
+                                       {4,4,4},{32,0,0},{16,0,0},
+                                       {8,0,0}, {4,0,0}, {2,0,0},
+                                      };
+#else
+static const uint8_t colorRing[][3] = {{0,0,32}, {0,0,16}, {0,0,8},
+                                       {0,0,4}, {16,16,16}, {8,8,8},
+                                       {4,4,4},{32,0,0},{16,0,0},
+                                       {8,0,0}, {4,0,0}, {2,0,0},
+                                      };
+#endif
+
+static void colorSpinEffect(uint8_t buffer[][3], bool reset)
+{
+  int i;
+  uint8_t temp[3];
+
+  if (reset)
+  {
+    for (i=0; i<NBR_LEDS; i++) {
+      COPY_COLOR(buffer[i], colorRing[i]);
+    }
+  }
+
+  COPY_COLOR(temp, buffer[0]);
+  for (i=0; i<(NBR_LEDS-1); i++) {
+    COPY_COLOR(buffer[i], buffer[i+1]);
+  }
+  COPY_COLOR(buffer[(NBR_LEDS-1)], temp);
+}
+
+static void spinEffect2(uint8_t buffer[][3], bool reset)
+{
+  int i;
+  uint8_t temp[3];
+
+  if (reset)
+  {
+    for (i=0; i<NBR_LEDS; i++) {
+      COPY_COLOR(buffer[(NBR_LEDS-i)%NBR_LEDS], blueRing[i]);
+    }
+  }
+
+  COPY_COLOR(temp, buffer[(NBR_LEDS-1)]);
+  for (i=(NBR_LEDS-1); i>0; i--) {
+    COPY_COLOR(buffer[i], buffer[i-1]);
+  }
+  COPY_COLOR(buffer[0], temp);
+}
+
+static void doubleSpinEffect(uint8_t buffer[][3], bool reset) {
+  static uint8_t sub1[NBR_LEDS][3];
+  static uint8_t sub2[NBR_LEDS][3];
+  int i;
+  static int step;
+
+  if (reset) step = 0;
+
+  whiteSpinEffect(sub1, reset);
+  spinEffect2(sub2, reset);
+  //if ((step%3)) spinEffect2(sub2, false);
+  //if (reset) spinEffect2(sub2, true);
+
+  for (i=0; i<NBR_LEDS; i++)
+  {
+    ADD_COLOR(buffer[i], sub1[i], sub2[i]);
+  }
+
+  step ++;
+}
+
+/**************** Dynamic tilt effect ***************/
+
+static void tiltEffect(uint8_t buffer[][3], bool reset)
+{
+  static int pitchid, rollid, thrust=-1;
+
+  // 2014-12-28 chad: Reset LEDs to off to avoid color artifacts
+  // when switching from other effects.
+  if (reset)
+    {
+        int i;
+        for (i=0; i<NBR_LEDS; i++) {
+            buffer[i][0] = 0;
+            buffer[i][1] = 0;
+            buffer[i][2] = 0;
+        }
+    }
+
+
+  if (thrust<0) {
+    //Init
+    pitchid = logGetVarId("stabilizer", "pitch");
+    rollid = logGetVarId("stabilizer", "roll");
+    thrust = logGetVarId("stabilizer", "thrust");
+  } else {
+    const int led_middle = 10;
+    float pitch = -1*logGetFloat(pitchid);
+    float roll  = -1*logGetFloat(rollid);
+
+    pitch = (pitch>20)?20:(pitch<-20)?-20:pitch;
+    roll = (roll>20)?20:(roll<-20)?-20:roll;
+
+    pitch=SIGN(pitch)*pitch*pitch;
+    roll*=SIGN(roll)*roll;
+
+    buffer[11][0] = LIMIT(led_middle + pitch);
+    buffer[0][0] = LIMIT(led_middle + pitch);
+    buffer[1][0] = LIMIT(led_middle + pitch);
+
+    buffer[2][2] = LIMIT(led_middle - roll);
+    buffer[3][2] = LIMIT(led_middle - roll);
+    buffer[4][2] = LIMIT(led_middle - roll);
+
+    buffer[5][0] = LIMIT(led_middle - pitch);
+    buffer[6][0] = LIMIT(led_middle - pitch);
+    buffer[7][0] = LIMIT(led_middle - pitch);
+
+    buffer[8][2] = LIMIT(led_middle + roll);
+    buffer[9][2] = LIMIT(led_middle + roll);
+    buffer[10][2] = LIMIT(led_middle + roll);
+  }
+}
+
+
+/*************** Gravity light effect *******************/
+
+static float gravityLightCalculateAngle(float pitch, float roll);
+static void gravityLightRender(uint8_t buffer[][3], float led_index, int intensity);
+
+static void gravityLight(uint8_t buffer[][3], bool reset)
+{
+  static int pitchid, rollid;
+  static bool isInitialized = false;
+
+  if (!isInitialized) {
+    pitchid = logGetVarId("stabilizer", "pitch");
+    rollid = logGetVarId("stabilizer", "roll");
+    isInitialized = true;
+  }
+
+  float pitch = logGetFloat(pitchid); // -180 to 180
+  float roll = logGetFloat(rollid); // -180 to 180
+
+  float angle = gravityLightCalculateAngle(pitch, roll);
+  float led_index = NBR_LEDS * angle / (2 * (float) M_PI);
+  int intensity = LIMIT(sqrtf(pitch * pitch + roll * roll));
+  gravityLightRender(buffer, led_index, intensity);
+}
+
+static float gravityLightCalculateAngle(float pitch, float roll) {
+  float angle = 0.0;
+
+  if (roll != 0) {
+    angle = atanf(pitch / roll) + (float) M_PI_2;
+
+    if (roll < 0.0f) {
+      angle += (float) M_PI;
+    }
+  }
+
+  return angle;
+}
+
+static void gravityLightRender(uint8_t buffer[][3], float led_index, int intensity) {
+  float width = 5;
+  float height = intensity;
+
+  int i;
+  for (i = 0; i < NBR_LEDS; i++) {
+	float distance = fabsf(led_index - i);
+	if (distance > NBR_LEDS / 2) {
+		distance = NBR_LEDS - distance;
+	}
+
+	int col = height - distance * (height / (width / 2));
+	SET_WHITE(buffer[i], LIMIT(col));
+  }
+}
+
+
+/*************** Brightness effect ********************/
+
+#define MAX_RATE 512
+
+static void brightnessEffect(uint8_t buffer[][3], bool reset)
+{
+
+  static int gyroYid, gyroZid, gyroXid =- 1;
+  static uint8_t brightness = 0;
+
+  if (gyroXid < 0)
+  {
+    //Init
+    gyroXid = logGetVarId("gyro", "x");
+    gyroYid = logGetVarId("gyro", "y");
+    gyroZid = logGetVarId("gyro", "z");
+  }
+  else
+  {
+    int i;
+    int gyroX = (int)logGetFloat(gyroXid);
+    int gyroY = (int)logGetFloat(gyroYid);
+    int gyroZ = (int)logGetFloat(gyroZid);
+
+    // Adjust to interval
+    gyroX = (gyroX>MAX_RATE) ? MAX_RATE:(gyroX<-MAX_RATE) ? -MAX_RATE:gyroX;
+    gyroY = (gyroY>MAX_RATE) ? MAX_RATE:(gyroY<-MAX_RATE) ? -MAX_RATE:gyroY;
+    gyroZ = (gyroZ>MAX_RATE) ? MAX_RATE:(gyroZ<-MAX_RATE) ? -MAX_RATE:gyroZ;
+
+    gyroX = SIGN(gyroX) * gyroX / 2;
+    gyroY = SIGN(gyroY) * gyroY / 2;
+    gyroZ = SIGN(gyroZ) * gyroZ / 2;
+
+    gyroX = DEADBAND(gyroX, 5);
+    gyroY = DEADBAND(gyroY, 5);
+    gyroZ = DEADBAND(gyroZ, 5);
+
+    for (i=0; i < NBR_LEDS; i++)
+    {
+      buffer[i][0] = (uint8_t)(LIMIT(gyroZ));
+      buffer[i][1] = (uint8_t)(LIMIT(gyroY));
+      buffer[i][2] = (uint8_t)(LIMIT(gyroX));
+    }
+
+    brightness++;
+  }
+}
+
+
+static void setHeadlightsOn(bool on)
+{
+  if (on)
+    GPIO_SetBits(GPIOB, GPIO_Pin_4);
+  else
+    GPIO_ResetBits(GPIOB, GPIO_Pin_4);
+}
+
+
+/* LED-ring test effect */
+#define TEST_INTENTS 20
+static uint8_t test_pat[3][3] = {{TEST_INTENTS, 0, 0}, {0, TEST_INTENTS, 0}, {0, 0, TEST_INTENTS}};
+static uint8_t test_eff_nbr = 0;
+#define TEST_DELAY 4
+static uint8_t test_delay_counter = 0;
+static uint8_t headlight_test_counter =0;
+static uint8_t test_front = false;
+static void ledTestEffect(uint8_t buffer[][3], bool reset)
+{
+  int i;
+  static float brightness=0;
+
+  if (reset) brightness = 0;
+
+  if (brightness<1) brightness += 0.05f;
+  else brightness = 1;
+
+  for (i=0; i<NBR_LEDS; i++)
+  {
+    buffer[i][0] = test_pat[test_eff_nbr][0];
+    buffer[i][1] = test_pat[test_eff_nbr][1];
+    buffer[i][2] = test_pat[test_eff_nbr][2];
+  }
+
+  test_delay_counter++;
+  headlight_test_counter++;
+
+  if (test_delay_counter > TEST_DELAY) {
+    test_delay_counter = 0;
+    test_eff_nbr = (test_eff_nbr + 1) % 3;
+  }
+
+  if (headlight_test_counter > (TEST_DELAY*3)) {
+    headlight_test_counter = 0;
+    test_front = !test_front;
+    headlightEnable = test_front;
+  }
+}
+
+/**
+ * An effect that shows the battery charge on the LED ring.
+ *
+ * Red means empty, blue means full.
+ */
+static float emptyCharge = 3.1, fullCharge = 4.2;
+static void batteryChargeEffect(uint8_t buffer[][3], bool reset)
+{
+  int i;
+  static int vbatid;
+  float vbat;
+
+  vbatid = logGetVarId("pm", "vbat");
+  vbat = logGetFloat(vbatid);
+
+  for (i = 0; i < NBR_LEDS; i++) {
+    buffer[i][0] = LIMIT(LINSCALE(emptyCharge, fullCharge, 255, 0, vbat)); // Red (emtpy)
+    buffer[i][1] = 0; // Green
+    buffer[i][2] = LIMIT(LINSCALE(emptyCharge, fullCharge, 0, 255, vbat)); // Blue (charged)
+  }
+}
+
+/**
+ * An effect mimicking a blue light siren
+ */
+static void siren(uint8_t buffer[][3], bool reset)
+{
+  int i;
+  static int tic = 0;
+
+  if (reset)
+  {
+    for (i=0; i<NBR_LEDS; i++) {
+      COPY_COLOR(buffer[i], part_black);
+    }
+  }
+
+  if ((tic < 10) && (tic & 1))
+  {
+    for (i=0; i<NBR_LEDS; i++) {
+      COPY_COLOR(buffer[i], blue);
+    }
+  }
+  else
+  {
+    for (i=0; i<NBR_LEDS; i++) {
+      COPY_COLOR(buffer[i], part_black);
+    }
+  }
+  if (++tic >= 20) tic = 0;
+}
+
+/**
+ * Display a solid color and fade to the next one in a given time
+ */
+static uint32_t fadeColor = 0;
+static float fadeTime = 0.5;
+
+static float currentFadeTime = 0.5;
+
+#include "log.h"
+
+/**
+ * The logs for the LED ring expansion deck contains two powerful front-facing white LEDs
+ * and 12 bottom-facing RGB individually addressable LEDs (it uses the same
+ * LEDs as used in the NeoPixel products by Adafruit).
+ *
+ * The deck is designed to be installed as the last deck on the bottom of the
+ * quad. It does not have pass-through holes for the expansion port connector.
+ */
+LOG_GROUP_START(ring)
+/**
+ * @brief Current fade time of fade color effect
+ */
+LOG_ADD(LOG_FLOAT, fadeTime, &currentFadeTime)
+LOG_GROUP_STOP(ring)
+
+static void fadeColorEffect(uint8_t buffer[][3], bool reset)
+{
+  static float currentRed = 255;
+  static float currentGreen = 255;
+  static float currentBlue = 255;
+  static float targetRed, targetGreen, targetBlue;
+  static uint32_t previousTargetColor = 0xffffffff;
+  static float cachedFadeTime = 0.5;
+
+  if (fadeColor != previousTargetColor) {
+    float alpha = currentFadeTime / cachedFadeTime;
+
+    currentRed = (alpha * currentRed) + ((1 - alpha) * targetRed);
+    currentGreen = (alpha * currentGreen) + ((1 - alpha) * targetGreen);
+    currentBlue = (alpha * currentBlue) + ((1 - alpha) * targetBlue);
+
+    currentFadeTime = fadeTime;
+    cachedFadeTime = fadeTime;
+    targetRed = (fadeColor >> 16) & 0x0FF;
+    targetGreen = (fadeColor >> 8) & 0x0FF;
+    targetBlue = (fadeColor >> 0) & 0x0FF;
+
+    previousTargetColor = fadeColor;
+  }
+
+  if (currentFadeTime > 0)
+  {
+    float alpha = currentFadeTime / cachedFadeTime;
+
+    int red = (alpha * currentRed) + ((1-alpha) * targetRed);
+    int green = (alpha * currentGreen) + ((1 - alpha) * targetGreen);
+    int blue = (alpha * currentBlue) + ((1 - alpha) * targetBlue);
+
+    for (int i = 0; i < NBR_LEDS; i++)
+    {
+      buffer[i][0] = red;
+      buffer[i][1] = green;
+      buffer[i][2] = blue;
+    }
+
+    currentFadeTime -= 50e-3f;
+  } else {
+    currentFadeTime = 0;
+    currentRed = (fadeColor >> 16) & 0x0FF;
+    currentGreen = (fadeColor >> 8) & 0x0FF;
+    currentBlue = (fadeColor >> 0) & 0x0FF;
+
+    for (int i = 0; i < NBR_LEDS; i++)
+    {
+      buffer[i][0] = currentRed;
+      buffer[i][1] = currentGreen;
+      buffer[i][2] = currentBlue;
+    }
+  }
+}
+
+/**
+ * An effect that shows the Signal Strength (RSSI) on the LED ring.
+ *
+ * Red means bad, green means good.
+ */
+static float badRssi = 85, goodRssi = 35;
+static void rssiEffect(uint8_t buffer[][3], bool reset)
+{
+  int i;
+  static int isConnectedId, rssiId;
+  float rssi;
+  bool isConnected;
+
+  isConnectedId = logGetVarId("radio", "isConnected");
+  isConnected = logGetUint(isConnectedId);
+
+  rssiId = logGetVarId("radio", "rssi");
+  rssi = logGetFloat(rssiId);
+  uint8_t rssi_scaled = LIMIT(LINSCALE(badRssi, goodRssi, 0, 255, rssi));
+
+  for (i = 0; i < NBR_LEDS; i++) {
+    if (isConnected) {
+      buffer[i][0] = 255 - rssi_scaled; // Red (bad)
+      buffer[i][1] = rssi_scaled; // Green (good)
+      buffer[i][2] = 0; // Blue
+    } else {
+      buffer[i][0] = 100; // Red
+      buffer[i][1] = 100; // Green
+      buffer[i][2] = 100; // Blue
+    }
+  }
+}
+
+/**
+ * An effect that shows the status of the lighthouse.
+ *
+ * Red means 0 angles, green means 16 angles (2 basestations x 4 crazyflie sensors x 2 sweeping directions).
+ */
+static void lighthouseEffect(uint8_t buffer[][3], bool reset)
+{
+  uint16_t validAngles = pulseProcessorAnglesQuality();
+
+  for (int i = 0; i < NBR_LEDS; i++) {
+    buffer[i][0] = LIMIT(LINSCALE(0.0f, 255.0f, 100.0f, 0.0f, validAngles)); // Red (small validAngles)
+    buffer[i][1] = LIMIT(LINSCALE(0.0f, 255.0f, 0.0f, 100.0f, validAngles)); // Green (large validAngles)
+    buffer[i][2] = 0;
+  }
+}
+
+/**
+ * An effect that shows the status of the location service.
+ *
+ * Red means bad, green means good.
+ * Blinking means battery was low during flight.
+ */
+static void locSrvStatus(uint8_t buffer[][3], bool reset)
+{
+  static int locSrvTickId = -1;
+  static int pmStateId = -1;
+
+  static int tic = 0;
+  static bool batteryEverLow = false;
+
+  // lazy initialization of the logging variables
+  if (locSrvTickId == -1) {
+    locSrvTickId = logGetVarId("locSrvZ", "tick");
+    pmStateId = logGetVarId("pm", "state");
+  }
+
+  // compute time since the last update in milliseconds
+  uint16_t time_since_last_update = xTaskGetTickCount() - logGetUint(locSrvTickId);
+  if (time_since_last_update > 30) {
+    time_since_last_update = 30;
+  }
+
+  int8_t pmstate = logGetInt(pmStateId);
+  if (pmstate == lowPower) {
+    batteryEverLow = true;
+  }
+
+  for (int i = 0; i < NBR_LEDS; i++) {
+    if (batteryEverLow && tic < 10) {
+      buffer[i][0] = 0;
+      buffer[i][1] = 0;
+    } else {
+      buffer[i][0] = LIMIT(LINSCALE(0, 30, 0, 100, time_since_last_update)); // Red (large time_since_last_update)
+      buffer[i][1] = LIMIT(LINSCALE(0, 30, 100, 0, time_since_last_update)); // Green (small time_since_last_update)
+    }
+    buffer[i][2] = 0;
+  }
+
+  if (++tic >= 20) {
+    tic = 0;
+  }
+}
+
+static bool isTimeMemDone(ledtiming current)
+{
+  return current.duration == 0 && current.color[0] == 0 &&
+         current.color[1] == 0;
+}
+
+static int timeEffectI = 0;
+static uint64_t timeEffectTime = 0;
+static uint8_t timeEffectPrevBuffer[NBR_LEDS][3];
+static float timeEffectRotation = 0;
+
+static void timeMemEffect(uint8_t outputBuffer[][3], bool reset)
+{
+  // Start timer when going to this
+  if (reset) {
+    for (int i = 0; i < NBR_LEDS; i++) {
+      COPY_COLOR(timeEffectPrevBuffer[i], part_black);
+      COPY_COLOR(outputBuffer[i], part_black);
+    }
+
+    timeEffectRotation = 0;
+    timeEffectTime = usecTimestamp() / 1000;
+    timeEffectI = 0;
+  }
+
+  ledtiming current = ledringtimingsmem.timings[timeEffectI];
+
+  // Stop when completed
+  if (isTimeMemDone(current))
+    return;
+
+  // Get the proper index
+  uint64_t time = usecTimestamp() / 1000;
+  while (timeEffectTime + LEDRING_TIME_MEM_SEC * current.duration < time) {
+    // Apply previous commands to the cache
+    uint8_t color[3];
+    RGB565_TO_RGB888(color, current.color)
+
+    if (current.leds == 0) {
+      for (int i = 0; i < NBR_LEDS; i++) {
+        COPY_COLOR(timeEffectPrevBuffer[i], color);
+      }
+    } else {
+      COPY_COLOR(timeEffectPrevBuffer[current.leds], color);
+    }
+
+    // Goto next effect
+    if(current.rotate)
+      timeEffectRotation += 1.0f * current.duration * LEDRING_TIME_MEM_SEC / (current.rotate * 1000);
+    timeEffectTime += LEDRING_TIME_MEM_SEC * current.duration;
+    timeEffectI++;
+    current = ledringtimingsmem.timings[timeEffectI];
+
+    if (isTimeMemDone(current))
+      return;
+  }
+
+  // Apply the current effect
+  uint8_t color[3];
+  RGB565_TO_RGB888(color, current.color)
+  uint8_t currentBuffer[NBR_LEDS][3];
+  for (int i = 0; i < NBR_LEDS; i++) {
+    COPY_COLOR(currentBuffer[i], timeEffectPrevBuffer[i]);
+  }
+
+  if (current.fade) {
+    float percent = 1.0 * (time - timeEffectTime) / (current.duration * LEDRING_TIME_MEM_SEC);
+    if (current.leds == 0)
+      for (int i = 0; i < NBR_LEDS; i++)
+        for (int j = 0; j < 3; j++)
+          currentBuffer[i][j] = (1.0f - percent) * timeEffectPrevBuffer[i][j] + percent * color[j];
+    else
+      for (int j = 0; j < 3; j++)
+        currentBuffer[current.leds][j] = (1.0f - percent) * timeEffectPrevBuffer[current.leds][j] + percent * color[j];
+  }
+  else {
+    if (current.leds == 0) {
+      for (int i = 0; i < NBR_LEDS; i++) {
+        COPY_COLOR(currentBuffer[i], color);
+      }
+    } else {
+      COPY_COLOR(currentBuffer[current.leds], color);
+    }
+  }
+
+  float rotate = timeEffectRotation;
+  if(current.rotate) {
+    rotate += 1.0f * (time - timeEffectTime) / (current.rotate * 1000);
+  }
+
+  int shift = rotate * NBR_LEDS;
+  float percentShift = rotate * NBR_LEDS - shift;
+  shift = shift % NBR_LEDS;
+
+  // Output current leds
+  for (int i = 0; i < NBR_LEDS; i++)
+    for (int j = 0; j < 3; j++)
+      outputBuffer[(i+shift) % NBR_LEDS][j] =
+        percentShift * currentBuffer[i][j] +
+        (1-percentShift) * currentBuffer[(i+1) % NBR_LEDS][j];
+}
+
+/**************** Effect list ***************/
+
+
+Ledring12Effect effectsFct[] =
+{
+  blackEffect,
+  whiteSpinEffect,
+  colorSpinEffect,
+  tiltEffect,
+  brightnessEffect,
+  spinEffect2,
+  doubleSpinEffect,
+  solidColorEffect,
+  ledTestEffect,
+  batteryChargeEffect,
+  boatEffect,
+  siren,
+  gravityLight,
+  virtualMemEffect,
+  fadeColorEffect,
+  rssiEffect,
+  locSrvStatus,
+  timeMemEffect,
+  lighthouseEffect,
+};
+
+/********** Light signal overriding **********/
+
+static struct {
+  uint8_t trigger;
+  uint32_t triggeredAt;
+  uint8_t active;
+} lightSignal = { 0, 0, 0 };
+
+static void checkLightSignalTrigger(void)
+{
+  if (lightSignal.trigger)
+  {
+    lightSignal.active = 1;
+    lightSignal.triggeredAt = xTaskGetTickCount();
+    lightSignal.trigger = 0;
+  }
+}
+
+static void overrideWithLightSignal(uint8_t buffer[][3])
+{
+  uint8_t color;
+  uint32_t diffMsec;
+
+  if (lightSignal.active) {
+    diffMsec = T2M(xTaskGetTickCount() - lightSignal.triggeredAt);
+
+    if (diffMsec >= 1500) {
+      lightSignal.active = 0;
+      lightSignal.triggeredAt = 0;
+      color = 0;
+    } else {
+      diffMsec = diffMsec % 300;
+      color = (diffMsec <= 100) ? 255 : 0;
+    }
+
+    memset(buffer, color, NBR_LEDS * 3);
+  }
+}
+
+/********** Ring init and switching **********/
+static xTimerHandle timer;
+
+
+
+void ledring12Worker(void * data)
+{
+  static int current_effect = 0;
+  static uint8_t buffer[NBR_LEDS][3];
+  bool reset = true;
+
+  if (/*!pmIsDischarging() ||*/ (effect > neffect)) {
+    ws2812Send(black, NBR_LEDS);
+    return;
+  }
+
+  if (current_effect != effect) {
+    reset = true;
+  } else {
+    reset = false;
+  }
+  current_effect = effect;
+
+  effectsFct[current_effect](buffer, reset);
+  overrideWithLightSignal(buffer);
+  ws2812Send(buffer, NBR_LEDS);
+}
+
+static void ledring12Timer(xTimerHandle timer)
+{
+  workerSchedule(ledring12Worker, NULL);
+
+  setHeadlightsOn(headlightEnable);
+  checkLightSignalTrigger();
+}
+
+static void ledring12Init(DeckInfo *info)
+{
+  if (isInit) {
+    return;
+  }
+
+  GPIO_InitTypeDef GPIO_InitStructure;
+
+  ws2812Init();
+
+  neffect = sizeof(effectsFct)/sizeof(effectsFct[0])-1;
+
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
+  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+  GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
+  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;
+  GPIO_Init(GPIOB, &GPIO_InitStructure);
+
+  memoryRegisterHandler(&ledringmemDef);
+  memoryRegisterHandler(&timingmemDef);
+
+  isInit = true;
+
+  timer = xTimerCreate( "ringTimer", M2T(50),
+                                     pdTRUE, NULL, ledring12Timer );
+  xTimerStart(timer, 100);
+}
+
+static bool handleLedringmemRead(const uint32_t memAddr, const uint8_t readLen, uint8_t* buffer) {
+  bool result = false;
+
+  if (memAddr + readLen <= sizeof(ledringmem)) {
+      if (memcpy(buffer, &(ledringmem[memAddr]), readLen)) {
+          result = true;
+      }
+  }
+
+  return result;
+}
+
+static bool handleLedringmemWrite(const uint32_t memAddr, const uint8_t writeLen, const uint8_t* buffer) {
+  bool result = false;
+
+  if ((memAddr + writeLen) <= sizeof(ledringmem)) {
+    memcpy(&(ledringmem[memAddr]), buffer, writeLen);
+    result = true;
+  }
+
+  return result;
+}
+
+static bool handleTimingmemRead(const uint32_t memAddr, const uint8_t readLen, uint8_t* buffer) {
+  bool result = false;
+
+  if (memAddr + readLen <= sizeof(ledringtimingsmem.timings)) {
+    uint8_t* mem = (uint8_t*) &ledringtimingsmem.timings;
+    memcpy(buffer, mem + memAddr, readLen);
+    result = true;
+  }
+
+  return result;
+}
+
+static bool handleTimingmemWrite(const uint32_t memAddr, const uint8_t writeLen, const uint8_t* buffer) {
+  bool result = false;
+
+  if ((memAddr + writeLen) <= sizeof(ledringtimingsmem.timings)) {
+    uint8_t* mem = (uint8_t*) &ledringtimingsmem.timings;
+    memcpy(mem+memAddr, buffer, writeLen);
+    result = true;
+  }
+
+  return result;
+}
+
+/**
+ * The LED ring expansion deck contains two powerful front-facing white LEDs
+ * and 12 bottom-facing RGB individually addressable LEDs (it uses the same
+ * LEDs as used in the NeoPixel products by Adafruit).
+ *
+ * The deck is designed to be installed as the last deck on the bottom of the
+ * quad. It does not have pass-through holes for the expansion port connector.
+ */
+PARAM_GROUP_START(ring)
+
+/**
+ * @brief Id of effect to use (default: 6)
+ *
+ * | Id | Effect              | \n
+ * | -  | -                   | \n
+ * | 0  | Off                 | \n
+ * | 1  | White spinner       | \n
+ * | 2  | Color spinner       | \n
+ * | 3  | Tilt                | \n
+ * | 4  | Brightness          | \n
+ * | 5  | Color spinner 2     | \n
+ * | 6  | Double spinner      | \n
+ * | 7  | Solid color effect  | \n
+ * | 8  | Factory test        | \n
+ * | 9  | Battery status      | \n
+ * | 10 | Boat lights         | \n
+ * | 11 | Alert               | \n
+ * | 12 | Gravity             | \n
+ */
+PARAM_ADD_CORE(PARAM_UINT8, effect, &effect)
+
+/**
+ * @brief Number of effects available
+ */
+PARAM_ADD_CORE(PARAM_UINT32 | PARAM_RONLY, neffect, &neffect)
+
+/**
+ * @brief Intensity of Red for Solid color effect (default: 20)
+ */
+PARAM_ADD_CORE(PARAM_UINT8, solidRed, &solidRed)
+
+/**
+ * @brief Intensity of Green for solid color effect (default: 20)
+ */
+PARAM_ADD_CORE(PARAM_UINT8, solidGreen, &solidGreen)
+
+/**
+ * @brief Intensity of Blue for solid color effect (default: 20)
+ */
+PARAM_ADD_CORE(PARAM_UINT8, solidBlue, &solidBlue)
+
+/**
+ * @brief Nonzero to Enable headlights (default: 0)
+ */
+PARAM_ADD_CORE(PARAM_UINT8, headlightEnable, &headlightEnable)
+
+/**
+ * @brief At what volt the Battery effect indicates empty
+ */
+PARAM_ADD_CORE(PARAM_FLOAT, emptyCharge, &emptyCharge)
+
+
+/**
+ * @brief At what volt the battery effect indicates full
+ */
+PARAM_ADD_CORE(PARAM_FLOAT, fullCharge, &fullCharge)
+
+/**
+ * @brief Color to fade to for Fade color effect
+ *
+ * Encoded as:
+ *
+ *   bit 32                                 0
+ *        00000000 RRRRRRRR GGGGGGGG BBBBBBBB
+ */
+PARAM_ADD_CORE(PARAM_UINT32, fadeColor, &fadeColor)
+
+/**
+ * @brief The time for face effect to complete
+ */
+PARAM_ADD_CORE(PARAM_FLOAT, fadeTime, &fadeTime)
+
+PARAM_GROUP_STOP(ring)
+
+PARAM_GROUP_START(system)
+
+/**
+ * @brief Highlight quad
+ *
+ * Uses functionality available, such as LEDs to highlight a quad, useful for
+ * swarms.
+ */
+PARAM_ADD_CORE(PARAM_UINT8, highlight, &lightSignal.trigger)
+PARAM_GROUP_STOP(system)
+
+static const DeckDriver ledring12_deck = {
+  .vid = 0xBC,
+  .pid = 0x01,
+  .name = "bcLedRing",
+
+  .usedPeriph = DECK_USING_TIMER3,
+  .usedGpio = DECK_USING_IO_2 | DECK_USING_IO_3,
+
+  .init = ledring12Init,
+};
+
+DECK_DRIVER(ledring12_deck);
+
+PARAM_GROUP_START(deck)
+
+/**
+ * @brief Nonzero if [LED-ring deck](%https://store.bitcraze.io/collections/decks/products/led-ring-deck) is attached
+ */
+PARAM_ADD_CORE(PARAM_UINT8 | PARAM_RONLY, bcLedRing, &isInit)
+
+PARAM_GROUP_STOP(deck)

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/lhtesterdeck.c

+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2018 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/>.
+ *
+ * lhtesterdeck.c - Deck driver for the Lighthouse tester deck.
+ *
+ * Used in production tests of Lighthouse decks.
+ *
+ * The deck has an IR LED that is used to test the receivers on Lighthouse decks.
+ * The LED is modulated at > 1 MHz and pulsed on/off.
+ *
+ */
+
+#include <stdint.h>
+#include <stdlib.h>
+#include "stm32fxxx.h"
+#include "FreeRTOS.h"
+#include "timers.h"
+
+
+#include "param.h"
+#include "deck.h"
+
+static bool isInit;
+
+#define TIM_PERIF       RCC_APB1Periph_TIM5
+#define TIM             TIM5
+#define TIM_DBG         DBGMCU_TIM5_STOP
+#define TIM_SETCOMPARE  TIM_SetCompare2
+#define TIM_GETCAPTURE  TIM_GetCapture2
+
+#define GPIO_POS_PERIF         RCC_AHB1Periph_GPIOA
+#define GPIO_POS_PORT          GPIOA
+#define GPIO_POS_PIN           GPIO_Pin_2 // TIM5_CH3
+#define GPIO_AF_POS_PIN        GPIO_PinSource2
+#define GPIO_AF_POS            GPIO_AF_TIM5
+
+#define PERIOD 0x40
+
+static void ledOnOff(bool on)
+{
+  uint8_t ratio = 0;
+  if (on) {
+    ratio = PERIOD / 2;
+  }
+
+  TIM_SetCompare3(TIM, ratio);
+  TIM_SetCompare4(TIM, ratio);
+}
+
+static void timerFcn(xTimerHandle xTimer)
+{
+  static int onCounter = 0;
+
+  bool isOn = (onCounter == 0);
+  ledOnOff(isOn);
+
+  onCounter++;
+  if (onCounter > 10) {
+    onCounter = 0;
+  }
+}
+
+static void startSwTimer() {
+  xTimerHandle timer = xTimerCreate("lhTesterTimer", M2T(1), pdTRUE, 0, timerFcn);
+  xTimerStart(timer, 0);
+}
+
+static void setUpHwTimer(){
+  //Init structures
+  GPIO_InitTypeDef GPIO_InitStructure;
+  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
+  TIM_OCInitTypeDef  TIM_OCInitStructure;
+
+  //Clock the gpio and the timers
+  RCC_AHB1PeriphClockCmd(GPIO_POS_PERIF, ENABLE);
+  RCC_APB1PeriphClockCmd(TIM_PERIF, ENABLE);
+
+  // Configure the GPIO for the timer output
+  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
+  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+  GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_DOWN;
+  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
+  GPIO_InitStructure.GPIO_Pin = GPIO_POS_PIN;
+  GPIO_Init(GPIO_POS_PORT, &GPIO_InitStructure);
+
+  //Map timers to alternate functions
+  GPIO_PinAFConfig(GPIO_POS_PORT, GPIO_AF_POS_PIN, GPIO_AF_POS);
+
+  //Timer configuration
+  TIM_TimeBaseStructure.TIM_Period = PERIOD - 1;
+  TIM_TimeBaseStructure.TIM_Prescaler = 0;
+  TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
+  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+  TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
+  TIM_TimeBaseInit(TIM, &TIM_TimeBaseStructure);
+
+  // PWM channels configuration
+  TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
+  TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
+  TIM_OCInitStructure.TIM_Pulse = 0;
+  TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
+  TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
+
+  // Configure OC3
+  TIM_OC3Init(TIM, &TIM_OCInitStructure);
+  TIM_OC3PreloadConfig(TIM, TIM_OCPreload_Enable);
+
+  //Enable the timer PWM outputs
+  TIM_CtrlPWMOutputs(TIM, ENABLE);
+  TIM_SetCompare3(TIM, 0x00);
+  TIM_SetCompare4(TIM, 0x00);
+
+  //Enable the timer
+  TIM_Cmd(TIM, ENABLE);
+}
+
+
+static void lhTesterDeckInit(DeckInfo *info)
+{
+  if (isInit) {
+    return;
+  }
+
+  // HW timer for modulating the LED with > 1MHz
+  setUpHwTimer();
+
+  // SW timer for turning the modulated light on and off with a 20 ms cycle
+  startSwTimer();
+
+  isInit = true;
+}
+
+static const DeckDriver lhTester_deck = {
+  .vid = 0x00,
+  .pid = 0x00,
+  .name = "bcLhTester",
+
+  .usedPeriph = DECK_USING_TIMER5,
+  .usedGpio = DECK_USING_TX2 | DECK_USING_RX2,
+
+  .init = lhTesterDeckInit,
+};
+
+DECK_DRIVER(lhTester_deck);
+
+PARAM_GROUP_START(deck)
+PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, bcLhTester, &isInit)
+PARAM_GROUP_STOP(deck)

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/lighthouse.c

+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2018-2020 Bitcraze AB
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * lighthouse.c: lighthouse tracking system receiver
+ */
+
+#include "deck.h"
+#include "param.h"
+
+#include "stm32fxxx.h"
+#include "config.h"
+#include "FreeRTOS.h"
+#include "task.h"
+#include "timers.h"
+
+#include "lighthouse.h"
+#include "lighthouse_core.h"
+#include "lighthouse_deck_flasher.h"
+
+// LED timer
+static StaticTimer_t timerBuffer;
+#define FIFTH_SECOND 200
+static void ledTimerHandle(xTimerHandle timer);
+
+static bool isInit = false;
+// lighthouseBaseStationsGeometry has been moved to lighthouse_core.c
+
+static void lighthouseInit(DeckInfo *info)
+{
+  if (isInit) {
+    return;
+  }
+
+  lighthouseCoreInit();
+
+  xTaskCreate(lighthouseCoreTask, LIGHTHOUSE_TASK_NAME,
+              2*configMINIMAL_STACK_SIZE, NULL, LIGHTHOUSE_TASK_PRI, NULL);
+
+  xTimerHandle timer;
+  timer = xTimerCreateStatic("ledTimer", M2T(FIFTH_SECOND), pdTRUE,
+    NULL, ledTimerHandle, &timerBuffer);
+  xTimerStart(timer, M2T(0));
+
+  isInit = true;
+}
+
+static void ledTimerHandle(xTimerHandle timer) {
+  lighthouseCoreLedTimer();
+}
+
+static const DeckMemDef_t memoryDef = {
+  .write = lighthouseDeckFlasherWrite,
+  .read = lighthouseDeckFlasherRead,
+  .properties = lighthouseDeckFlasherPropertiesQuery,
+  .supportsUpgrade = true,
+
+  .requiredSize = LIGHTHOUSE_BITSTREAM_SIZE,
+  .requiredHash = LIGHTHOUSE_BITSTREAM_CRC,
+};
+
+static const DeckDriver lighthouse_deck = {
+  .vid = 0xBC,
+  .pid = 0x10,
+  .name = "bcLighthouse4",
+
+  .usedGpio = 0,  // FIXME: set the used pins
+  .requiredEstimator = kalmanEstimator,
+
+  .memoryDef = &memoryDef,
+
+  .init = lighthouseInit,
+};
+
+
+DECK_DRIVER(lighthouse_deck);
+
+PARAM_GROUP_START(deck)
+
+/**
+ * @brief Nonzero if [Lighthouse positioning deck](%https://store.bitcraze.io/collections/decks/products/lighthouse-positioning-deck) is attached
+ */
+PARAM_ADD_CORE(PARAM_UINT8 | PARAM_RONLY, bcLighthouse4, &isInit)
+
+PARAM_GROUP_STOP(deck)

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/locodeck.c

+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2016-2021 Bitcraze AB
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * locodeck.c: Dwm1000 deck driver.
+ */
+
+#define DEBUG_MODULE "DWM"
+
+#include <stdint.h>
+#include <string.h>
+#include "stm32fxxx.h"
+
+#include "FreeRTOS.h"
+#include "semphr.h"
+#include "task.h"
+#include "queue.h"
+
+#include "deck.h"
+#include "system.h"
+#include "debug.h"
+#include "log.h"
+#include "param.h"
+#include "nvicconf.h"
+#include "estimator.h"
+#include "statsCnt.h"
+#include "mem.h"
+
+#include "locodeck.h"
+
+#include "lpsTdoa2Tag.h"
+#include "lpsTdoa3Tag.h"
+#include "lpsTwrTag.h"
+
+
+#define CS_PIN DECK_GPIO_IO1
+
+// LOCO deck alternative IRQ and RESET pins(IO_2, IO_3) instead of default (RX1, TX1), leaving UART1 free for use
+#ifdef LOCODECK_USE_ALT_PINS
+  #define GPIO_PIN_IRQ 	  DECK_GPIO_IO2
+
+  #ifndef LOCODECK_ALT_PIN_RESET
+  #define GPIO_PIN_RESET 	DECK_GPIO_IO3
+  #else
+  #define GPIO_PIN_RESET 	LOCODECK_ALT_PIN_RESET
+  #endif
+
+  #define EXTI_PortSource EXTI_PortSourceGPIOB
+  #define EXTI_PinSource 	EXTI_PinSource5
+  #define EXTI_LineN 		  EXTI_Line5
+#else
+  #define GPIO_PIN_IRQ 	  DECK_GPIO_RX1
+  #define GPIO_PIN_RESET 	DECK_GPIO_TX1
+  #define EXTI_PortSource EXTI_PortSourceGPIOC
+  #define EXTI_PinSource 	EXTI_PinSource11
+  #define EXTI_LineN 		  EXTI_Line11
+#endif
+
+
+#define DEFAULT_RX_TIMEOUT 10000
+
+// The anchor position can be set using parameters
+// As an option you can set a static position in this file and set
+// combinedAnchorPositionOk to enable sending the anchor rangings to the Kalman filter
+
+static lpsAlgoOptions_t algoOptions = {
+  // .userRequestedMode is the wanted algorithm, available as a parameter
+#if LPS_TDOA_ENABLE
+  .userRequestedMode = lpsMode_TDoA2,
+#elif LPS_TDOA3_ENABLE
+  .userRequestedMode = lpsMode_TDoA3,
+#elif defined(LPS_TWR_ENABLE)
+  .userRequestedMode = lpsMode_TWR,
+#else
+  .userRequestedMode = lpsMode_auto,
+#endif
+  // .currentRangingMode is the currently running algorithm, available as a log
+  // lpsMode_auto is an impossible mode which forces initialization of the requested mode
+  // at startup
+  .currentRangingMode = lpsMode_auto,
+  .modeAutoSearchActive = true,
+  .modeAutoSearchDoInitialize = true,
+};
+
+struct {
+  uwbAlgorithm_t *algorithm;
+  char *name;
+} algorithmsList[LPS_NUMBER_OF_ALGORITHMS + 1] = {
+  [lpsMode_TWR] = {.algorithm = &uwbTwrTagAlgorithm, .name="TWR"},
+  [lpsMode_TDoA2] = {.algorithm = &uwbTdoa2TagAlgorithm, .name="TDoA2"},
+  [lpsMode_TDoA3] = {.algorithm = &uwbTdoa3TagAlgorithm, .name="TDoA3"},
+};
+
+#if LPS_TDOA_ENABLE
+static uwbAlgorithm_t *algorithm = &uwbTdoa2TagAlgorithm;
+#elif LPS_TDOA3_ENABLE
+static uwbAlgorithm_t *algorithm = &uwbTdoa3TagAlgorithm;
+#else
+static uwbAlgorithm_t *algorithm = &uwbTwrTagAlgorithm;
+#endif
+
+static bool isInit = false;
+static TaskHandle_t uwbTaskHandle = 0;
+static SemaphoreHandle_t algoSemaphore;
+static dwDevice_t dwm_device;
+static dwDevice_t *dwm = &dwm_device;
+
+static QueueHandle_t lppShortQueue;
+
+static uint32_t timeout;
+
+static STATS_CNT_RATE_DEFINE(spiWriteCount, 1000);
+static STATS_CNT_RATE_DEFINE(spiReadCount, 1000);
+
+// Memory read/write handling
+#define MEM_LOCO_INFO             0x0000
+#define MEM_LOCO_ANCHOR_BASE      0x1000
+#define MEM_LOCO_ANCHOR_PAGE_SIZE 0x0100
+#define MEM_LOCO_PAGE_LEN         (3 * sizeof(float) + 1)
+
+#define MEM_ANCHOR_ID_LIST_LENGTH 256
+
+#define MEM_LOCO2_ID_LIST          0x0000
+#define MEM_LOCO2_ACTIVE_LIST      0x1000
+#define MEM_LOCO2_ANCHOR_BASE      0x2000
+#define MEM_LOCO2_ANCHOR_PAGE_SIZE 0x0100
+#define MEM_LOCO2_PAGE_LEN         (3 * sizeof(float) + 1)
+
+static uint32_t handleMemGetSize(void) { return MEM_LOCO_ANCHOR_BASE + MEM_LOCO_ANCHOR_PAGE_SIZE * 256; }
+static bool handleMemRead(const uint32_t memAddr, const uint8_t readLen, uint8_t* dest);
+static const MemoryHandlerDef_t memDef = {
+  .type = MEM_TYPE_LOCO2,
+  .getSize = handleMemGetSize,
+  .read = handleMemRead,
+  .write = 0, // Write is not supported
+};
+static void buildAnchorMemList(const uint32_t memAddr, const uint8_t readLen, uint8_t* dest, const uint32_t pageBase_address, const uint8_t anchorCount, const uint8_t unsortedAnchorList[]);
+
+static void txCallback(dwDevice_t *dev)
+{
+  timeout = algorithm->onEvent(dev, eventPacketSent);
+}
+
+static void rxCallback(dwDevice_t *dev)
+{
+  timeout = algorithm->onEvent(dev, eventPacketReceived);
+}
+
+static void rxTimeoutCallback(dwDevice_t * dev) {
+  timeout = algorithm->onEvent(dev, eventReceiveTimeout);
+}
+
+static bool handleMemRead(const uint32_t memAddr, const uint8_t readLen, uint8_t* dest) {
+  bool result = false;
+
+  static uint8_t unsortedAnchorList[MEM_ANCHOR_ID_LIST_LENGTH];
+
+  if (memAddr >= MEM_LOCO2_ID_LIST && memAddr < MEM_LOCO2_ACTIVE_LIST) {
+    uint8_t anchorCount = locoDeckGetAnchorIdList(unsortedAnchorList, MEM_ANCHOR_ID_LIST_LENGTH);
+    buildAnchorMemList(memAddr, readLen, dest, MEM_LOCO2_ID_LIST, anchorCount, unsortedAnchorList);
+    result = true;
+  } else if (memAddr >= MEM_LOCO2_ACTIVE_LIST && memAddr < MEM_LOCO2_ANCHOR_BASE) {
+    uint8_t anchorCount = locoDeckGetActiveAnchorIdList(unsortedAnchorList, MEM_ANCHOR_ID_LIST_LENGTH);
+    buildAnchorMemList(memAddr, readLen, dest, MEM_LOCO2_ACTIVE_LIST, anchorCount, unsortedAnchorList);
+    result = true;
+  } else {
+    if (memAddr >= MEM_LOCO2_ANCHOR_BASE) {
+      uint32_t pageAddress = memAddr - MEM_LOCO2_ANCHOR_BASE;
+      if ((pageAddress % MEM_LOCO2_ANCHOR_PAGE_SIZE) == 0 && MEM_LOCO2_PAGE_LEN == readLen) {
+        uint32_t anchorId = pageAddress / MEM_LOCO2_ANCHOR_PAGE_SIZE;
+
+        point_t position;
+        memset(&position, 0, sizeof(position));
+        locoDeckGetAnchorPosition(anchorId, &position);
+
+        float* destAsFloat = (float*)dest;
+        destAsFloat[0] = position.x;
+        destAsFloat[1] = position.y;
+        destAsFloat[2] = position.z;
+
+        bool hasBeenSet = (position.timestamp != 0);
+        dest[sizeof(float) * 3] = hasBeenSet;
+
+        result = true;
+      }
+    }
+  }
+
+  return result;
+}
+
+static void buildAnchorMemList(const uint32_t memAddr, const uint8_t readLen, uint8_t* dest, const uint32_t pageBase_address, const uint8_t anchorCount, const uint8_t unsortedAnchorList[]) {
+  for (int i = 0; i < readLen; i++) {
+    int address = memAddr + i;
+    int addressInPage = address - pageBase_address;
+    uint8_t val = 0;
+
+    if (addressInPage == 0) {
+      val = anchorCount;
+    } else {
+      int anchorIndex = addressInPage - 1;
+      if (anchorIndex < anchorCount) {
+        val = unsortedAnchorList[anchorIndex];
+      }
+    }
+
+    dest[i] = val;
+  }
+}
+
+// This function is called from the memory sub system that runs in a different
+// task, protect it from concurrent calls from this task
+bool locoDeckGetAnchorPosition(const uint8_t anchorId, point_t* position)
+{
+  if (!isInit) {
+    return false;
+  }
+
+  xSemaphoreTake(algoSemaphore, portMAX_DELAY);
+  bool result = algorithm->getAnchorPosition(anchorId, position);
+  xSemaphoreGive(algoSemaphore);
+  return result;
+}
+
+// This function is called from the memory sub system that runs in a different
+// task, protect it from concurrent calls from this task
+uint8_t locoDeckGetAnchorIdList(uint8_t unorderedAnchorList[], const int maxListSize) {
+  if (!isInit) {
+    return 0;
+  }
+
+  xSemaphoreTake(algoSemaphore, portMAX_DELAY);
+  uint8_t result = algorithm->getAnchorIdList(unorderedAnchorList, maxListSize);
+  xSemaphoreGive(algoSemaphore);
+  return result;
+}
+
+// This function is called from the memory sub system that runs in a different
+// task, protect it from concurrent calls from this task
+uint8_t locoDeckGetActiveAnchorIdList(uint8_t unorderedAnchorList[], const int maxListSize) {
+  if (!isInit) {
+    return 0;
+  }
+
+  xSemaphoreTake(algoSemaphore, portMAX_DELAY);
+  uint8_t result = algorithm->getActiveAnchorIdList(unorderedAnchorList, maxListSize);
+  xSemaphoreGive(algoSemaphore);
+  return result;
+}
+
+static bool switchToMode(const lpsMode_t newMode) {
+  bool result = false;
+
+  if (lpsMode_auto != newMode && newMode <= LPS_NUMBER_OF_ALGORITHMS) {
+    algoOptions.currentRangingMode = newMode;
+    algorithm = algorithmsList[algoOptions.currentRangingMode].algorithm;
+
+    algorithm->init(dwm);
+    timeout = algorithm->onEvent(dwm, eventTimeout);
+
+    result = true;
+  }
+
+  return result;
+}
+
+static void autoModeSearchTryMode(const lpsMode_t newMode, const uint32_t now) {
+  // Set up next time to check
+  algoOptions.nextSwitchTick = now + LPS_AUTO_MODE_SWITCH_PERIOD;
+  switchToMode(newMode);
+}
+
+static lpsMode_t autoModeSearchGetNextMode() {
+  lpsMode_t newMode = algoOptions.currentRangingMode + 1;
+  if (newMode > LPS_NUMBER_OF_ALGORITHMS) {
+    newMode = lpsMode_TWR;
+  }
+
+  return newMode;
+}
+
+static void processAutoModeSwitching() {
+  uint32_t now = xTaskGetTickCount();
+
+  if (algoOptions.modeAutoSearchActive) {
+    if (algoOptions.modeAutoSearchDoInitialize) {
+      autoModeSearchTryMode(lpsMode_TDoA2, now);
+      algoOptions.modeAutoSearchDoInitialize = false;
+    } else {
+      if (now > algoOptions.nextSwitchTick) {
+        if (algorithm->isRangingOk()) {
+          // We have found an algorithm, stop searching and lock to it.
+          algoOptions.modeAutoSearchActive = false;
+          DEBUG_PRINT("Automatic mode: detected %s\n", algorithmsList[algoOptions.currentRangingMode].name);
+        } else {
+          lpsMode_t newMode = autoModeSearchGetNextMode();
+          autoModeSearchTryMode(newMode, now);
+        }
+      }
+    }
+  }
+}
+
+static void resetAutoSearchMode() {
+  algoOptions.modeAutoSearchActive = true;
+  algoOptions.modeAutoSearchDoInitialize = true;
+}
+
+static void handleModeSwitch() {
+  if (algoOptions.userRequestedMode == lpsMode_auto) {
+    processAutoModeSwitching();
+  } else {
+    resetAutoSearchMode();
+    if (algoOptions.userRequestedMode != algoOptions.currentRangingMode) {
+      if (switchToMode(algoOptions.userRequestedMode)) {
+        DEBUG_PRINT("Switching to mode %s\n", algorithmsList[algoOptions.currentRangingMode].name);
+      }
+    }
+  }
+}
+
+static void uwbTask(void* parameters) {
+  lppShortQueue = xQueueCreate(10, sizeof(lpsLppShortPacket_t));
+
+  algoOptions.currentRangingMode = lpsMode_auto;
+
+  systemWaitStart();
+
+  while(1) {
+    xSemaphoreTake(algoSemaphore, portMAX_DELAY);
+    handleModeSwitch();
+    xSemaphoreGive(algoSemaphore);
+
+    if (ulTaskNotifyTake(pdTRUE, timeout / portTICK_PERIOD_MS) > 0) {
+      do{
+        xSemaphoreTake(algoSemaphore, portMAX_DELAY);
+        dwHandleInterrupt(dwm);
+        xSemaphoreGive(algoSemaphore);
+      } while(digitalRead(GPIO_PIN_IRQ) != 0);
+    } else {
+      xSemaphoreTake(algoSemaphore, portMAX_DELAY);
+      timeout = algorithm->onEvent(dwm, eventTimeout);
+      xSemaphoreGive(algoSemaphore);
+    }
+  }
+}
+
+static lpsLppShortPacket_t lppShortPacket;
+
+bool lpsSendLppShort(uint8_t destId, void* data, size_t length)
+{
+  bool result = false;
+
+  if (isInit)
+  {
+    lppShortPacket.dest = destId;
+    lppShortPacket.length = length;
+    memcpy(lppShortPacket.data, data, length);
+    result = xQueueSend(lppShortQueue, &lppShortPacket,0) == pdPASS;
+  }
+
+  return result;
+}
+
+bool lpsGetLppShort(lpsLppShortPacket_t* shortPacket)
+{
+  return xQueueReceive(lppShortQueue, shortPacket, 0) == pdPASS;
+}
+
+static uint8_t spiTxBuffer[196];
+static uint8_t spiRxBuffer[196];
+static uint16_t spiSpeed = SPI_BAUDRATE_2MHZ;
+
+/************ Low level ops for libdw **********/
+static void spiWrite(dwDevice_t* dev, const void *header, size_t headerLength,
+                                      const void* data, size_t dataLength)
+{
+  spiBeginTransaction(spiSpeed);
+  digitalWrite(CS_PIN, LOW);
+  memcpy(spiTxBuffer, header, headerLength);
+  memcpy(spiTxBuffer+headerLength, data, dataLength);
+  spiExchange(headerLength+dataLength, spiTxBuffer, spiRxBuffer);
+  digitalWrite(CS_PIN, HIGH);
+  spiEndTransaction();
+  STATS_CNT_RATE_EVENT(&spiWriteCount);
+}
+
+static void spiRead(dwDevice_t* dev, const void *header, size_t headerLength,
+                                     void* data, size_t dataLength)
+{
+  spiBeginTransaction(spiSpeed);
+  digitalWrite(CS_PIN, LOW);
+  memcpy(spiTxBuffer, header, headerLength);
+  memset(spiTxBuffer+headerLength, 0, dataLength);
+  spiExchange(headerLength+dataLength, spiTxBuffer, spiRxBuffer);
+  memcpy(data, spiRxBuffer+headerLength, dataLength);
+  digitalWrite(CS_PIN, HIGH);
+  spiEndTransaction();
+  STATS_CNT_RATE_EVENT(&spiReadCount);
+}
+
+#if LOCODECK_USE_ALT_PINS
+  void __attribute__((used)) EXTI5_Callback(void)
+#else
+  void __attribute__((used)) EXTI11_Callback(void)
+#endif
+  {
+    portBASE_TYPE  xHigherPriorityTaskWoken = pdFALSE;
+
+    // Unlock interrupt handling task
+    vTaskNotifyGiveFromISR(uwbTaskHandle, &xHigherPriorityTaskWoken);
+
+    if(xHigherPriorityTaskWoken) {
+      portYIELD();
+    }
+  }
+
+static void spiSetSpeed(dwDevice_t* dev, dwSpiSpeed_t speed)
+{
+  if (speed == dwSpiSpeedLow)
+  {
+    spiSpeed = SPI_BAUDRATE_2MHZ;
+  }
+  else if (speed == dwSpiSpeedHigh)
+  {
+    spiSpeed = SPI_BAUDRATE_21MHZ;
+  }
+}
+
+static void delayms(dwDevice_t* dev, unsigned int delay)
+{
+  vTaskDelay(M2T(delay));
+}
+
+static dwOps_t dwOps = {
+  .spiRead = spiRead,
+  .spiWrite = spiWrite,
+  .spiSetSpeed = spiSetSpeed,
+  .delayms = delayms,
+};
+
+/*********** Deck driver initialization ***************/
+
+static void dwm1000Init(DeckInfo *info)
+{
+  EXTI_InitTypeDef EXTI_InitStructure;
+
+  spiBegin();
+
+  // Set up interrupt
+  SYSCFG_EXTILineConfig(EXTI_PortSource, EXTI_PinSource);
+
+  EXTI_InitStructure.EXTI_Line = EXTI_LineN;
+  EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
+  EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising;
+  EXTI_InitStructure.EXTI_LineCmd = ENABLE;
+  EXTI_Init(&EXTI_InitStructure);
+
+  // Init pins
+  pinMode(CS_PIN, OUTPUT);
+  pinMode(GPIO_PIN_RESET, OUTPUT);
+  pinMode(GPIO_PIN_IRQ, INPUT);
+
+  // Reset the DW1000 chip
+  digitalWrite(GPIO_PIN_RESET, 0);
+  vTaskDelay(M2T(10));
+  digitalWrite(GPIO_PIN_RESET, 1);
+  vTaskDelay(M2T(10));
+
+  // Initialize the driver
+  dwInit(dwm, &dwOps);       // Init libdw
+
+  int result = dwConfigure(dwm);
+  if (result != 0) {
+    isInit = false;
+    DEBUG_PRINT("Failed to configure DW1000!\r\n");
+    return;
+  }
+
+  dwEnableAllLeds(dwm);
+
+  dwTime_t delay = {.full = 0};
+  dwSetAntenaDelay(dwm, delay);
+
+  dwAttachSentHandler(dwm, txCallback);
+  dwAttachReceivedHandler(dwm, rxCallback);
+  dwAttachReceiveTimeoutHandler(dwm, rxTimeoutCallback);
+
+  dwNewConfiguration(dwm);
+  dwSetDefaults(dwm);
+
+
+  #ifdef LPS_LONGER_RANGE
+  dwEnableMode(dwm, MODE_SHORTDATA_MID_ACCURACY);
+  #else
+  dwEnableMode(dwm, MODE_SHORTDATA_FAST_ACCURACY);
+  #endif
+
+  dwSetChannel(dwm, CHANNEL_2);
+  dwSetPreambleCode(dwm, PREAMBLE_CODE_64MHZ_9);
+
+  #ifdef LPS_FULL_TX_POWER
+  dwUseSmartPower(dwm, false);
+  dwSetTxPower(dwm, 0x1F1F1F1Ful);
+  #else
+  dwUseSmartPower(dwm, true);
+  #endif
+
+  dwSetReceiveWaitTimeout(dwm, DEFAULT_RX_TIMEOUT);
+
+  dwCommitConfiguration(dwm);
+
+  memoryRegisterHandler(&memDef);
+
+  algoSemaphore= xSemaphoreCreateMutex();
+
+  xTaskCreate(uwbTask, LPS_DECK_TASK_NAME, 3 * configMINIMAL_STACK_SIZE, NULL,
+                    LPS_DECK_TASK_PRI, &uwbTaskHandle);
+
+  isInit = true;
+}
+
+uint16_t locoDeckGetRangingState() {
+  return algoOptions.rangingState;
+}
+
+void locoDeckSetRangingState(const uint16_t newState) {
+  algoOptions.rangingState = newState;
+}
+
+
+static bool dwm1000Test()
+{
+  if (!isInit) {
+    DEBUG_PRINT("Error while initializing DWM1000\n");
+  }
+
+  return isInit;
+}
+
+static const DeckDriver dwm1000_deck = {
+  .vid = 0xBC,
+  .pid = 0x06,
+  .name = "bcDWM1000",
+
+  .usedGpio = 0,  // FIXME: set the used pins
+  .requiredEstimator = kalmanEstimator,
+  #ifdef LOCODECK_NO_LOW_INTERFERENCE
+  .requiredLowInterferenceRadioMode = false,
+  #else
+  .requiredLowInterferenceRadioMode = true,
+  #endif
+
+  .init = dwm1000Init,
+  .test = dwm1000Test,
+};
+
+DECK_DRIVER(dwm1000_deck);
+
+PARAM_GROUP_START(deck)
+
+/**
+ * @brief Nonzero if [Loco positioning deck](%https://store.bitcraze.io/products/loco-positioning-deck) is attached
+ */
+PARAM_ADD_CORE(PARAM_UINT8 | PARAM_RONLY, bcDWM1000, &isInit)
+
+PARAM_GROUP_STOP(deck)
+
+LOG_GROUP_START(ranging)
+LOG_ADD(LOG_UINT16, state, &algoOptions.rangingState)
+LOG_GROUP_STOP(ranging)
+
+/**
+ * Log group for basic information about the Loco Positioning System
+ */
+LOG_GROUP_START(loco)
+
+/**
+ * @brief The current mode of the Loco Positionning system
+ *
+ * | Value | Mode   | \n
+ * | -     | -      | \n
+ * |   1   | TWR    | \n
+ * |   2   | TDoA 2 | \n
+ * |   3   | TDoA 3 | \n
+ */
+LOG_ADD_CORE(LOG_UINT8, mode, &algoOptions.currentRangingMode)
+
+STATS_CNT_RATE_LOG_ADD(spiWr, &spiWriteCount)
+STATS_CNT_RATE_LOG_ADD(spiRe, &spiReadCount)
+LOG_GROUP_STOP(loco)
+
+/**
+ * The Loco Positioning System implements three different positioning modes:
+ * Two Way Ranging (TWR), Time Difference of Arrival 2 (TDoA 2) and Time Difference of Arrival 3 (TDoA 3)
+ *
+ * ### TWR mode
+ *
+ * In this mode, the tag pings the anchors in sequence, this allows it to
+ * measure the distance between the tag and the anchors. Using this information
+ * a theoretical minimum of 4 Anchors is required to calculate the 3D position
+ * of a Tag, but a more realistic number is 6 to add redundancy and accuracy.
+ * This mode is the most accurate mode and also works when the tag or quad
+ * leaves the space delimited by the anchors. The tag is actively communicating
+ * with the anchors in a time slotted fashion and in this mode only one tag or
+ * quad can be positioned with a maximum of 8 anchors.
+ *
+ * ### TDoA 2 mode
+ *
+ * In TDoA 2 mode, the anchor system is continuously sending synchronization
+ * packets. A tag listening to these packets can calculate the relative
+ * distance to two anchors by measuring the time difference of arrival of the
+ * packets. From the TDoA information it is possible to calculate the 3D
+ * position in space. In this mode the tag is only passively listening, so new
+ * tags do not add any load to the system which makes it possible to position
+ * any number of tags or quads simultaneously. This makes it a perfect
+ * mode for swarming.
+ *
+ * Compared to TWR, TDoA 2 is more restrictive when it comes to the space where
+ * positioning works, ideally the tag should be within, or very close to,
+ * the space delimited by the anchor system. This means that TDoA 2 works best
+ * with 8 anchors placed in the corners of the flying space. In this space the
+ * accuracy and precision is comparable to TWR.
+
+ * In this mode the anchor system is time slotted and synchronized and the
+ * number of anchors is limited to 8.
+ *
+ * ### TDoA 3 mode
+ *
+ * The TDoA 3 mode has many similarities with TDoA 2 and supports any number
+ * of tags or quads. The main difference is that the time slotted scheme of
+ * TDoA 2 has been replaced by a randomized transmission schedule which makes
+ * it possible to add more anchors. By adding more anchors the system can be
+ * scaled to larger spaces or span multiple rooms without line of sight between
+ * all anchors. It also makes it more robust and can handle loss or addition of
+ * anchors dynamically. The estimated position in this mode might be slightly
+ * more noisy compared to TDoA 2.
+ */
+PARAM_GROUP_START(loco)
+
+/**
+ * @brief The Loco positioning mode to use (default: 0)
+ *
+ * | Value | Mode   |\n
+ * | -     | -      |\n
+ * |   0   | Auto   |\n
+ * |   1   | TWR    |\n
+ * |   2   | TDoA 2 |\n
+ * |   3   | TDoA 3 |\n
+ */
+PARAM_ADD_CORE(PARAM_UINT8, mode, &algoOptions.userRequestedMode)
+
+PARAM_GROUP_STOP(loco)

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/lpsTdoa2Tag.c

+/*
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * LPS node firmware.
+ *
+ * Copyright 2016, Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * lpsTdoa2Tag.c 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 lpsTdoa2Tag.c.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+#include <string.h>
+
+#include "FreeRTOS.h"
+#include "task.h"
+
+#include "log.h"
+#include "param.h"
+#include "lpsTdoa2Tag.h"
+
+#include "stabilizer_types.h"
+#include "cfassert.h"
+
+#include "estimator.h"
+
+#include "physicalConstants.h"
+#include "tdoaEngineInstance.h"
+
+#if ANCHOR_STORAGE_COUNT < LOCODECK_NR_OF_TDOA2_ANCHORS
+  #error "Tdoa engine storage is too small"
+#endif
+#if REMOTE_ANCHOR_DATA_COUNT < LOCODECK_NR_OF_TDOA2_ANCHORS
+  #error "Tdoa engine storage is too small"
+#endif
+
+// Config
+static lpsTdoa2AlgoOptions_t defaultOptions = {
+   .anchorAddress = {
+     0xbccf000000000000,
+     0xbccf000000000001,
+     0xbccf000000000002,
+     0xbccf000000000003,
+     0xbccf000000000004,
+     0xbccf000000000005,
+     0xbccf000000000006,
+     0xbccf000000000007,
+   },
+};
+
+static lpsTdoa2AlgoOptions_t* options = &defaultOptions;
+
+// State
+typedef struct {
+  uint32_t anchorStatusTimeout;
+} history_t;
+
+static uint8_t previousAnchor;
+// Holds data for the latest packet from all anchors
+static history_t history[LOCODECK_NR_OF_TDOA2_ANCHORS];
+
+
+// LPP packet handling
+static lpsLppShortPacket_t lppPacket;
+static bool lppPacketToSend;
+static int lppPacketSendTryCounter;
+
+static void lpsHandleLppShortPacket(const uint8_t srcId, const uint8_t *data, tdoaAnchorContext_t* anchorCtx);
+
+// Log data
+static float logUwbTdoaDistDiff[LOCODECK_NR_OF_TDOA2_ANCHORS];
+static float logClockCorrection[LOCODECK_NR_OF_TDOA2_ANCHORS];
+static uint16_t logAnchorDistance[LOCODECK_NR_OF_TDOA2_ANCHORS];
+
+static bool rangingOk;
+
+// The default receive time in the anchors for messages from other anchors is 0
+// and is overwritten with the actual receive time when a packet arrives.
+// That is, if no message was received the rx time will be 0.
+static bool isValidTimeStamp(const int64_t anchorRxTime) {
+  return anchorRxTime != 0;
+}
+
+static bool isConsecutiveIds(const uint8_t previousAnchor, const uint8_t currentAnchor) {
+  return (((previousAnchor + 1) & 0x07) == currentAnchor);
+}
+
+static void updateRemoteData(tdoaAnchorContext_t* anchorCtx, const rangePacket2_t* packet) {
+  const uint8_t anchorId = tdoaStorageGetId(anchorCtx);
+  for (uint8_t i = 0; i < LOCODECK_NR_OF_TDOA2_ANCHORS; i++) {
+    if (anchorId != i) {
+      uint8_t remoteId = i;
+      int64_t remoteRxTime = packet->timestamps[i];
+      uint8_t remoteSeqNr = packet->sequenceNrs[i] & 0x7f;
+
+      if (isValidTimeStamp(remoteRxTime)) {
+        tdoaStorageSetRemoteRxTime(anchorCtx, remoteId, remoteRxTime, remoteSeqNr);
+      }
+
+      bool hasDistance = (packet->distances[i] != 0);
+      if (hasDistance) {
+        int64_t tof = packet->distances[i];
+        if (isValidTimeStamp(tof)) {
+          tdoaStorageSetTimeOfFlight(anchorCtx, remoteId, tof);
+
+          if (isConsecutiveIds(previousAnchor, anchorId)) {
+            logAnchorDistance[anchorId] = packet->distances[previousAnchor];
+          }
+        }
+      }
+    }
+  }
+}
+
+static void handleLppPacket(const int dataLength, const packet_t* rxPacket, tdoaAnchorContext_t* anchorCtx) {
+  const int32_t payloadLength = dataLength - MAC802154_HEADER_LENGTH;
+  const int32_t startOfLppDataInPayload = LPS_TDOA2_LPP_HEADER;
+  const int32_t lppDataLength = payloadLength - startOfLppDataInPayload;
+
+  if (lppDataLength > 0) {
+    const uint8_t lppPacketHeader = rxPacket->payload[LPS_TDOA2_LPP_HEADER];
+    if (lppPacketHeader == LPP_HEADER_SHORT_PACKET) {
+      int srcId = -1;
+
+      for (int i=0; i < LOCODECK_NR_OF_TDOA2_ANCHORS; i++) {
+        if (rxPacket->sourceAddress == options->anchorAddress[i]) {
+          srcId = i;
+          break;
+        }
+      }
+
+      if (srcId >= 0) {
+        lpsHandleLppShortPacket(srcId, &rxPacket->payload[LPS_TDOA2_LPP_TYPE], anchorCtx);
+      }
+    }
+  }
+}
+
+// Send an LPP packet, the radio will automatically go back in RX mode
+static void sendLppShort(dwDevice_t *dev, lpsLppShortPacket_t *packet)
+{
+  static packet_t txPacket;
+  dwIdle(dev);
+
+  MAC80215_PACKET_INIT(txPacket, MAC802154_TYPE_DATA);
+
+  txPacket.payload[LPS_TDOA2_TYPE_INDEX] = LPP_HEADER_SHORT_PACKET;
+  memcpy(&txPacket.payload[LPS_TDOA2_SEND_LPP_PAYLOAD_INDEX], packet->data, packet->length);
+
+  txPacket.pan = 0xbccf;
+  txPacket.sourceAddress = 0xbccf000000000000 | 0xff;
+  txPacket.destAddress = options->anchorAddress[packet->dest];
+
+  dwNewTransmit(dev);
+  dwSetDefaults(dev);
+  dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+1+packet->length);
+
+  dwWaitForResponse(dev, true);
+  dwStartTransmit(dev);
+}
+
+static bool rxcallback(dwDevice_t *dev) {
+  tdoaStats_t* stats = &tdoaEngineState.stats;
+  STATS_CNT_RATE_EVENT(&stats->packetsReceived);
+
+  int dataLength = dwGetDataLength(dev);
+  packet_t rxPacket;
+
+  dwGetData(dev, (uint8_t*)&rxPacket, dataLength);
+  const rangePacket2_t* packet = (rangePacket2_t*)rxPacket.payload;
+
+  bool lppSent = false;
+  if (packet->type == PACKET_TYPE_TDOA2) {
+    const uint8_t anchor = rxPacket.sourceAddress & 0xff;
+
+    // Check if we need to send the current LPP packet
+    if (lppPacketToSend && lppPacket.dest == anchor) {
+      sendLppShort(dev, &lppPacket);
+      lppSent = true;
+    }
+
+    dwTime_t arrival = {.full = 0};
+    dwGetReceiveTimestamp(dev, &arrival);
+
+    if (anchor < LOCODECK_NR_OF_TDOA2_ANCHORS) {
+      uint32_t now_ms = T2M(xTaskGetTickCount());
+
+      const int64_t rxAn_by_T_in_cl_T = arrival.full;
+      const int64_t txAn_in_cl_An = packet->timestamps[anchor];
+      const uint8_t seqNr = packet->sequenceNrs[anchor] & 0x7f;
+
+      tdoaAnchorContext_t anchorCtx;
+      tdoaEngineGetAnchorCtxForPacketProcessing(&tdoaEngineState, anchor, now_ms, &anchorCtx);
+      updateRemoteData(&anchorCtx, packet);
+      tdoaEngineProcessPacket(&tdoaEngineState, &anchorCtx, txAn_in_cl_An, rxAn_by_T_in_cl_T);
+      tdoaStorageSetRxTxData(&anchorCtx, rxAn_by_T_in_cl_T, txAn_in_cl_An, seqNr);
+
+      logClockCorrection[anchor] = tdoaStorageGetClockCorrection(&anchorCtx);
+
+      previousAnchor = anchor;
+
+      handleLppPacket(dataLength, &rxPacket, &anchorCtx);
+
+      rangingOk = true;
+    }
+  }
+
+  return lppSent;
+}
+
+static void setRadioInReceiveMode(dwDevice_t *dev) {
+  dwNewReceive(dev);
+  dwSetDefaults(dev);
+  dwStartReceive(dev);
+}
+
+static uint32_t onEvent(dwDevice_t *dev, uwbEvent_t event) {
+  switch(event) {
+    case eventPacketReceived:
+      if (rxcallback(dev)) {
+        lppPacketToSend = false;
+      } else {
+        setRadioInReceiveMode(dev);
+
+        // Discard lpp packet if we cannot send it for too long
+        if (++lppPacketSendTryCounter >= TDOA2_LPP_PACKET_SEND_TIMEOUT) {
+          lppPacketToSend = false;
+        }
+      }
+
+      if (!lppPacketToSend) {
+        // Get next lpp packet
+        lppPacketToSend = lpsGetLppShort(&lppPacket);
+        lppPacketSendTryCounter = 0;
+      }
+      break;
+    case eventTimeout:
+      setRadioInReceiveMode(dev);
+      break;
+    case eventReceiveTimeout:
+      setRadioInReceiveMode(dev);
+      break;
+    case eventPacketSent:
+      // Service packet sent, the radio is back to receive automatically
+      break;
+    default:
+      ASSERT_FAILED();
+  }
+
+  uint32_t now = xTaskGetTickCount();
+  uint16_t rangingState = 0;
+  for (int anchor = 0; anchor < LOCODECK_NR_OF_TDOA2_ANCHORS; anchor++) {
+    if (now < history[anchor].anchorStatusTimeout) {
+      rangingState |= (1 << anchor);
+    }
+  }
+  locoDeckSetRangingState(rangingState);
+
+  return MAX_TIMEOUT;
+}
+
+
+static void sendTdoaToEstimatorCallback(tdoaMeasurement_t* tdoaMeasurement) {
+  estimatorEnqueueTDOA(tdoaMeasurement);
+
+  #ifdef LPS_2D_POSITION_HEIGHT
+  // If LPS_2D_POSITION_HEIGHT is defined we assume that we are doing 2D positioning.
+  // LPS_2D_POSITION_HEIGHT contains the height (Z) that the tag will be located at
+  heightMeasurement_t heightData;
+  heightData.timestamp = xTaskGetTickCount();
+  heightData.height = LPS_2D_POSITION_HEIGHT;
+  heightData.stdDev = 0.0001;
+  estimatorEnqueueAbsoluteHeight(&heightData);
+  #endif
+
+  const uint8_t idA = tdoaMeasurement->anchorIds[0];
+  const uint8_t idB = tdoaMeasurement->anchorIds[1];
+  if (isConsecutiveIds(idA, idB)) {
+    logUwbTdoaDistDiff[idB] = tdoaMeasurement->distanceDiff;
+  }
+}
+
+
+static void Initialize(dwDevice_t *dev) {
+  uint32_t now_ms = T2M(xTaskGetTickCount());
+  tdoaEngineInit(&tdoaEngineState, now_ms, sendTdoaToEstimatorCallback, LOCODECK_TS_FREQ, TdoaEngineMatchingAlgorithmYoungest);
+
+  previousAnchor = 0;
+
+  lppPacketToSend = false;
+
+  locoDeckSetRangingState(0);
+  dwSetReceiveWaitTimeout(dev, TDOA2_RECEIVE_TIMEOUT);
+
+  dwCommitConfiguration(dev);
+
+  rangingOk = false;
+}
+
+static bool isRangingOk()
+{
+  return rangingOk;
+}
+
+static bool getAnchorPosition(const uint8_t anchorId, point_t* position) {
+  tdoaAnchorContext_t anchorCtx;
+  uint32_t now_ms = T2M(xTaskGetTickCount());
+
+  bool contextFound = tdoaStorageGetAnchorCtx(tdoaEngineState.anchorInfoArray, anchorId, now_ms, &anchorCtx);
+  if (contextFound) {
+    tdoaStorageGetAnchorPosition(&anchorCtx, position);
+    return true;
+  }
+
+  return false;
+}
+
+static uint8_t getAnchorIdList(uint8_t unorderedAnchorList[], const int maxListSize) {
+  return tdoaStorageGetListOfAnchorIds(tdoaEngineState.anchorInfoArray, unorderedAnchorList, maxListSize);
+}
+
+static uint8_t getActiveAnchorIdList(uint8_t unorderedAnchorList[], const int maxListSize) {
+  uint32_t now_ms = T2M(xTaskGetTickCount());
+  return tdoaStorageGetListOfActiveAnchorIds(tdoaEngineState.anchorInfoArray, unorderedAnchorList, maxListSize, now_ms);
+}
+
+// Loco Posisioning Protocol (LPP) handling
+static void lpsHandleLppShortPacket(const uint8_t srcId, const uint8_t *data, tdoaAnchorContext_t* anchorCtx)
+{
+  uint8_t type = data[0];
+
+  if (type == LPP_SHORT_ANCHORPOS) {
+    if (srcId < LOCODECK_NR_OF_TDOA2_ANCHORS) {
+      struct lppShortAnchorPos_s *newpos = (struct lppShortAnchorPos_s*)&data[1];
+      tdoaStorageSetAnchorPosition(anchorCtx, newpos->x, newpos->y, newpos->z);
+    }
+  }
+}
+
+uwbAlgorithm_t uwbTdoa2TagAlgorithm = {
+  .init = Initialize,
+  .onEvent = onEvent,
+  .isRangingOk = isRangingOk,
+  .getAnchorPosition = getAnchorPosition,
+  .getAnchorIdList = getAnchorIdList,
+  .getActiveAnchorIdList = getActiveAnchorIdList,
+};
+
+void lpsTdoa2TagSetOptions(lpsTdoa2AlgoOptions_t* newOptions) {
+  options = newOptions;
+}
+
+LOG_GROUP_START(tdoa2)
+LOG_ADD(LOG_FLOAT, d7-0, &logUwbTdoaDistDiff[0])
+LOG_ADD(LOG_FLOAT, d0-1, &logUwbTdoaDistDiff[1])
+LOG_ADD(LOG_FLOAT, d1-2, &logUwbTdoaDistDiff[2])
+LOG_ADD(LOG_FLOAT, d2-3, &logUwbTdoaDistDiff[3])
+LOG_ADD(LOG_FLOAT, d3-4, &logUwbTdoaDistDiff[4])
+LOG_ADD(LOG_FLOAT, d4-5, &logUwbTdoaDistDiff[5])
+LOG_ADD(LOG_FLOAT, d5-6, &logUwbTdoaDistDiff[6])
+LOG_ADD(LOG_FLOAT, d6-7, &logUwbTdoaDistDiff[7])
+
+LOG_ADD(LOG_FLOAT, cc0, &logClockCorrection[0])
+LOG_ADD(LOG_FLOAT, cc1, &logClockCorrection[1])
+LOG_ADD(LOG_FLOAT, cc2, &logClockCorrection[2])
+LOG_ADD(LOG_FLOAT, cc3, &logClockCorrection[3])
+LOG_ADD(LOG_FLOAT, cc4, &logClockCorrection[4])
+LOG_ADD(LOG_FLOAT, cc5, &logClockCorrection[5])
+LOG_ADD(LOG_FLOAT, cc6, &logClockCorrection[6])
+LOG_ADD(LOG_FLOAT, cc7, &logClockCorrection[7])
+
+LOG_ADD(LOG_UINT16, dist7-0, &logAnchorDistance[0])
+LOG_ADD(LOG_UINT16, dist0-1, &logAnchorDistance[1])
+LOG_ADD(LOG_UINT16, dist1-2, &logAnchorDistance[2])
+LOG_ADD(LOG_UINT16, dist2-3, &logAnchorDistance[3])
+LOG_ADD(LOG_UINT16, dist3-4, &logAnchorDistance[4])
+LOG_ADD(LOG_UINT16, dist4-5, &logAnchorDistance[5])
+LOG_ADD(LOG_UINT16, dist5-6, &logAnchorDistance[6])
+LOG_ADD(LOG_UINT16, dist6-7, &logAnchorDistance[7])
+LOG_GROUP_STOP(tdoa2)

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/lpsTdoa3Tag.c

+/*
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie firmware.
+ *
+ * Copyright 2018, Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * lpsTdoa3Tag.c 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 lpsTdoa3Tag.c. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+/*
+
+The tag is assumed to move around in a large system of anchors. Any anchor ids
+can be used, and the same anchor id can even be used by multiple anchors as long
+as they are not visible in the same area. It is assumed that the anchor density
+is evenly distributed in the covered volume and that 5-20 anchors are visible
+in every point. The tag is attached to a physical object and the expected
+velocity is a few m/s, this means that anchors are within range for a time
+period of seconds.
+
+The implementation must handle
+1. An infinite number of anchors, where around 20 are visible at one time
+2. Any anchor ids
+3. Dynamically changing visibility of anchors over time
+4. Random TX times from anchors with possible packet collisions and packet loss
+
+*/
+
+#include <string.h>
+
+#include "FreeRTOS.h"
+#include "task.h"
+
+#include "lpsTdoa3Tag.h"
+#include "tdoaEngineInstance.h"
+#include "tdoaStats.h"
+#include "estimator.h"
+
+#include "libdw1000.h"
+#include "mac.h"
+
+#define DEBUG_MODULE "TDOA3"
+#include "debug.h"
+#include "cfassert.h"
+
+// Positions for sent LPP packets
+#define LPS_TDOA3_TYPE 0
+#define LPS_TDOA3_SEND_LPP_PAYLOAD 1
+
+#define PACKET_TYPE_TDOA3 0x30
+
+#define TDOA3_RECEIVE_TIMEOUT 10000
+
+typedef struct {
+  uint8_t type;
+  uint8_t seq;
+  uint32_t txTimeStamp;
+  uint8_t remoteCount;
+} __attribute__((packed)) rangePacketHeader3_t;
+
+typedef struct {
+  uint8_t id;
+  uint8_t seq;
+  uint32_t rxTimeStamp;
+  uint16_t distance;
+} __attribute__((packed)) remoteAnchorDataFull_t;
+
+typedef struct {
+  uint8_t id;
+  uint8_t seq;
+  uint32_t rxTimeStamp;
+} __attribute__((packed)) remoteAnchorDataShort_t;
+
+typedef struct {
+  rangePacketHeader3_t header;
+  uint8_t remoteAnchorData;
+} __attribute__((packed)) rangePacket3_t;
+
+
+// Outgoing LPP packet
+static lpsLppShortPacket_t lppPacket;
+
+static bool rangingOk;
+
+static bool isValidTimeStamp(const int64_t anchorRxTime) {
+  return anchorRxTime != 0;
+}
+
+static int updateRemoteData(tdoaAnchorContext_t* anchorCtx, const void* payload) {
+  const rangePacket3_t* packet = (rangePacket3_t*)payload;
+  const void* anchorDataPtr = &packet->remoteAnchorData;
+  for (uint8_t i = 0; i < packet->header.remoteCount; i++) {
+    remoteAnchorDataFull_t* anchorData = (remoteAnchorDataFull_t*)anchorDataPtr;
+
+    uint8_t remoteId = anchorData->id;
+    int64_t remoteRxTime = anchorData->rxTimeStamp;
+    uint8_t remoteSeqNr = anchorData->seq & 0x7f;
+
+    if (isValidTimeStamp(remoteRxTime)) {
+      tdoaStorageSetRemoteRxTime(anchorCtx, remoteId, remoteRxTime, remoteSeqNr);
+    }
+
+    bool hasDistance = ((anchorData->seq & 0x80) != 0);
+    if (hasDistance) {
+      int64_t tof = anchorData->distance;
+      if (isValidTimeStamp(tof)) {
+        tdoaStorageSetTimeOfFlight(anchorCtx, remoteId, tof);
+
+        uint8_t anchorId = tdoaStorageGetId(anchorCtx);
+        tdoaStats_t* stats = &tdoaEngineState.stats;
+        if (anchorId == stats->anchorId && remoteId == stats->remoteAnchorId) {
+          stats->tof = (uint16_t)tof;
+        }
+      }
+
+      anchorDataPtr += sizeof(remoteAnchorDataFull_t);
+    } else {
+      anchorDataPtr += sizeof(remoteAnchorDataShort_t);
+    }
+  }
+
+  return (uint8_t*)anchorDataPtr - (uint8_t*)packet;
+}
+
+static void handleLppShortPacket(tdoaAnchorContext_t* anchorCtx, const uint8_t *data, const int length) {
+  uint8_t type = data[0];
+
+  if (type == LPP_SHORT_ANCHORPOS) {
+    struct lppShortAnchorPos_s *newpos = (struct lppShortAnchorPos_s*)&data[1];
+    tdoaStorageSetAnchorPosition(anchorCtx, newpos->x, newpos->y, newpos->z);
+  }
+}
+
+static void handleLppPacket(const int dataLength, int rangePacketLength, const packet_t* rxPacket, tdoaAnchorContext_t* anchorCtx) {
+  const int32_t payloadLength = dataLength - MAC802154_HEADER_LENGTH;
+  const int32_t startOfLppDataInPayload = rangePacketLength;
+  const int32_t lppDataLength = payloadLength - startOfLppDataInPayload;
+  const int32_t lppTypeInPayload = startOfLppDataInPayload + 1;
+
+  if (lppDataLength > 0) {
+    const uint8_t lppPacketHeader = rxPacket->payload[startOfLppDataInPayload];
+    if (lppPacketHeader == LPP_HEADER_SHORT_PACKET) {
+      const int32_t lppTypeAndPayloadLength = lppDataLength - 1;
+      handleLppShortPacket(anchorCtx, &rxPacket->payload[lppTypeInPayload], lppTypeAndPayloadLength);
+    }
+  }
+}
+
+static void rxcallback(dwDevice_t *dev) {
+  tdoaStats_t* stats = &tdoaEngineState.stats;
+  STATS_CNT_RATE_EVENT(&stats->packetsReceived);
+
+  int dataLength = dwGetDataLength(dev);
+  packet_t rxPacket;
+
+  dwGetData(dev, (uint8_t*)&rxPacket, dataLength);
+  const uint8_t anchorId = rxPacket.sourceAddress & 0xff;
+
+  dwTime_t arrival = {.full = 0};
+  dwGetReceiveTimestamp(dev, &arrival);
+  const int64_t rxAn_by_T_in_cl_T = arrival.full;
+
+  const rangePacket3_t* packet = (rangePacket3_t*)rxPacket.payload;
+  if (packet->header.type == PACKET_TYPE_TDOA3) {
+    const int64_t txAn_in_cl_An = packet->header.txTimeStamp;;
+    const uint8_t seqNr = packet->header.seq & 0x7f;;
+
+    tdoaAnchorContext_t anchorCtx;
+    uint32_t now_ms = T2M(xTaskGetTickCount());
+
+    tdoaEngineGetAnchorCtxForPacketProcessing(&tdoaEngineState, anchorId, now_ms, &anchorCtx);
+    int rangeDataLength = updateRemoteData(&anchorCtx, packet);
+    tdoaEngineProcessPacket(&tdoaEngineState, &anchorCtx, txAn_in_cl_An, rxAn_by_T_in_cl_T);
+    tdoaStorageSetRxTxData(&anchorCtx, rxAn_by_T_in_cl_T, txAn_in_cl_An, seqNr);
+    handleLppPacket(dataLength, rangeDataLength, &rxPacket, &anchorCtx);
+
+    rangingOk = true;
+  }
+}
+
+static void setRadioInReceiveMode(dwDevice_t *dev) {
+  dwNewReceive(dev);
+  dwSetDefaults(dev);
+  dwStartReceive(dev);
+}
+
+static void sendLppShort(dwDevice_t *dev, lpsLppShortPacket_t *packet)
+{
+  static packet_t txPacket;
+  dwIdle(dev);
+
+  MAC80215_PACKET_INIT(txPacket, MAC802154_TYPE_DATA);
+
+  txPacket.payload[LPS_TDOA3_TYPE] = LPP_HEADER_SHORT_PACKET;
+  memcpy(&txPacket.payload[LPS_TDOA3_SEND_LPP_PAYLOAD], packet->data, packet->length);
+
+  txPacket.pan = 0xbccf;
+  txPacket.sourceAddress = 0xbccf000000000000 | 0xff;
+  txPacket.destAddress = 0xbccf000000000000 | packet->dest;
+
+  dwNewTransmit(dev);
+  dwSetDefaults(dev);
+  dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+1+packet->length);
+
+  dwStartTransmit(dev);
+}
+
+static bool sendLpp(dwDevice_t *dev) {
+  bool lppPacketToSend = lpsGetLppShort(&lppPacket);
+  if (lppPacketToSend) {
+    sendLppShort(dev, &lppPacket);
+    return true;
+  }
+
+  return false;
+}
+
+static uint32_t onEvent(dwDevice_t *dev, uwbEvent_t event) {
+  switch(event) {
+    case eventPacketReceived:
+      rxcallback(dev);
+      break;
+    case eventTimeout:
+      break;
+    case eventReceiveTimeout:
+      break;
+    case eventPacketSent:
+      // Service packet sent, the radio is back to receive automatically
+      break;
+    default:
+      ASSERT_FAILED();
+  }
+
+  if(!sendLpp(dev)) {
+    setRadioInReceiveMode(dev);
+  }
+
+  uint32_t now_ms = T2M(xTaskGetTickCount());
+  tdoaStatsUpdate(&tdoaEngineState.stats, now_ms);
+
+  return MAX_TIMEOUT;
+}
+
+static void sendTdoaToEstimatorCallback(tdoaMeasurement_t* tdoaMeasurement) {
+  estimatorEnqueueTDOA(tdoaMeasurement);
+
+  #ifdef LPS_2D_POSITION_HEIGHT
+  // If LPS_2D_POSITION_HEIGHT is defined we assume that we are doing 2D positioning.
+  // LPS_2D_POSITION_HEIGHT contains the height (Z) that the tag will be located at
+  heightMeasurement_t heightData;
+  heightData.timestamp = xTaskGetTickCount();
+  heightData.height = LPS_2D_POSITION_HEIGHT;
+  heightData.stdDev = 0.0001;
+  estimatorEnqueueAbsoluteHeight(&heightData);
+  #endif
+}
+
+static bool getAnchorPosition(const uint8_t anchorId, point_t* position) {
+  tdoaAnchorContext_t anchorCtx;
+  uint32_t now_ms = T2M(xTaskGetTickCount());
+
+  bool contextFound = tdoaStorageGetAnchorCtx(tdoaEngineState.anchorInfoArray, anchorId, now_ms, &anchorCtx);
+  if (contextFound) {
+    tdoaStorageGetAnchorPosition(&anchorCtx, position);
+    return true;
+  }
+
+  return false;
+}
+
+static uint8_t getAnchorIdList(uint8_t unorderedAnchorList[], const int maxListSize) {
+  return tdoaStorageGetListOfAnchorIds(tdoaEngineState.anchorInfoArray, unorderedAnchorList, maxListSize);
+}
+
+static uint8_t getActiveAnchorIdList(uint8_t unorderedAnchorList[], const int maxListSize) {
+  uint32_t now_ms = T2M(xTaskGetTickCount());
+  return tdoaStorageGetListOfActiveAnchorIds(tdoaEngineState.anchorInfoArray, unorderedAnchorList, maxListSize, now_ms);
+}
+
+static void Initialize(dwDevice_t *dev) {
+  uint32_t now_ms = T2M(xTaskGetTickCount());
+  tdoaEngineInit(&tdoaEngineState, now_ms, sendTdoaToEstimatorCallback, LOCODECK_TS_FREQ, TdoaEngineMatchingAlgorithmRandom);
+
+  #ifdef LPS_2D_POSITION_HEIGHT
+  DEBUG_PRINT("2D positioning enabled at %f m height\n", LPS_2D_POSITION_HEIGHT);
+  #endif
+
+  dwSetReceiveWaitTimeout(dev, TDOA3_RECEIVE_TIMEOUT);
+
+  dwCommitConfiguration(dev);
+
+  rangingOk = false;
+}
+
+static bool isRangingOk()
+{
+  return rangingOk;
+}
+
+uwbAlgorithm_t uwbTdoa3TagAlgorithm = {
+  .init = Initialize,
+  .onEvent = onEvent,
+  .isRangingOk = isRangingOk,
+  .getAnchorPosition = getAnchorPosition,
+  .getAnchorIdList = getAnchorIdList,
+  .getActiveAnchorIdList = getActiveAnchorIdList,
+};

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/lpsTwrTag.c

+/*
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * LPS node firmware.
+ *
+ * Copyright 2016, Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Foobar 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 Foobar.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/* uwb_twr_anchor.c: Uwb two way ranging anchor implementation */
+
+
+#include <string.h>
+#include <math.h>
+
+#include "lpsTwrTag.h"
+#include "lpsTdma.h"
+
+#include "FreeRTOS.h"
+#include "task.h"
+
+#include "log.h"
+#include "crtp_localization_service.h"
+
+#include "stabilizer_types.h"
+#include "estimator.h"
+#include "cf_math.h"
+
+#include "physicalConstants.h"
+#include "configblock.h"
+#include "lpsTdma.h"
+#include "static_mem.h"
+
+// Config
+static lpsTwrAlgoOptions_t defaultOptions = {
+   .tagAddress = 0xbccf000000000008,
+   .anchorAddress = {
+     0xbccf000000000000,
+     0xbccf000000000001,
+     0xbccf000000000002,
+     0xbccf000000000003,
+     0xbccf000000000004,
+     0xbccf000000000005,
+ #if LOCODECK_NR_OF_TWR_ANCHORS > 6
+     0xbccf000000000006,
+ #endif
+ #if LOCODECK_NR_OF_TWR_ANCHORS > 7
+     0xbccf000000000007,
+ #endif
+   },
+   .antennaDelay = LOCODECK_ANTENNA_DELAY,
+   .rangingFailedThreshold = 6,
+
+   .combinedAnchorPositionOk = false,
+
+ #ifdef LPS_TDMA_ENABLE
+   .useTdma = true,
+   .tdmaSlot = TDMA_SLOT,
+ #endif
+
+   // To set a static anchor position from startup, uncomment and modify the
+   // following code:
+ //   .anchorPosition = {
+ //     {timestamp: 1, x: 0.99, y: 1.49, z: 1.80},
+ //     {timestamp: 1, x: 0.99, y: 3.29, z: 1.80},
+ //     {timestamp: 1, x: 4.67, y: 2.54, z: 1.80},
+ //     {timestamp: 1, x: 0.59, y: 2.27, z: 0.20},
+ //     {timestamp: 1, x: 4.70, y: 3.38, z: 0.20},
+ //     {timestamp: 1, x: 4.70, y: 1.14, z: 0.20},
+ //   },
+ //
+ //   .combinedAnchorPositionOk = true,
+};
+
+typedef struct {
+  float distance[LOCODECK_NR_OF_TWR_ANCHORS];
+  float pressures[LOCODECK_NR_OF_TWR_ANCHORS];
+  int failedRanging[LOCODECK_NR_OF_TWR_ANCHORS];
+} twrState_t;
+
+static twrState_t state;
+static lpsTwrAlgoOptions_t* options = &defaultOptions;
+
+// Outlier rejection
+#define RANGING_HISTORY_LENGTH 32
+#define OUTLIER_TH 4
+NO_DMA_CCM_SAFE_ZERO_INIT static struct {
+  float32_t history[RANGING_HISTORY_LENGTH];
+  size_t ptr;
+} rangingStats[LOCODECK_NR_OF_TWR_ANCHORS];
+
+// Rangin statistics
+static uint8_t rangingPerSec[LOCODECK_NR_OF_TWR_ANCHORS];
+static uint8_t rangingSuccessRate[LOCODECK_NR_OF_TWR_ANCHORS];
+// Used to calculate above values
+static uint8_t succededRanging[LOCODECK_NR_OF_TWR_ANCHORS];
+static uint8_t failedRanging[LOCODECK_NR_OF_TWR_ANCHORS];
+
+// Timestamps for ranging
+static dwTime_t poll_tx;
+static dwTime_t poll_rx;
+static dwTime_t answer_tx;
+static dwTime_t answer_rx;
+static dwTime_t final_tx;
+static dwTime_t final_rx;
+
+static packet_t txPacket;
+static volatile uint8_t curr_seq = 0;
+static uint8_t current_anchor = 0;
+
+static bool ranging_complete = false;
+static bool lpp_transaction = false;
+
+static lpsLppShortPacket_t lppShortPacket;
+
+// TDMA handling
+static bool tdmaSynchronized;
+static dwTime_t frameStart;
+
+static bool rangingOk;
+
+static void lpsHandleLppShortPacket(const uint8_t srcId, const uint8_t *data);
+
+static void txcallback(dwDevice_t *dev)
+{
+  dwTime_t departure;
+  dwGetTransmitTimestamp(dev, &departure);
+  departure.full += (options->antennaDelay / 2);
+
+  switch (txPacket.payload[0]) {
+    case LPS_TWR_POLL:
+      poll_tx = departure;
+      break;
+    case LPS_TWR_FINAL:
+      final_tx = departure;
+      break;
+  }
+}
+
+
+static uint32_t rxcallback(dwDevice_t *dev) {
+  dwTime_t arival = { .full=0 };
+  int dataLength = dwGetDataLength(dev);
+
+  if (dataLength == 0) return 0;
+
+  packet_t rxPacket;
+  memset(&rxPacket, 0, MAC802154_HEADER_LENGTH);
+
+  dwGetData(dev, (uint8_t*)&rxPacket, dataLength);
+
+  if (rxPacket.destAddress != options->tagAddress) {
+    dwNewReceive(dev);
+    dwSetDefaults(dev);
+    dwStartReceive(dev);
+    return MAX_TIMEOUT;
+  }
+
+  txPacket.destAddress = rxPacket.sourceAddress;
+  txPacket.sourceAddress = rxPacket.destAddress;
+
+  switch(rxPacket.payload[LPS_TWR_TYPE]) {
+    // Tag received messages
+    case LPS_TWR_ANSWER:
+      if (rxPacket.payload[LPS_TWR_SEQ] != curr_seq) {
+        return 0;
+      }
+
+      if (dataLength - MAC802154_HEADER_LENGTH > 3) {
+        if (rxPacket.payload[LPS_TWR_LPP_HEADER] == LPP_HEADER_SHORT_PACKET) {
+          int srcId = -1;
+
+          for (int i=0; i<LOCODECK_NR_OF_TWR_ANCHORS; i++) {
+            if (rxPacket.sourceAddress == options->anchorAddress[i]) {
+              srcId = i;
+              break;
+            }
+          }
+
+          if (srcId >= 0) {
+            lpsHandleLppShortPacket(srcId, &rxPacket.payload[LPS_TWR_LPP_TYPE]);
+          }
+        }
+      }
+
+      txPacket.payload[LPS_TWR_TYPE] = LPS_TWR_FINAL;
+      txPacket.payload[LPS_TWR_SEQ] = rxPacket.payload[LPS_TWR_SEQ];
+
+      dwGetReceiveTimestamp(dev, &arival);
+      arival.full -= (options->antennaDelay / 2);
+      answer_rx = arival;
+
+      dwNewTransmit(dev);
+      dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+2);
+
+      dwWaitForResponse(dev, true);
+      dwStartTransmit(dev);
+
+      break;
+    case LPS_TWR_REPORT:
+    {
+      lpsTwrTagReportPayload_t *report = (lpsTwrTagReportPayload_t *)(rxPacket.payload+2);
+      double tround1, treply1, treply2, tround2, tprop_ctn, tprop;
+
+      if (rxPacket.payload[LPS_TWR_SEQ] != curr_seq) {
+        return 0;
+      }
+
+      memcpy(&poll_rx, &report->pollRx, 5);
+      memcpy(&answer_tx, &report->answerTx, 5);
+      memcpy(&final_rx, &report->finalRx, 5);
+
+      tround1 = answer_rx.low32 - poll_tx.low32;
+      treply1 = answer_tx.low32 - poll_rx.low32;
+      tround2 = final_rx.low32 - answer_tx.low32;
+      treply2 = final_tx.low32 - answer_rx.low32;
+
+      tprop_ctn = ((tround1*tround2) - (treply1*treply2)) / (tround1 + tround2 + treply1 + treply2);
+
+      tprop = tprop_ctn / LOCODECK_TS_FREQ;
+      state.distance[current_anchor] = SPEED_OF_LIGHT * tprop;
+      state.pressures[current_anchor] = report->asl;
+
+      // Outliers rejection
+      rangingStats[current_anchor].ptr = (rangingStats[current_anchor].ptr + 1) % RANGING_HISTORY_LENGTH;
+      float32_t mean;
+      float32_t stddev;
+
+      arm_std_f32(rangingStats[current_anchor].history, RANGING_HISTORY_LENGTH, &stddev);
+      arm_mean_f32(rangingStats[current_anchor].history, RANGING_HISTORY_LENGTH, &mean);
+      float32_t diff = fabsf(mean - state.distance[current_anchor]);
+
+      rangingStats[current_anchor].history[rangingStats[current_anchor].ptr] = state.distance[current_anchor];
+
+      rangingOk = true;
+
+      if ((options->combinedAnchorPositionOk || options->anchorPosition[current_anchor].timestamp) &&
+          (diff < (OUTLIER_TH*stddev))) {
+        distanceMeasurement_t dist;
+        dist.distance = state.distance[current_anchor];
+        dist.x = options->anchorPosition[current_anchor].x;
+        dist.y = options->anchorPosition[current_anchor].y;
+        dist.z = options->anchorPosition[current_anchor].z;
+        dist.anchorId = current_anchor;
+        dist.stdDev = 0.25;
+        estimatorEnqueueDistance(&dist);
+      }
+
+      if (options->useTdma && current_anchor == 0) {
+        // Final packet is sent by us and received by the anchor
+        // We use it as synchonisation time for TDMA
+        dwTime_t offset = { .full =final_tx.full - final_rx.full };
+        frameStart.full = TDMA_LAST_FRAME(final_rx.full) + offset.full;
+        tdmaSynchronized = true;
+      }
+
+      ranging_complete = true;
+
+      return 0;
+      break;
+    }
+  }
+  return MAX_TIMEOUT;
+}
+
+/* Adjust time for schedule transfer by DW1000 radio. Set 9 LSB to 0 */
+static uint32_t adjustTxRxTime(dwTime_t *time)
+{
+  uint32_t added = (1<<9) - (time->low32 & ((1<<9)-1));
+  time->low32 = (time->low32 & ~((1<<9)-1)) + (1<<9);
+  return added;
+}
+
+/* Calculate the transmit time for a given timeslot in the current frame */
+static dwTime_t transmitTimeForSlot(int slot)
+{
+  dwTime_t transmitTime = { .full = 0 };
+  // Calculate start of the slot
+  transmitTime.full = frameStart.full + slot*TDMA_SLOT_LEN;
+
+  // DW1000 can only schedule time with 9 LSB at 0, adjust for it
+  adjustTxRxTime(&transmitTime);
+  return transmitTime;
+}
+
+static void initiateRanging(dwDevice_t *dev)
+{
+  if (!options->useTdma || tdmaSynchronized) {
+    if (options->useTdma) {
+      // go to next TDMA frame
+      frameStart.full += TDMA_FRAME_LEN;
+    }
+
+    current_anchor ++;
+    if (current_anchor >= LOCODECK_NR_OF_TWR_ANCHORS) {
+      current_anchor = 0;
+    }
+  } else {
+    current_anchor = 0;
+  }
+
+  dwIdle(dev);
+
+  txPacket.payload[LPS_TWR_TYPE] = LPS_TWR_POLL;
+  txPacket.payload[LPS_TWR_SEQ] = ++curr_seq;
+
+  txPacket.sourceAddress = options->tagAddress;
+  txPacket.destAddress = options->anchorAddress[current_anchor];
+
+  dwNewTransmit(dev);
+  dwSetDefaults(dev);
+  dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+2);
+
+  if (options->useTdma && tdmaSynchronized) {
+    dwTime_t txTime = transmitTimeForSlot(options->tdmaSlot);
+    dwSetTxRxTime(dev, txTime);
+  }
+
+  dwWaitForResponse(dev, true);
+  dwStartTransmit(dev);
+}
+
+static void sendLppShort(dwDevice_t *dev, lpsLppShortPacket_t *packet)
+{
+  dwIdle(dev);
+
+  txPacket.payload[LPS_TWR_TYPE] = LPS_TWR_LPP_SHORT;
+  memcpy(&txPacket.payload[LPS_TWR_SEND_LPP_PAYLOAD], packet->data, packet->length);
+
+  txPacket.sourceAddress = options->tagAddress;
+  txPacket.destAddress = options->anchorAddress[packet->dest];
+
+  dwNewTransmit(dev);
+  dwSetDefaults(dev);
+  dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+1+packet->length);
+
+  dwWaitForResponse(dev, false);
+  dwStartTransmit(dev);
+}
+
+static uint32_t twrTagOnEvent(dwDevice_t *dev, uwbEvent_t event)
+{
+  static uint32_t statisticStartTick = 0;
+
+  if (statisticStartTick == 0) {
+    statisticStartTick = xTaskGetTickCount();
+  }
+
+  switch(event) {
+    case eventPacketReceived:
+      return rxcallback(dev);
+      break;
+    case eventPacketSent:
+      txcallback(dev);
+
+      if (lpp_transaction) {
+        return 0;
+      }
+      return MAX_TIMEOUT;
+      break;
+    case eventTimeout:  // Comes back to timeout after each ranging attempt
+      {
+        uint16_t rangingState = locoDeckGetRangingState();
+        if (!ranging_complete && !lpp_transaction) {
+          rangingState &= ~(1<<current_anchor);
+          if (state.failedRanging[current_anchor] < options->rangingFailedThreshold) {
+            state.failedRanging[current_anchor] ++;
+            rangingState |= (1<<current_anchor);
+          }
+
+          locSrvSendRangeFloat(current_anchor, NAN);
+          failedRanging[current_anchor]++;
+        } else {
+          rangingState |= (1<<current_anchor);
+          state.failedRanging[current_anchor] = 0;
+
+          locSrvSendRangeFloat(current_anchor, state.distance[current_anchor]);
+          succededRanging[current_anchor]++;
+        }
+        locoDeckSetRangingState(rangingState);
+      }
+
+      // Handle ranging statistic
+      if (xTaskGetTickCount() > (statisticStartTick+1000)) {
+        statisticStartTick = xTaskGetTickCount();
+
+        for (int i=0; i<LOCODECK_NR_OF_TWR_ANCHORS; i++) {
+          rangingPerSec[i] = failedRanging[i] + succededRanging[i];
+          if (rangingPerSec[i] > 0) {
+            rangingSuccessRate[i] = 100.0f*(float)succededRanging[i] / (float)rangingPerSec[i];
+          } else {
+            rangingSuccessRate[i] = 0.0f;
+          }
+
+          failedRanging[i] = 0;
+          succededRanging[i] = 0;
+        }
+      }
+
+
+      if (lpsGetLppShort(&lppShortPacket)) {
+        lpp_transaction = true;
+        sendLppShort(dev, &lppShortPacket);
+      } else {
+        lpp_transaction = false;
+        ranging_complete = false;
+        initiateRanging(dev);
+      }
+      return MAX_TIMEOUT;
+      break;
+    case eventReceiveTimeout:
+    case eventReceiveFailed:
+      return 0;
+      break;
+    default:
+      configASSERT(false);
+  }
+
+  return MAX_TIMEOUT;
+}
+
+// Loco Posisioning Protocol (LPP) handling
+static void lpsHandleLppShortPacket(const uint8_t srcId, const uint8_t *data)
+{
+  uint8_t type = data[0];
+
+  if (type == LPP_SHORT_ANCHORPOS) {
+    if (srcId < LOCODECK_NR_OF_TWR_ANCHORS) {
+      struct lppShortAnchorPos_s *newpos = (struct lppShortAnchorPos_s*)&data[1];
+      options->anchorPosition[srcId].timestamp = xTaskGetTickCount();
+      options->anchorPosition[srcId].x = newpos->x;
+      options->anchorPosition[srcId].y = newpos->y;
+      options->anchorPosition[srcId].z = newpos->z;
+    }
+  }
+}
+
+static void updateTagTdmaSlot(lpsTwrAlgoOptions_t * options)
+{
+  if (options->tdmaSlot < 0) {
+    uint64_t radioAddress = configblockGetRadioAddress();
+    int nslot = 1;
+    for (int i=0; i<TDMA_NSLOTS_BITS; i++) {
+      nslot *= 2;
+    }
+    options->tdmaSlot = radioAddress % nslot;
+  }
+  options->tagAddress += options->tdmaSlot;
+}
+
+
+static void twrTagInit(dwDevice_t *dev)
+{
+  updateTagTdmaSlot(options);
+
+  // Initialize the packet in the TX buffer
+  memset(&txPacket, 0, sizeof(txPacket));
+  MAC80215_PACKET_INIT(txPacket, MAC802154_TYPE_DATA);
+  txPacket.pan = 0xbccf;
+
+  memset(&poll_tx, 0, sizeof(poll_tx));
+  memset(&poll_rx, 0, sizeof(poll_rx));
+  memset(&answer_tx, 0, sizeof(answer_tx));
+  memset(&answer_rx, 0, sizeof(answer_rx));
+  memset(&final_tx, 0, sizeof(final_tx));
+  memset(&final_rx, 0, sizeof(final_rx));
+
+  curr_seq = 0;
+  current_anchor = 0;
+
+  locoDeckSetRangingState(0);
+  ranging_complete = false;
+
+  tdmaSynchronized = false;
+
+  memset(state.distance, 0, sizeof(state.distance));
+  memset(state.pressures, 0, sizeof(state.pressures));
+  memset(state.failedRanging, 0, sizeof(state.failedRanging));
+
+  dwSetReceiveWaitTimeout(dev, TWR_RECEIVE_TIMEOUT);
+
+  dwCommitConfiguration(dev);
+
+  rangingOk = false;
+}
+
+static bool isRangingOk()
+{
+  return rangingOk;
+}
+
+void uwbTwrTagSetOptions(lpsTwrAlgoOptions_t* newOptions) {
+  options = newOptions;
+}
+
+float lpsTwrTagGetDistance(const uint8_t anchorId) {
+  return state.distance[anchorId];
+}
+
+static bool getAnchorPosition(const uint8_t anchorId, point_t* position) {
+  if (anchorId < LOCODECK_NR_OF_TWR_ANCHORS) {
+    *position = options->anchorPosition[anchorId];
+    return true;
+  }
+
+  return false;
+}
+
+static uint8_t getAnchorIdList(uint8_t unorderedAnchorList[], const int maxListSize) {
+  for (int i = 0; i < LOCODECK_NR_OF_TWR_ANCHORS; i++) {
+    unorderedAnchorList[i] = i;
+  }
+
+  return LOCODECK_NR_OF_TWR_ANCHORS;
+}
+
+static uint8_t getActiveAnchorIdList(uint8_t unorderedAnchorList[], const int maxListSize) {
+  uint8_t count = 0;
+
+  for (int i = 0; i < LOCODECK_NR_OF_TWR_ANCHORS; i++) {
+    if (state.failedRanging[i] < options->rangingFailedThreshold) {
+      unorderedAnchorList[count] = i;
+      count++;
+    }
+  }
+
+  return count;
+}
+
+uwbAlgorithm_t uwbTwrTagAlgorithm = {
+  .init = twrTagInit,
+  .onEvent = twrTagOnEvent,
+  .isRangingOk = isRangingOk,
+  .getAnchorPosition = getAnchorPosition,
+  .getAnchorIdList = getAnchorIdList,
+  .getActiveAnchorIdList = getActiveAnchorIdList,
+};
+
+/**
+ * Log group for Two Way Ranging data
+ */
+LOG_GROUP_START(twr)
+
+/**
+ * @brief Successful ranging ratio with anchor 0 [%]
+ */
+LOG_ADD(LOG_UINT8, rangingSuccessRate0, &rangingSuccessRate[0])
+
+/**
+ * @brief Ranging attempt rate with anchor 0 [1/s]
+ */
+LOG_ADD(LOG_UINT8, rangingPerSec0, &rangingPerSec[0])
+
+/**
+ * @brief Successful ranging ratio with anchor 1 [%]
+ */
+LOG_ADD(LOG_UINT8, rangingSuccessRate1, &rangingSuccessRate[1])
+
+/**
+ * @brief Ranging attempt rate with anchor 1 [1/s]
+ */
+LOG_ADD(LOG_UINT8, rangingPerSec1, &rangingPerSec[1])
+
+/**
+ * @brief Successful ranging ratio with anchor 2 [%]
+ */
+LOG_ADD(LOG_UINT8, rangingSuccessRate2, &rangingSuccessRate[2])
+
+/**
+ * @brief Ranging attempt rate with anchor 2 [1/s]
+ */
+LOG_ADD(LOG_UINT8, rangingPerSec2, &rangingPerSec[2])
+
+/**
+ * @brief Successful ranging ratio with anchor 3 [%]
+ */
+LOG_ADD(LOG_UINT8, rangingSuccessRate3, &rangingSuccessRate[3])
+
+/**
+ * @brief Ranging attempt rate with anchor 3 [1/s]
+ */
+LOG_ADD(LOG_UINT8, rangingPerSec3, &rangingPerSec[3])
+
+/**
+ * @brief Successful ranging ratio with anchor 4 [%]
+ */
+LOG_ADD(LOG_UINT8, rangingSuccessRate4, &rangingSuccessRate[4])
+
+/**
+ * @brief Ranging attempt rate with anchor 4 [1/s]
+ */
+LOG_ADD(LOG_UINT8, rangingPerSec4, &rangingPerSec[4])
+
+/**
+ * @brief Successful ranging ratio with anchor 5 [%]
+ */
+LOG_ADD(LOG_UINT8, rangingSuccessRate5, &rangingSuccessRate[5])
+
+/**
+ * @brief Ranging attempt rate with anchor 5 [1/s]
+ */
+LOG_ADD(LOG_UINT8, rangingPerSec5, &rangingPerSec[5])
+LOG_GROUP_STOP(twr)
+
+/**
+ * Log group for distances (ranges) to anchors aquired by Two Way Ranging (TWR)
+ */
+LOG_GROUP_START(ranging)
+#if (LOCODECK_NR_OF_TWR_ANCHORS > 0)
+/**
+ * @brief Distance to anchor 0 [m]
+ */
+LOG_ADD(LOG_FLOAT, distance0, &state.distance[0])
+#endif
+
+#if (LOCODECK_NR_OF_TWR_ANCHORS > 1)
+/**
+ * @brief Distance to anchor 1 [m]
+ */
+LOG_ADD(LOG_FLOAT, distance1, &state.distance[1])
+#endif
+
+#if (LOCODECK_NR_OF_TWR_ANCHORS > 2)
+/**
+ * @brief Distance to anchor 2 [m]
+ */
+LOG_ADD(LOG_FLOAT, distance2, &state.distance[2])
+#endif
+
+#if (LOCODECK_NR_OF_TWR_ANCHORS > 3)
+/**
+ * @brief Distance to anchor 3 [m]
+ */
+LOG_ADD(LOG_FLOAT, distance3, &state.distance[3])
+#endif
+
+#if (LOCODECK_NR_OF_TWR_ANCHORS > 4)
+/**
+ * @brief Distance to anchor 4 [m]
+ */
+LOG_ADD(LOG_FLOAT, distance4, &state.distance[4])
+#endif
+
+#if (LOCODECK_NR_OF_TWR_ANCHORS > 5)
+/**
+ * @brief Distance to anchor 5 [m]
+ */
+LOG_ADD(LOG_FLOAT, distance5, &state.distance[5])
+#endif
+
+#if (LOCODECK_NR_OF_TWR_ANCHORS > 6)
+/**
+ * @brief Distance to anchor 6 [m]
+ */
+LOG_ADD(LOG_FLOAT, distance6, &state.distance[6])
+#endif
+
+#if (LOCODECK_NR_OF_TWR_ANCHORS > 7)
+/**
+ * @brief Distance to anchor 7 [m]
+ */
+LOG_ADD(LOG_FLOAT, distance7, &state.distance[7])
+#endif
+
+#if (LOCODECK_NR_OF_TWR_ANCHORS > 0)
+LOG_ADD(LOG_FLOAT, pressure0, &state.pressures[0])
+#endif
+#if (LOCODECK_NR_OF_TWR_ANCHORS > 1)
+LOG_ADD(LOG_FLOAT, pressure1, &state.pressures[1])
+#endif
+#if (LOCODECK_NR_OF_TWR_ANCHORS > 2)
+LOG_ADD(LOG_FLOAT, pressure2, &state.pressures[2])
+#endif
+#if (LOCODECK_NR_OF_TWR_ANCHORS > 3)
+LOG_ADD(LOG_FLOAT, pressure3, &state.pressures[3])
+#endif
+#if (LOCODECK_NR_OF_TWR_ANCHORS > 4)
+LOG_ADD(LOG_FLOAT, pressure4, &state.pressures[4])
+#endif
+#if (LOCODECK_NR_OF_TWR_ANCHORS > 5)
+LOG_ADD(LOG_FLOAT, pressure5, &state.pressures[5])
+#endif
+#if (LOCODECK_NR_OF_TWR_ANCHORS > 6)
+LOG_ADD(LOG_FLOAT, pressure6, &state.pressures[6])
+#endif
+#if (LOCODECK_NR_OF_TWR_ANCHORS > 7)
+LOG_ADD(LOG_FLOAT, pressure7, &state.pressures[7])
+#endif
+LOG_GROUP_STOP(ranging)

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/multiranger.c

+/*
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * LPS node firmware.
+ *
+ * Copyright 2018, Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Foobar 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 Foobar.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/* multiranger.c: Multiranger deck driver */
+#include "deck.h"
+#include "param.h"
+
+#define DEBUG_MODULE "MR"
+
+#include "system.h"
+#include "debug.h"
+#include "log.h"
+#include "pca95x4.h"
+#include "vl53l1x.h"
+#include "range.h"
+#include "static_mem.h"
+
+#include "i2cdev.h"
+
+#include "FreeRTOS.h"
+#include "task.h"
+
+#include <stdlib.h>
+
+static bool isInit = false;
+static bool isTested = false;
+static bool isPassed = false;
+
+#define MR_PIN_UP     PCA95X4_P0
+#define MR_PIN_FRONT  PCA95X4_P4
+#define MR_PIN_BACK   PCA95X4_P1
+#define MR_PIN_LEFT   PCA95X4_P6
+#define MR_PIN_RIGHT  PCA95X4_P2
+
+NO_DMA_CCM_SAFE_ZERO_INIT static VL53L1_Dev_t devFront;
+NO_DMA_CCM_SAFE_ZERO_INIT static VL53L1_Dev_t devBack;
+NO_DMA_CCM_SAFE_ZERO_INIT static VL53L1_Dev_t devUp;
+NO_DMA_CCM_SAFE_ZERO_INIT static VL53L1_Dev_t devLeft;
+NO_DMA_CCM_SAFE_ZERO_INIT static VL53L1_Dev_t devRight;
+
+static bool mrInitSensor(VL53L1_Dev_t *pdev, uint32_t pca95pin, char *name)
+{
+  bool status;
+
+  // Bring up VL53 by releasing XSHUT
+  pca95x4SetOutput(pca95pin);
+  // Let VL53 boot
+  vTaskDelay(M2T(2));
+  // Init VL53
+  if (vl53l1xInit(pdev, I2C1_DEV))
+  {
+      DEBUG_PRINT("Init %s sensor [OK]\n", name);
+      status = true;
+  }
+  else
+  {
+      DEBUG_PRINT("Init %s sensor [FAIL]\n", name);
+      status = false;
+  }
+
+  return status;
+}
+
+static uint16_t mrGetMeasurementAndRestart(VL53L1_Dev_t *dev)
+{
+    VL53L1_Error status = VL53L1_ERROR_NONE;
+    VL53L1_RangingMeasurementData_t rangingData;
+    uint8_t dataReady = 0;
+    uint16_t range;
+
+    while (dataReady == 0)
+    {
+        status = VL53L1_GetMeasurementDataReady(dev, &dataReady);
+        vTaskDelay(M2T(1));
+    }
+
+    status = VL53L1_GetRangingMeasurementData(dev, &rangingData);
+    range = rangingData.RangeMilliMeter;
+
+    VL53L1_StopMeasurement(dev);
+    status = VL53L1_StartMeasurement(dev);
+    status = status;
+
+    return range;
+}
+
+static void mrTask(void *param)
+{
+    VL53L1_Error status = VL53L1_ERROR_NONE;
+
+    systemWaitStart();
+
+    // Restart all sensors
+    status = VL53L1_StopMeasurement(&devFront);
+    status = VL53L1_StartMeasurement(&devFront);
+    status = VL53L1_StopMeasurement(&devBack);
+    status = VL53L1_StartMeasurement(&devBack);
+    status = VL53L1_StopMeasurement(&devUp);
+    status = VL53L1_StartMeasurement(&devUp);
+    status = VL53L1_StopMeasurement(&devLeft);
+    status = VL53L1_StartMeasurement(&devLeft);
+    status = VL53L1_StopMeasurement(&devRight);
+    status = VL53L1_StartMeasurement(&devRight);
+    status = status;
+
+    TickType_t lastWakeTime = xTaskGetTickCount();
+
+    while (1)
+    {
+        vTaskDelayUntil(&lastWakeTime, M2T(100));
+
+        rangeSet(rangeFront, mrGetMeasurementAndRestart(&devFront)/1000.0f);
+        rangeSet(rangeBack, mrGetMeasurementAndRestart(&devBack)/1000.0f);
+        rangeSet(rangeUp, mrGetMeasurementAndRestart(&devUp)/1000.0f);
+        rangeSet(rangeLeft, mrGetMeasurementAndRestart(&devLeft)/1000.0f);
+        rangeSet(rangeRight, mrGetMeasurementAndRestart(&devRight)/1000.0f);
+    }
+}
+
+static void mrInit()
+{
+    if (isInit)
+    {
+        return;
+    }
+
+    pca95x4Init();
+
+    pca95x4ConfigOutput(~(MR_PIN_UP |
+                          MR_PIN_RIGHT |
+                          MR_PIN_LEFT |
+                          MR_PIN_FRONT |
+                          MR_PIN_BACK));
+
+    pca95x4ClearOutput(MR_PIN_UP |
+                       MR_PIN_RIGHT |
+                       MR_PIN_LEFT |
+                       MR_PIN_FRONT |
+                       MR_PIN_BACK);
+
+    isInit = true;
+
+    xTaskCreate(mrTask, MULTIRANGER_TASK_NAME, MULTIRANGER_TASK_STACKSIZE, NULL,
+        MULTIRANGER_TASK_PRI, NULL);
+}
+
+static bool mrTest()
+{
+    if (isTested)
+    {
+        return isPassed;
+    }
+
+    isPassed = isInit;
+
+    isPassed &= mrInitSensor(&devFront, MR_PIN_FRONT, "front");
+    isPassed &= mrInitSensor(&devBack, MR_PIN_BACK, "back");
+    isPassed &= mrInitSensor(&devUp, MR_PIN_UP, "up");
+    isPassed &= mrInitSensor(&devLeft, MR_PIN_LEFT, "left");
+    isPassed &= mrInitSensor(&devRight, MR_PIN_RIGHT, "right");
+
+    isTested = true;
+
+    return isPassed;
+}
+
+static const DeckDriver multiranger_deck = {
+    .vid = 0xBC,
+    .pid = 0x0C,
+    .name = "bcMultiranger",
+
+    .usedGpio = 0, // FIXME: set the used pins
+
+    .init = mrInit,
+    .test = mrTest,
+};
+
+DECK_DRIVER(multiranger_deck);
+
+PARAM_GROUP_START(deck)
+
+/**
+ * @brief Nonzero if [Multi-ranger deck](%https://store.bitcraze.io/collections/decks/products/multi-ranger-deck) is attached
+ */
+PARAM_ADD_CORE(PARAM_UINT8 | PARAM_RONLY, bcMultiranger, &isInit)
+
+PARAM_GROUP_STOP(deck)

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/oa.c

+/*
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * LPS node firmware.
+ *
+ * Copyright 2017, Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Foobar 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 Foobar.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/* oa.c: Codename Obstacle Avoidance driver */
+#include "deck.h"
+#include "param.h"
+
+#define DEBUG_MODULE "OA"
+
+#include "system.h"
+#include "debug.h"
+#include "log.h"
+#include "pca95x4.h"
+#include "vl53l0x.h"
+
+#include "i2cdev.h"
+
+#include "FreeRTOS.h"
+#include "task.h"
+
+#include <stdlib.h>
+
+static bool isInit = false;
+static bool isTested = false;
+
+#define OA_PIN_UP     PCA95X4_P0
+#define OA_PIN_FRONT  PCA95X4_P4
+#define OA_PIN_BACK   PCA95X4_P1
+#define OA_PIN_LEFT   PCA95X4_P6
+#define OA_PIN_RIGHT  PCA95X4_P2
+
+static VL53L0xDev devFront;
+static VL53L0xDev devBack;
+static VL53L0xDev devUp;
+static VL53L0xDev devLeft;
+static VL53L0xDev devRight;
+
+static uint16_t rangeFront;
+static uint16_t rangeBack;
+static uint16_t rangeUp;
+static uint16_t rangeLeft;
+static uint16_t rangeRight;
+
+static void oaTask(void *param)
+{
+  systemWaitStart();
+
+  vl53l0xStartContinuous(&devFront, 0);
+  vl53l0xStartContinuous(&devBack, 0);
+  vl53l0xStartContinuous(&devUp, 0);
+  vl53l0xStartContinuous(&devLeft, 0);
+  vl53l0xStartContinuous(&devRight, 0);
+
+  TickType_t lastWakeTime = xTaskGetTickCount();
+
+  while(1) {
+    vTaskDelayUntil(&lastWakeTime, M2T(50));
+
+    rangeFront = vl53l0xReadRangeContinuousMillimeters(&devFront);
+    rangeBack = vl53l0xReadRangeContinuousMillimeters(&devBack);
+    rangeUp = vl53l0xReadRangeContinuousMillimeters(&devUp);
+    rangeLeft = vl53l0xReadRangeContinuousMillimeters(&devLeft);
+    rangeRight = vl53l0xReadRangeContinuousMillimeters(&devRight);
+  }
+}
+
+static void oaInit()
+{
+  if (isInit) {
+    return;
+  }
+
+  pca95x4Init();
+
+  pca95x4ConfigOutput(~(OA_PIN_UP |
+                        OA_PIN_RIGHT |
+                        OA_PIN_LEFT |
+                        OA_PIN_FRONT |
+                        OA_PIN_BACK));
+
+  pca95x4ClearOutput(OA_PIN_UP |
+                     OA_PIN_RIGHT |
+                     OA_PIN_LEFT |
+                     OA_PIN_FRONT |
+                     OA_PIN_BACK);
+
+  isInit = true;
+
+  xTaskCreate(oaTask, OA_DECK_TASK_NAME, 2*configMINIMAL_STACK_SIZE, NULL,
+              OA_DECK_TASK_PRI, NULL);
+}
+
+static bool oaTest()
+{
+  bool pass = isInit;
+
+  if (isTested) {
+    DEBUG_PRINT("Cannot test OA deck a second time\n");
+    return false;
+  }
+
+  pca95x4SetOutput(OA_PIN_FRONT);
+  if (vl53l0xInit(&devFront, I2C1_DEV, true)) {
+    DEBUG_PRINT("Init front sensor [OK]\n");
+  } else {
+    DEBUG_PRINT("Init front sensor [FAIL]\n");
+    pass = false;
+  }
+
+  pca95x4SetOutput(OA_PIN_BACK);
+  if (vl53l0xInit(&devBack, I2C1_DEV, true)) {
+    DEBUG_PRINT("Init back sensor [OK]\n");
+  } else {
+    DEBUG_PRINT("Init back sensor [FAIL]\n");
+    pass = false;
+  }
+
+  pca95x4SetOutput(OA_PIN_UP);
+  if (vl53l0xInit(&devUp, I2C1_DEV, true)) {
+    DEBUG_PRINT("Init up sensor [OK]\n");
+  } else {
+    DEBUG_PRINT("Init up sensor [FAIL]\n");
+    pass = false;
+  }
+
+  pca95x4SetOutput(OA_PIN_LEFT);
+  if (vl53l0xInit(&devLeft, I2C1_DEV, true)) {
+    DEBUG_PRINT("Init left sensor [OK]\n");
+  } else {
+    DEBUG_PRINT("Init left sensor [FAIL]\n");
+    pass = false;
+  }
+
+  pca95x4SetOutput(OA_PIN_RIGHT);
+  if (vl53l0xInit(&devRight, I2C1_DEV, true)) {
+    DEBUG_PRINT("Init right sensor [OK]\n");
+  } else {
+    DEBUG_PRINT("Init right sensor [FAIL]\n");
+    pass = false;
+  }
+
+  isTested = true;
+
+  return pass;
+}
+
+static const DeckDriver oa_deck = {
+  .vid = 0xBC,
+  .pid = 0x0B,
+  .name = "bcOA",
+
+  .usedGpio = 0,  // FIXME: set the used pins
+
+  .init = oaInit,
+  .test = oaTest,
+};
+
+DECK_DRIVER(oa_deck);
+
+PARAM_GROUP_START(deck)
+PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, bcOA, &isInit)
+PARAM_GROUP_STOP(deck)
+
+LOG_GROUP_START(oa)
+LOG_ADD(LOG_UINT16, front, &rangeFront)
+LOG_ADD(LOG_UINT16, back, &rangeBack)
+LOG_ADD(LOG_UINT16, up, &rangeUp)
+LOG_ADD(LOG_UINT16, left, &rangeLeft)
+LOG_ADD(LOG_UINT16, right, &rangeRight)
+LOG_GROUP_STOP(oa)