FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/test/activeMarkerUartTest.c

+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * 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/>.
+ *
+ * activeMarkerUartTest.c: Production test for Active marker deck UART
+ *                               Uart
+ */
+
+#include <stdbool.h>
+
+#include "FreeRTOS.h"
+#include "task.h"
+
+#include "param.h"
+#include "debug.h"
+
+#include "deck.h"
+#include "uart1.h"
+
+#define DEBUG_MODULE "ActiveMarkerDeckUartTest"
+
+#include "log.h"
+
+static bool isInit = false;
+
+static bool trigger = false;
+static bool passed = false;
+
+static void task(void *param)
+{
+  while(1) {
+    if (trigger) {
+      trigger = false;
+
+      DEBUG_PRINT("Sending...\n");
+      uart1Putchar(0x55);
+
+      uint8_t answer;
+      DEBUG_PRINT("Waiting...\n");
+      if (uart1GetDataWithDefaultTimeout(&answer)) {
+        DEBUG_PRINT("Received %02x\n", (unsigned int)answer);
+
+        if (answer == 0xaa) {
+          passed = true;
+        }
+      } else {
+        DEBUG_PRINT("Timeout!!!\n");
+      }
+
+    }
+    vTaskDelay(M2T(100));
+  }
+
+}
+
+static void init(DeckInfo *info)
+{
+  if (isInit) {
+    return;
+  }
+
+  DEBUG_PRINT("Initializing Active Marker deck Uart test driver...\n");
+
+  uart1InitWithParity(115200, uart1ParityNone);
+
+  xTaskCreate(task, "amarkUartTest",
+              configMINIMAL_STACK_SIZE, NULL, 3, NULL);
+
+  isInit = true;
+}
+
+static const DeckDriver deckDriver = {
+  .name="activeMarkerUartTest",
+
+  .init = init,
+};
+
+DECK_DRIVER(deckDriver);
+
+
+PARAM_GROUP_START(amarkUartTest)
+PARAM_ADD(PARAM_UINT8, trigger, &trigger)
+PARAM_GROUP_STOP(amarkUartTest)
+
+LOG_GROUP_START(amarkUartTest)
+LOG_ADD(LOG_UINT8, passed, &passed)
+LOG_GROUP_STOP(amarkUartTest)

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/test/aidecktest.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/>.
+ *
+ * aidecktest.c - Testing of AI deck in production
+ */
+#define DEBUG_MODULE "AIDECK-TEST"
+
+#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 "uart1.h"
+#include "uart2.h"
+
+//#define DEBUG_PRINT_COM DEBUG_PRINT
+#define DEBUG_PRINT_COM(...)
+
+#define START_UP_BYTE 0xBC
+
+static bool isInit;
+
+static void aitdecktestInit(DeckInfo *info)
+{
+  if (isInit)
+    return;
+
+  DEBUG_PRINT("Initialize AI-deck test\n");
+
+  // FOr the GAP8
+  uart1Init(115200);
+  // For the NINA
+  uart2Init(115200);
+
+  isInit = true;
+}
+
+
+static bool testOnNina(const uint8_t command, const uint8_t expected)
+{
+  uint8_t byte;
+
+  uart2Putchar(command);
+
+  if (uart2GetDataWithDefaultTimeout(&byte) == true)
+  {
+    DEBUG_PRINT_COM("[NINA] Received: 0x%02X\r\n", byte);
+    if (byte == expected)
+    {
+      return true;
+    }
+  }
+  DEBUG_PRINT_COM("[NINA] Received timeout!\r\n");
+  return false;
+}
+
+static bool testOnNinaMask(const uint8_t command, uint8_t *byte)
+{
+
+  uart2Putchar(command);
+
+  if (uart2GetDataWithDefaultTimeout(byte) == true)
+  {
+    DEBUG_PRINT_COM("Received mask: 0x%02X\r\n", *byte);
+
+    return true;
+  }
+
+  return false;
+}
+
+static bool testOnGAP8(const uint8_t command, const uint8_t expected)
+{
+  uint8_t byte;
+  uint8_t timeout_counter = 0;
+
+  uart1Putchar(command);
+
+  while(timeout_counter < 5){
+    if (uart1GetDataWithDefaultTimeout(&byte) == true)
+    {
+      DEBUG_PRINT_COM("[GAP8] Received: 0x%02X\r\n", byte);
+
+      if (byte == expected)
+      {
+        return true;
+      }
+    }
+    timeout_counter++;
+  }
+
+  return false;
+}
+
+/* At startup NINA and GAP8 should send 0xBC */
+
+/* NINA command set
+ * 0x01 - Button has been high (1=true, 0=false)
+ * 0x02 - Button has been low (1=true, 0=false)
+ * 0x03 - Reset button low status (1=pass,0=fail) (will probably just return pass, it's just for setting)
+ * 0x04 - Read GPIO mask (read out bitmask bits 0-5 as XX54 3210)
+ * 0x05 - Reset GAP8 though NINA (1=pass,0=fail) (will probably just return pass, it's just for setting)
+ */
+
+/* GAP8 command set
+ * 0x01 - Test Hyper flash (1=pass, 0=fail)
+ * 0x02 - Test camera (1=pass, 0=fail)
+ * 0x03 - Test I2C by reading EEPROM
+ * 0x40 - 5 bits for GPIO output [XA54 3210] where A=1 and 0-5 is the GPIOs in the list below (1=pass, 0=fail) (will probably just return pass, it's just for setting)
+ */
+
+/* GPIO list for 0x40
+ * 0 - GAP8_SPI_MISO
+ * 1 - GAP8_GPIO_NINA_IO
+ * 2 - GAP8_SPI_MOSI
+ * 3 - NINA_GPIO_GAP8_IO
+ * 4 - GAP8_SPI_CS0
+ * 5 - GAP8_SPI_CLK
+ */
+
+/* GPIO list for 0xC0
+ * 0 - I2C SDA
+ * 1 - I2C SCL
+ */
+
+/* Bit   - Function (1=fail, 0=pass)
+ *
+ * 0     - Nina button high
+ * 1     - Nina button low (by press)
+ * 2     - Nina button low (by IO)
+ * 3-8   - GPIO NINA <-> GAP8
+ * 9-14  - GPIO NINA <-> GAP8 inverted
+ * 15    - Hyper flash
+ * 16    - Camera
+ * 17    - I2C
+ * 18    - GAP8 reset though NINA
+ * 19    - GAP8 reset tough RST
+ * 20    - NINA reset tough RST
+ */
+
+uint32_t testmask = 0x01FFFFFUL;    // Not true, set all bits in mask to 1
+uint8_t testdone;                   // Set to 1 when testing is completed
+
+#define NINA_INIT_CHAR 0xBC
+#define GAP8_INIT_CHAR 0xBC
+
+#define NINA_BOOT_HIGH                0x01
+#define NINA_BOOT_HIGH_EXPECTED       0x01
+#define NINA_BOOT_HIGH_POS            0x00
+
+#define NINA_BOOT_LOW                 0x02
+#define NINA_BOOT_LOW_EXPECTED        0x01
+#define NINA_BOOT_LOW_POS             0x01UL
+
+#define NINA_BOOT_LOW_RESET           0x03
+#define NINA_BOOT_LOW_RESET_EXPECTED  0x01
+#define NINA_BOOT_LOW_IO_POS          0x02UL
+
+#define GAP8_GPIO_COMMAND             0x40
+#define GAP8_GPIO_MASK                0x15
+#define GAP8_GPIO_MASK_EXPECTED       0x01UL
+
+#define NINA_GAP8_GPIO_COMMAND        0x04
+#define NINA_GAP8_GPIO_POS            0x03
+#define NINA_GAP8_GPIO_INV_POS        0x09UL
+
+#define GAP8_HYPER_COMMAND            0x01
+#define GAP8_HYPER_EXPECTED           0x01
+#define GAP8_HYPER_POS                0x0FUL
+
+#define GAP8_CAMERA_COMMAND           0x02
+#define GAP8_CAMERA_EXPECTED          0x01
+#define GAP8_CAMERA_POS               0x10UL
+
+#define GAP8_I2C_COMMAND              0x03
+#define GAP8_I2C_EXPECTED             0x01
+#define GAP8_I2C_POS                  0x11UL
+
+#define NINA_GAP8_RST_COMMAND 0x05
+#define NINA_GAP8_RST_EXPECTED 0x01
+#define NINA_GAP8_RST_POS 0x12
+
+#define CF2_GAP8_RST_POS 0x13
+#define CF2_NINA_RST_POS 0x14
+
+static bool aitdecktestTest()
+{
+  bool testHasBeenTriggered = false;
+  uint8_t byte;
+  uint8_t gpio_mask;
+
+  DEBUG_PRINT("Running AI-deck test, waiting for button press\r\n");
+
+  //Reset GAP8 and NINA to start with
+  pinMode(DECK_GPIO_IO4, OUTPUT);
+  digitalWrite(DECK_GPIO_IO4, LOW);
+  vTaskDelay(10);
+  digitalWrite(DECK_GPIO_IO4, HIGH);
+  pinMode(DECK_GPIO_IO4, INPUT_PULLUP);
+
+  // Wait for the NINA to start
+  vTaskDelay(M2T(1000));
+  // Empty the buffer from NINA
+  while (uart2GetDataWithDefaultTimeout(&byte) == true)
+    ;
+
+  while (uart1GetDataWithDefaultTimeout(&byte) == true)
+    ;
+
+  while (!testHasBeenTriggered)
+  {
+    // Send test for button low to NINA
+    if (testOnNina(NINA_BOOT_LOW, NINA_BOOT_LOW_EXPECTED) == true)
+    {
+      testmask &= ~(1UL << NINA_BOOT_LOW_POS);
+      testHasBeenTriggered = true;
+    }
+
+    vTaskDelay(M2T(100));
+  }
+
+  // Send test for button high NINA
+  if (testOnNina(NINA_BOOT_HIGH, NINA_BOOT_HIGH_EXPECTED) == true)
+  {
+    testmask &= ~(1UL << NINA_BOOT_HIGH_POS);
+    DEBUG_PRINT("NINA button test [OK]\r\n");
+  }
+  else
+  {
+    DEBUG_PRINT("NINA button test [FAILED]\r\n");
+  }
+
+
+
+  if (testOnNina(NINA_BOOT_LOW_RESET, NINA_BOOT_LOW_RESET_EXPECTED) == true)
+  {
+    // Pull boot and read out button low on NINA
+
+    pinMode(DECK_GPIO_IO1, OUTPUT);
+    digitalWrite(DECK_GPIO_IO1, LOW);
+    vTaskDelay(150);
+    digitalWrite(DECK_GPIO_IO1, HIGH);
+
+    // Send reset button status to NINA (not done on NINA yet)
+    if (testOnNina(NINA_BOOT_LOW, NINA_BOOT_LOW_EXPECTED) == true)
+    {
+      testmask &= ~(1UL << NINA_BOOT_LOW_IO_POS);
+      DEBUG_PRINT("NINA boot pin test [OK]\r\n");
+    }
+    else
+    {
+      DEBUG_PRINT("NINA boot pin test [FAILED]\r\n");
+    }
+  }
+
+
+
+  // Send test for GPIO mask to GAP8 (should be optimized to it's inveterd to neighbours)
+
+  //     In GAP8, the command (GAP8_GPIO_MASK 0x04) should be removed from the mask so that it is like
+  //     GAP8_GPIO_MASK again
+
+  if (testOnGAP8(GAP8_GPIO_COMMAND|GAP8_GPIO_MASK, GAP8_GPIO_MASK_EXPECTED) == true)
+  {
+
+    vTaskDelay(M2T(100));
+    // Send test for GPIO mask to NINA
+    if (testOnNinaMask(NINA_GAP8_GPIO_COMMAND, &gpio_mask) == true)
+    {
+
+      uint32_t gpio_mask_result = (uint32_t)0X3F << NINA_GAP8_GPIO_POS;
+      gpio_mask_result ^= 0x01FFFFF;
+      gpio_mask_result |= (uint32_t)(gpio_mask ^ GAP8_GPIO_MASK) << NINA_GAP8_GPIO_POS;
+      testmask &= gpio_mask_result;
+      DEBUG_PRINT("GAP8->NINA gpio test [OK]\r\n");
+    }
+    else
+    {
+      DEBUG_PRINT("GAP8->NINA gpio test [FAILED]\r\n");
+    }
+  }
+  else
+  {
+    DEBUG_PRINT("Set GAP8 gpio mask [FAILED]\r\n");
+  }
+  vTaskDelay(M2T(100));
+
+
+  uint8_t not_mask = (~GAP8_GPIO_MASK) & 0X3F;
+
+  // Send test for ~ GPIO mask to GAP8
+  if (testOnGAP8(GAP8_GPIO_COMMAND | not_mask, GAP8_GPIO_MASK_EXPECTED) == true)
+  {
+    vTaskDelay(M2T(100));
+    // Send test for ~ GPIO mask to NINA
+    if (testOnNinaMask(NINA_GAP8_GPIO_COMMAND, &gpio_mask) == true)
+    {
+      uint32_t gpio_mask_result = (uint32_t)0X3F << NINA_GAP8_GPIO_INV_POS;
+      gpio_mask_result ^= 0x01FFFFF;
+      gpio_mask_result |= (uint32_t)(gpio_mask ^ ((~GAP8_GPIO_MASK) & 0X3F)) << NINA_GAP8_GPIO_INV_POS;
+      testmask &= gpio_mask_result;
+      DEBUG_PRINT("GAP8->NINA gpio not-test [OK]\r\n");
+    }
+    else
+    {
+      DEBUG_PRINT("GAP8->NINA gpio not-test [FAILED]\r\n");
+    }
+  }
+  else
+  {
+    DEBUG_PRINT("Set GAP8 gpio not-mask [FAILED]\r\n");
+  }
+
+
+
+
+  // Send test for Hyper flash to GAP8
+  if (testOnGAP8(GAP8_HYPER_COMMAND, GAP8_HYPER_EXPECTED) == true)
+  {
+    testmask &= ~(1UL << GAP8_HYPER_POS);
+    DEBUG_PRINT("GAP8 Hyper test [OK]\r\n");
+  }
+  else
+  {
+    DEBUG_PRINT("GAP8 Hyper test [FAILED]\r\n");
+  }
+
+  // Send test for Camera to GAP8
+  if (testOnGAP8(GAP8_CAMERA_COMMAND, GAP8_CAMERA_EXPECTED) == true)
+  {
+    testmask &= ~(1UL << GAP8_CAMERA_POS);
+    DEBUG_PRINT("GAP8 Camera test [OK]\r\n");
+  }
+  else
+  {
+    DEBUG_PRINT("GAP8 Camera test [FAILED]\r\n");
+  }
+
+  // Test I2C by GAP8 by reading the EEPROM for the address and magic number
+  //       MAGIC               0x43427830
+  //       EEPROM_I2C_ADDR     0x50
+  // NOTE: should be not be run at startup!
+
+  if (testOnGAP8(GAP8_I2C_COMMAND, GAP8_I2C_EXPECTED) == true)
+  {
+    testmask &= ~(1UL << GAP8_I2C_POS);
+    DEBUG_PRINT("GAP8 I2C test [OK]\r\n");
+  }
+  else
+  {
+    DEBUG_PRINT("GAP8 I2C test [FAILED]\r\n");
+  }
+
+  // Test RST of GAP8 though NINA
+  // (listen on GAP8 uart for hello)
+  if (testOnNina(NINA_GAP8_RST_COMMAND, NINA_GAP8_RST_EXPECTED) == true)
+  {
+    while (uart1GetDataWithDefaultTimeout(&byte) == true)
+    {
+      if (byte == GAP8_INIT_CHAR)
+      {
+        testmask &= ~(1UL << NINA_GAP8_RST_POS);
+        DEBUG_PRINT("NINA->GAP8 reset [OK]\r\n");
+        break;
+      }
+    }
+    if (byte != GAP8_INIT_CHAR)
+    {
+      DEBUG_PRINT("NINA->GAP8 reset [FAILED]\r\n");
+    }
+  }
+  else
+  {
+    DEBUG_PRINT("NINA->GAP8 reset, NINA not responding. [FAILED]\r\n");
+  }
+
+  //Test RST of both GAP8 and NINA by pulling reset
+  pinMode(DECK_GPIO_IO4, OUTPUT);
+  digitalWrite(DECK_GPIO_IO4, LOW);
+  vTaskDelay(10);
+  digitalWrite(DECK_GPIO_IO4, HIGH);
+  pinMode(DECK_GPIO_IO4, INPUT);
+
+  // (listen on GAP8 and NINA uart for 0xbc)
+  while (uart2GetDataWithDefaultTimeout(&byte) == true)
+  {
+    if (byte == NINA_INIT_CHAR)
+    {
+      testmask &= ~(1UL << CF2_NINA_RST_POS);
+      DEBUG_PRINT("NINA reset [OK]\r\n");
+      break;
+    }
+  }
+  if (byte != NINA_INIT_CHAR)
+  {
+    DEBUG_PRINT("NINA reset [FAILED]\r\n");
+  }
+
+  while (uart1GetDataWithDefaultTimeout(&byte) == true)
+  {
+    if (byte == GAP8_INIT_CHAR)
+    {
+      DEBUG_PRINT("GAP8 reset [OK]\r\n");
+      testmask &= ~(1UL << CF2_GAP8_RST_POS);
+      break;
+    }
+  }
+  if (byte != GAP8_INIT_CHAR)
+  {
+    DEBUG_PRINT("GAP8 reset [FAILED]\r\n");
+  }
+
+  // Set all tests done
+  DEBUG_PRINT("AI deck test mask: 0x%08X\r\n", testmask);
+
+
+  testdone = 1;
+
+  return (testmask == 0);
+}
+
+static const DeckDriver aitest_deck = {
+    .name = "bcAIDeckTest",
+
+    .usedPeriph = 0,
+    .usedGpio = 0, // FIXME: Edit the used GPIOs
+
+    .init = aitdecktestInit,
+    .test = aitdecktestTest,
+};
+
+DECK_DRIVER(aitest_deck);
+
+LOG_GROUP_START(aidecktest)
+LOG_ADD(LOG_UINT32, testresult, &testmask)
+LOG_ADD(LOG_UINT8, done, &testdone)
+LOG_GROUP_STOP(aidecktest)

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/test/bigquadtest.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/>.
+ *
+ * bigquadtest.c - Testing of the Big Quad deck.
+ */
+#define DEBUG_MODULE "BQ-TEST"
+
+
+#include <stdint.h>
+
+#include "FreeRTOS.h"
+#include "task.h"
+
+#include "stm32fxxx.h"
+#include "config.h"
+#include "debug.h"
+#include "deck.h"
+#include "deck_test.h"
+#include "param.h"
+
+#define VBAT_TEST_VOLTAGE_LOW      (3.0 / ((1.0 + 69.0 + 10.0) / 10.0) * 0.95) /* 0.35625 */
+#define VBAT_TEST_VOLTAGE_HIGH     (3.0 / ((1.0 + 69.0 + 10.0) / 10.0) * 1.05) /* 0.39375 */
+
+#define TEST(result, str, status) decktestEval(result, DEBUG_MODULE ": " str, status)
+
+static uint8_t testsPass = 0;
+
+static bool bigquadtestRun()
+{
+  bool status = true;
+  float readVoltage;
+  GpioRegBuf gpioSaved;
+  UBaseType_t prioSaved;
+
+  /* Raise priority to prevent task switch during testing */
+  prioSaved = uxTaskPriorityGet(NULL);
+  vTaskPrioritySet(NULL, configMAX_PRIORITIES - 1);
+
+  /* Save GPIO states */
+  decktestSaveGPIOStatesABC(&gpioSaved);
+
+  /* Test SDA and CPPM(MOSI) which should be bridged */
+  pinMode(DECK_GPIO_SDA, INPUT);
+  pinMode(DECK_GPIO_MOSI, INPUT);
+  TEST(digitalRead(DECK_GPIO_SDA) == HIGH, "SDA == HIGH", &status);
+  TEST(digitalRead(DECK_GPIO_MOSI) == HIGH, "CPPM == HIGH", &status);
+  pinMode(DECK_GPIO_MOSI, OUTPUT);
+  digitalWrite(DECK_GPIO_MOSI, LOW);
+  vTaskDelay(1);
+  TEST(digitalRead(DECK_GPIO_SDA) == LOW, "SDA == LOW", &status);
+  pinMode(DECK_GPIO_MOSI, INPUT);
+
+  /* Test SCL and BUZ(IO_4) which should be bridged */
+  pinMode(DECK_GPIO_SCL, INPUT);
+  pinMode(DECK_GPIO_IO4, INPUT);
+  TEST(digitalRead(DECK_GPIO_SCL) == HIGH, "SCL == HIGH", &status);
+  TEST(digitalRead(DECK_GPIO_IO4) == HIGH, "BUZ == HIGH", &status);
+  pinMode(DECK_GPIO_IO4, OUTPUT);
+  digitalWrite(DECK_GPIO_IO4, LOW);
+  vTaskDelay(1);
+  TEST(digitalRead(DECK_GPIO_SCL) == LOW, "SCL == LOW", &status);
+  pinMode(DECK_GPIO_IO4, INPUT);
+
+  /* Test M1(TX2), M2(IO_3), M3(IO_2), M4(RX2) */
+  /* which are pulled high with 1k */
+  pinMode(DECK_GPIO_TX2, INPUT_PULLDOWN);
+  pinMode(DECK_GPIO_RX2, INPUT_PULLDOWN);
+  pinMode(DECK_GPIO_IO2, INPUT_PULLDOWN);
+  pinMode(DECK_GPIO_IO3, INPUT_PULLDOWN);
+  TEST(digitalRead(DECK_GPIO_TX2) == HIGH, "M1(TX2) == HIGH", &status);
+  TEST(digitalRead(DECK_GPIO_IO3) == HIGH, "M2(IO_3) == HIGH", &status);
+  TEST(digitalRead(DECK_GPIO_IO2) == HIGH, "M3(IO_2) == HIGH", &status);
+  TEST(digitalRead(DECK_GPIO_RX2) == HIGH, "M4(RX2) == HIGH", &status);
+
+  /* Test TX and RX  which should be bridged */
+  pinMode(DECK_GPIO_RX1, INPUT_PULLUP);
+  pinMode(DECK_GPIO_TX1, OUTPUT);
+  digitalWrite(DECK_GPIO_TX1, LOW);
+  vTaskDelay(1);
+  TEST(digitalRead(DECK_GPIO_RX1) == LOW, "TX, RX", &status);
+  digitalWrite(DECK_GPIO_TX1, HIGH);
+  vTaskDelay(1);
+  TEST(digitalRead(DECK_GPIO_RX1) == HIGH, "TX, RX", &status);
+  pinMode(DECK_GPIO_TX1, INPUT); // restore
+
+  /* Test CUR(SCK) and VBAT(MISO) which should be bridged */
+  /* CUR set to output 3V and VBAT to measure */
+  pinMode(DECK_GPIO_SCK, OUTPUT);
+  digitalWrite(DECK_GPIO_SCK, HIGH);
+  vTaskDelay(10);
+  readVoltage = analogReadVoltage(DECK_GPIO_MISO);
+  TEST((readVoltage > VBAT_TEST_VOLTAGE_LOW &&
+        readVoltage < VBAT_TEST_VOLTAGE_HIGH),
+        "VBAT(MISO) voltage", &status);
+
+  decktestRestoreGPIOStatesABC(&gpioSaved);
+
+  if (status)
+  {
+    testsPass = 1;
+    DEBUG_PRINT("BigQuad deck test [OK]\n");
+  }
+
+  /* Restore priority */
+  vTaskPrioritySet(NULL, prioSaved);
+
+  return status;
+}
+
+static const DeckDriver bigquadtest_deck = {
+  .name = "bcBigQuadTest",
+
+  .usedPeriph = 0,
+  .usedGpio = 0,
+
+  .test = bigquadtestRun,
+};
+
+DECK_DRIVER(bigquadtest_deck);
+
+PARAM_GROUP_START(BigQuadTest)
+PARAM_ADD(PARAM_UINT8, pass, &testsPass)
+PARAM_GROUP_STOP(BigQuadTest)
+

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/test/exptest.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 "ET"
+
+#include <stdint.h>
+
+#include "stm32fxxx.h"
+#include "config.h"
+#include "debug.h"
+#include "deck.h"
+#include "deck_test.h"
+
+#include "sensors.h"
+
+//Hardware configuration
+#define ET_GPIO_PERIF   (RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_GPIOC)
+
+#define ET_GPIO_PORT_TX1  GPIOC
+#define ET_GPIO_PIN_TX1   GPIO_Pin_10
+#define ET_GPIO_PORT_RX1  GPIOC
+#define ET_GPIO_PIN_RX1   GPIO_Pin_11
+
+#define ET_GPIO_PORT_TX2  GPIOA
+#define ET_GPIO_PIN_TX2   GPIO_Pin_2
+#define ET_GPIO_PORT_RX2  GPIOA
+#define ET_GPIO_PIN_RX2   GPIO_Pin_3
+
+#define ET_GPIO_PORT_SCK  GPIOA
+#define ET_GPIO_PIN_SCK   GPIO_Pin_5
+#define ET_GPIO_PORT_MOSI GPIOA
+#define ET_GPIO_PIN_MOSI  GPIO_Pin_6
+#define ET_GPIO_PORT_MISO GPIOA
+#define ET_GPIO_PIN_MISO  GPIO_Pin_7
+
+#define ET_GPIO_PORT_SDA  GPIOB
+#define ET_GPIO_PIN_SDA   GPIO_Pin_7
+#define ET_GPIO_PORT_SCL  GPIOB
+#define ET_GPIO_PIN_SCL   GPIO_Pin_6
+
+#define ET_GPIO_PORT_IO1  GPIOB
+#define ET_GPIO_PIN_IO1   GPIO_Pin_8
+#define ET_GPIO_PORT_IO2  GPIOB
+#define ET_GPIO_PIN_IO2   GPIO_Pin_5
+#define ET_GPIO_PORT_IO3  GPIOB
+#define ET_GPIO_PIN_IO3   GPIO_Pin_4
+#define ET_GPIO_PORT_IO4  GPIOC
+#define ET_GPIO_PIN_IO4   GPIO_Pin_12
+
+#define ET_NBR_PINS         11
+#define ET_IO4_PIN          (ET_NBR_PINS - 1)
+
+typedef struct _etGpio
+{
+  GPIO_TypeDef     *port;
+  uint16_t          pin;
+  char              name[6];
+} EtGpio;
+
+static EtGpio etGpioIn[ET_NBR_PINS] =
+{
+    {ET_GPIO_PORT_TX1,  ET_GPIO_PIN_TX1, "TX1"},
+    {ET_GPIO_PORT_RX1,  ET_GPIO_PIN_RX1, "RX1"},
+    {ET_GPIO_PORT_TX2,  ET_GPIO_PIN_TX2, "TX2"},
+    {ET_GPIO_PORT_RX2,  ET_GPIO_PIN_RX2, "RX2"},
+    {ET_GPIO_PORT_SCK,  ET_GPIO_PIN_SCK, "SCK"},
+    {ET_GPIO_PORT_MOSI, ET_GPIO_PIN_MOSI, "MOSI"},
+    {ET_GPIO_PORT_MISO, ET_GPIO_PIN_MISO, "MISO"},
+    {ET_GPIO_PORT_IO1,  ET_GPIO_PIN_IO1, "IO1"},
+    {ET_GPIO_PORT_IO2,  ET_GPIO_PIN_IO2, "IO2"},
+    {ET_GPIO_PORT_IO3,  ET_GPIO_PIN_IO3, "IO3"},
+    {ET_GPIO_PORT_IO4,  ET_GPIO_PIN_IO4, "IO4"}
+};
+
+static EtGpio etGpioSDA = {ET_GPIO_PORT_SDA,  ET_GPIO_PIN_SDA, "SDA"};
+static EtGpio etGpioSCL = {ET_GPIO_PORT_SCL,  ET_GPIO_PIN_SCL, "SCL"};
+
+static bool isInit;
+
+static bool exptestTestAllPins(bool test);
+static bool exptestTestPin(EtGpio *etPin, bool test);
+
+static bool exptestRun(void)
+{
+  int i;
+  volatile int delay;
+  bool status = true;
+  GPIO_InitTypeDef GPIO_InitStructure;
+  GpioRegBuf gpioSaved;
+
+  isInit = true;
+
+  status &= sensorsManufacturingTest();
+
+
+  // Enable GPIOs
+  RCC_AHB1PeriphClockCmd(ET_GPIO_PERIF, ENABLE);
+
+  decktestSaveGPIOStatesABC(&gpioSaved);
+
+  for (i = 0; i < ET_NBR_PINS; i++)
+  {
+    //Initialize the pins as inputs
+    GPIO_InitStructure.GPIO_Pin = etGpioIn[i].pin;
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
+    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
+    GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
+    GPIO_Init(etGpioIn[i].port, &GPIO_InitStructure);
+  }
+
+  for (i = 0; i < ET_NBR_PINS && status; i++)
+  {
+    // Configure pin as output to poke others
+    GPIO_InitStructure.GPIO_Pin = etGpioIn[i].pin;
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
+    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+    GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
+    GPIO_Init(etGpioIn[i].port, &GPIO_InitStructure);
+
+    // Test high
+    GPIO_SetBits(etGpioIn[i].port, etGpioIn[i].pin);
+    for (delay = 0; delay < 1000; delay++);
+    if (!exptestTestAllPins(1))
+    {
+      status = false;
+    }
+
+    // Test low
+    GPIO_ResetBits(etGpioIn[i].port, etGpioIn[i].pin);
+    for (delay = 0; delay < 1000; delay++);
+    if (!exptestTestAllPins(0))
+    {
+      status = false;
+    }
+
+    // Restore
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
+    GPIO_Init(etGpioIn[i].port, &GPIO_InitStructure);
+  }
+
+  decktestRestoreGPIOStatesABC(&gpioSaved);
+
+  if (status)
+  {
+    // Configure SDA & SCL to turn on OK leds
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
+    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+    GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
+    GPIO_InitStructure.GPIO_Pin = etGpioSDA.pin;
+    GPIO_Init(etGpioSDA.port, &GPIO_InitStructure);
+    GPIO_InitStructure.GPIO_Pin = etGpioSCL.pin;
+    GPIO_Init(etGpioSCL.port, &GPIO_InitStructure);
+    // Turn on OK LEDs.
+    GPIO_ResetBits(etGpioSDA.port, etGpioSDA.pin);
+    GPIO_ResetBits(etGpioSCL.port, etGpioSCL.pin);
+  }
+
+  return status;
+}
+
+
+static bool exptestTestAllPins(bool test)
+{
+  int i;
+  bool status = true;
+
+  for (i = 0; i < ET_NBR_PINS; i++)
+  {
+    if (!exptestTestPin(&etGpioIn[i], test))
+    {
+      status = false;
+    }
+  }
+
+  return status;
+}
+
+static bool exptestTestPin(EtGpio *etPin, bool test)
+{
+  if (test == GPIO_ReadInputDataBit(etPin->port, etPin->pin))
+  {
+    return true;
+  }
+  else
+  {
+    DEBUG_PRINT("Pin:%s != %d [FAIL]\n", etPin->name, test);
+    return false;
+  }
+}
+
+static const DeckDriver exptest_deck = {
+  .vid = 0xBC,
+  .pid = 0xFF,
+  .name = "bcExpTest",
+
+  .usedGpio = 0,               // FIXME: Edit the used GPIOs
+
+  .test = exptestRun,
+};
+
+DECK_DRIVER(exptest_deck);

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/test/exptestBolt.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/>.
+ *
+ * exptestBolt.c - Testing of expansion port and motor connectors
+ */
+#define DEBUG_MODULE "ET"
+
+#include <stdint.h>
+
+#include "stm32fxxx.h"
+#include "config.h"
+#include "debug.h"
+#include "deck.h"
+#include "deck_test.h"
+
+#include "sensors.h"
+
+//Hardware configuration
+#define ET_GPIO_PERIF   (RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_GPIOC)
+
+#define ET_GPIO_PORT_TX1  GPIOC
+#define ET_GPIO_PIN_TX1   GPIO_Pin_10
+#define ET_GPIO_PORT_RX1  GPIOC
+#define ET_GPIO_PIN_RX1   GPIO_Pin_11
+
+#define ET_GPIO_PORT_TX2  GPIOA
+#define ET_GPIO_PIN_TX2   GPIO_Pin_2
+#define ET_GPIO_PORT_RX2  GPIOA
+#define ET_GPIO_PIN_RX2   GPIO_Pin_3
+
+#define ET_GPIO_PORT_SCK  GPIOA
+#define ET_GPIO_PIN_SCK   GPIO_Pin_5
+#define ET_GPIO_PORT_MOSI GPIOA
+#define ET_GPIO_PIN_MOSI  GPIO_Pin_6
+#define ET_GPIO_PORT_MISO GPIOA
+#define ET_GPIO_PIN_MISO  GPIO_Pin_7
+
+#define ET_GPIO_PORT_SDA  GPIOB
+#define ET_GPIO_PIN_SDA   GPIO_Pin_7
+#define ET_GPIO_PORT_SCL  GPIOB
+#define ET_GPIO_PIN_SCL   GPIO_Pin_6
+
+#define ET_GPIO_PORT_IO1  GPIOB
+#define ET_GPIO_PIN_IO1   GPIO_Pin_8
+#define ET_GPIO_PORT_IO2  GPIOB
+#define ET_GPIO_PIN_IO2   GPIO_Pin_5
+#define ET_GPIO_PORT_IO3  GPIOB
+#define ET_GPIO_PIN_IO3   GPIO_Pin_4
+#define ET_GPIO_PORT_IO4  GPIOC
+#define ET_GPIO_PIN_IO4   GPIO_Pin_12
+
+#define ET_GPIO_PORT_M1  GPIOA
+#define ET_GPIO_PIN_M1   GPIO_Pin_1
+#define ET_GPIO_PORT_M2  GPIOB
+#define ET_GPIO_PIN_M2   GPIO_Pin_11
+#define ET_GPIO_PORT_M3  GPIOA
+#define ET_GPIO_PIN_M3   GPIO_Pin_15
+#define ET_GPIO_PORT_M4  GPIOB
+#define ET_GPIO_PIN_M4   GPIO_Pin_9
+
+#define ET_GPIO_PORT_M1_OR  GPIOA
+#define ET_GPIO_PIN_M1_OR   GPIO_Pin_0
+#define ET_GPIO_PORT_M2_OR  GPIOB
+#define ET_GPIO_PIN_M2_OR   GPIO_Pin_12
+#define ET_GPIO_PORT_M3_OR  GPIOC
+#define ET_GPIO_PIN_M3_OR   GPIO_Pin_8
+#define ET_GPIO_PORT_M4_OR  GPIOC
+#define ET_GPIO_PIN_M4_OR   GPIO_Pin_15
+
+#define ET_NBR_PINS         11
+#define ET_NBR_MOTOR_PINS   8
+#define ET_NBR_SIG_PINS     4
+
+typedef struct _etGpio
+{
+  GPIO_TypeDef     *port;
+  uint16_t          pin;
+  char              name[6];
+} EtGpio;
+
+static EtGpio etGpioIn[ET_NBR_PINS] =
+{
+    {ET_GPIO_PORT_TX1,  ET_GPIO_PIN_TX1, "TX1"},
+    {ET_GPIO_PORT_RX1,  ET_GPIO_PIN_RX1, "RX1"},
+    {ET_GPIO_PORT_TX2,  ET_GPIO_PIN_TX2, "TX2"},
+    {ET_GPIO_PORT_RX2,  ET_GPIO_PIN_RX2, "RX2"},
+    {ET_GPIO_PORT_SCK,  ET_GPIO_PIN_SCK, "SCK"},
+    {ET_GPIO_PORT_MOSI, ET_GPIO_PIN_MOSI, "MOSI"},
+    {ET_GPIO_PORT_MISO, ET_GPIO_PIN_MISO, "MISO"},
+    {ET_GPIO_PORT_IO1,  ET_GPIO_PIN_IO1, "IO1"},
+    {ET_GPIO_PORT_IO2,  ET_GPIO_PIN_IO2, "IO2"},
+    {ET_GPIO_PORT_IO3,  ET_GPIO_PIN_IO3, "IO3"},
+    {ET_GPIO_PORT_IO4,  ET_GPIO_PIN_IO4, "IO4"}
+};
+
+static EtGpio etMotorGpio[ET_NBR_MOTOR_PINS] =
+{
+    {ET_GPIO_PORT_M1,      ET_GPIO_PIN_M1, "M1"},
+    {ET_GPIO_PORT_M2,      ET_GPIO_PIN_M2, "M2"},
+    {ET_GPIO_PORT_M3,      ET_GPIO_PIN_M3, "M3"},
+    {ET_GPIO_PORT_M4,      ET_GPIO_PIN_M4, "M4"},
+    {ET_GPIO_PORT_M1_OR,   ET_GPIO_PIN_M1_OR, "M1_OR"},
+    {ET_GPIO_PORT_M2_OR,   ET_GPIO_PIN_M2_OR, "M2_OR"},
+    {ET_GPIO_PORT_M3_OR,   ET_GPIO_PIN_M3_OR, "M3_OR"},
+    {ET_GPIO_PORT_M4_OR,   ET_GPIO_PIN_M4_OR, "M4_OR"}
+};
+
+static EtGpio etGpioSDA = {ET_GPIO_PORT_SDA,  ET_GPIO_PIN_SDA, "SDA"};
+static EtGpio etGpioSCL = {ET_GPIO_PORT_SCL,  ET_GPIO_PIN_SCL, "SCL"};
+
+static bool isInit;
+
+static bool exptestTestAllPins(bool test);
+static bool exptestTestPin(EtGpio *etPin, bool test);
+
+static bool exptestRun(void)
+{
+  int i;
+  volatile int delay;
+  bool status = true;
+  GPIO_InitTypeDef GPIO_InitStructure;
+  GpioRegBuf gpioSaved;
+
+  isInit = true;
+
+  status &= sensorsManufacturingTest();
+
+
+  // Enable GPIOs
+  RCC_AHB1PeriphClockCmd(ET_GPIO_PERIF, ENABLE);
+
+  decktestSaveGPIOStatesABC(&gpioSaved);
+
+  for (i = 0; i < ET_NBR_PINS; i++)
+  {
+    //Initialize the pins as inputs
+    GPIO_InitStructure.GPIO_Pin = etGpioIn[i].pin;
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
+    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
+    GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
+    GPIO_Init(etGpioIn[i].port, &GPIO_InitStructure);
+  }
+
+  for (i = 0; i < ET_NBR_PINS && status; i++)
+  {
+    // Configure pin as output to poke others
+    GPIO_InitStructure.GPIO_Pin = etGpioIn[i].pin;
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
+    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+    GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
+    GPIO_Init(etGpioIn[i].port, &GPIO_InitStructure);
+
+    // Test high
+    GPIO_SetBits(etGpioIn[i].port, etGpioIn[i].pin);
+    for (delay = 0; delay < 1000; delay++);
+    if (!exptestTestAllPins(1))
+    {
+      status = false;
+    }
+
+    // Test low
+    GPIO_ResetBits(etGpioIn[i].port, etGpioIn[i].pin);
+    for (delay = 0; delay < 1000; delay++);
+    if (!exptestTestAllPins(0))
+    {
+      status = false;
+    }
+
+    // Restore
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
+    GPIO_Init(etGpioIn[i].port, &GPIO_InitStructure);
+  }
+
+  // Do Bolt specific tests. Test motor signals
+  // Initialize the Motor signal pins as inputs
+  for (i = 0; i < ET_NBR_MOTOR_PINS; i++)
+  {
+    //Initialize the pins as inputs
+    GPIO_InitStructure.GPIO_Pin = etMotorGpio[i].pin;
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
+    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
+    GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
+    GPIO_Init(etMotorGpio[i].port, &GPIO_InitStructure);
+  }
+
+  for (delay = 0; delay < 10000; delay++);
+
+  for (i = 0; i < ET_NBR_SIG_PINS && status; i++)
+  {
+    if (!exptestTestPin(&etMotorGpio[i], 1))
+    {
+      status = false;
+    }
+  }
+
+  for (i = ET_NBR_SIG_PINS; i < ET_NBR_MOTOR_PINS && status; i++)
+  {
+    // Initialize the mosfet pins as outputs.
+    GPIO_InitStructure.GPIO_Pin = etMotorGpio[i].pin;
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
+    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
+    GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
+    GPIO_Init(etMotorGpio[i].port, &GPIO_InitStructure);
+    // Set high to enable mosfet to pull low
+    GPIO_SetBits(etMotorGpio[i].port, etMotorGpio[i].pin);
+
+    for (delay = 0; delay < 10000; delay++);
+    if (!exptestTestPin(&etMotorGpio[i-ET_NBR_SIG_PINS], 0))
+    {
+      status = false;
+    }
+  }
+
+  //decktestRestoreGPIOStatesABC(&gpioSaved);
+
+  if (status)
+  {
+    // Configure SDA & SCL to turn on OK leds
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
+    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+    GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
+    GPIO_InitStructure.GPIO_Pin = etGpioSDA.pin;
+    GPIO_Init(etGpioSDA.port, &GPIO_InitStructure);
+    GPIO_InitStructure.GPIO_Pin = etGpioSCL.pin;
+    GPIO_Init(etGpioSCL.port, &GPIO_InitStructure);
+    // Turn on OK LEDs.
+    GPIO_ResetBits(etGpioSDA.port, etGpioSDA.pin);
+    GPIO_ResetBits(etGpioSCL.port, etGpioSCL.pin);
+  }
+
+  return status;
+}
+
+
+static bool exptestTestAllPins(bool test)
+{
+  int i;
+  bool status = true;
+
+  for (i = 0; i < ET_NBR_PINS; i++)
+  {
+    if (!exptestTestPin(&etGpioIn[i], test))
+    {
+      status = false;
+    }
+  }
+
+  return status;
+}
+
+static bool exptestTestPin(EtGpio *etPin, bool test)
+{
+  if (test == GPIO_ReadInputDataBit(etPin->port, etPin->pin))
+  {
+    return true;
+  }
+  else
+  {
+    DEBUG_PRINT("Pin:%s != %d [FAIL]\n", etPin->name, test);
+    return false;
+  }
+}
+
+static const DeckDriver exptestbolt_deck = {
+  .vid = 0xBC,
+  .pid = 0xFD,
+  .name = "bcBoltTest",
+
+  .usedGpio = 0,               // FIXME: Edit the used GPIOs
+
+  .test = exptestRun,
+};
+
+DECK_DRIVER(exptestbolt_deck);

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/test/exptestRR.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/>.
+ *
+ * exptestRR.c - Testing of expansion port.
+ */
+#define DEBUG_MODULE "ET"
+
+#include <stdint.h>
+
+#include "stm32fxxx.h"
+#include "config.h"
+#include "debug.h"
+#include "deck.h"
+#include "deck_test.h"
+
+#include "sensors.h"
+
+//Hardware configuration
+#define ET_GPIO_PERIF   (RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_GPIOC)
+
+#define ET_GPIO_PORT_TX2  GPIOA
+#define ET_GPIO_PIN_TX2   GPIO_Pin_2
+#define ET_GPIO_PORT_RX2  GPIOA
+#define ET_GPIO_PIN_RX2   GPIO_Pin_3
+
+#define ET_GPIO_PORT_SDA  GPIOB
+#define ET_GPIO_PIN_SDA   GPIO_Pin_7
+#define ET_GPIO_PORT_SCL  GPIOB
+#define ET_GPIO_PIN_SCL   GPIO_Pin_6
+
+#define ET_GPIO_PORT_IO2  GPIOB
+#define ET_GPIO_PIN_IO2   GPIO_Pin_5
+#define ET_GPIO_PORT_IO3  GPIOB
+#define ET_GPIO_PIN_IO3   GPIO_Pin_4
+#define ET_GPIO_PORT_IO4  GPIOC
+#define ET_GPIO_PIN_IO4   GPIO_Pin_12
+
+#define ET_NBR_PINS         5
+#define ET_IO4_PIN          (ET_NBR_PINS - 1)
+
+typedef struct _etGpio
+{
+  GPIO_TypeDef     *port;
+  uint16_t          pin;
+  char              name[6];
+} EtGpio;
+
+static EtGpio etRRGpioIn[ET_NBR_PINS] =
+{
+    {ET_GPIO_PORT_TX2,  ET_GPIO_PIN_TX2, "TX2"},
+    {ET_GPIO_PORT_RX2,  ET_GPIO_PIN_RX2, "RX2"},
+    {ET_GPIO_PORT_IO2,  ET_GPIO_PIN_IO2, "IO2"},
+    {ET_GPIO_PORT_IO3,  ET_GPIO_PIN_IO3, "IO3"},
+    {ET_GPIO_PORT_IO4,  ET_GPIO_PIN_IO4, "IO4"}
+};
+
+static EtGpio etRRGpioSDA = {ET_GPIO_PORT_SDA,  ET_GPIO_PIN_SDA, "SDA"};
+static EtGpio etRRGpioSCL = {ET_GPIO_PORT_SCL,  ET_GPIO_PIN_SCL, "SCL"};
+
+static bool isInit;
+
+static bool exptestRRTestAllPins(bool test);
+static bool exptestRRTestPin(EtGpio *etPin, bool test);
+
+static bool exptestRRRun(void)
+{
+  int i;
+  volatile int delay;
+  bool status = true;
+  GPIO_InitTypeDef GPIO_InitStructure;
+  GpioRegBuf gpioSaved;
+
+  isInit = true;
+
+  status &= sensorsManufacturingTest();
+
+
+  // Enable GPIOs
+  RCC_AHB1PeriphClockCmd(ET_GPIO_PERIF, ENABLE);
+
+  decktestSaveGPIOStatesABC(&gpioSaved);
+
+  for (i = 0; i < ET_NBR_PINS; i++)
+  {
+    //Initialize the pins as inputs
+    GPIO_InitStructure.GPIO_Pin = etRRGpioIn[i].pin;
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
+    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
+    GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
+    GPIO_Init(etRRGpioIn[i].port, &GPIO_InitStructure);
+  }
+
+  for (i = 0; i < ET_NBR_PINS && status; i++)
+  {
+    // Configure pin as output to poke others
+    GPIO_InitStructure.GPIO_Pin = etRRGpioIn[i].pin;
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
+    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+    GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
+    GPIO_Init(etRRGpioIn[i].port, &GPIO_InitStructure);
+
+    // Test high
+    GPIO_SetBits(etRRGpioIn[i].port, etRRGpioIn[i].pin);
+    for (delay = 0; delay < 1000; delay++);
+    if (!exptestRRTestAllPins(1))
+    {
+      status = false;
+    }
+
+    // Test low
+    GPIO_ResetBits(etRRGpioIn[i].port, etRRGpioIn[i].pin);
+    for (delay = 0; delay < 1000; delay++);
+    if (!exptestRRTestAllPins(0))
+    {
+      status = false;
+    }
+
+    // Restore
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
+    GPIO_Init(etRRGpioIn[i].port, &GPIO_InitStructure);
+  }
+
+  decktestRestoreGPIOStatesABC(&gpioSaved);
+
+  if (status)
+  {
+    // Configure SDA & SCL to turn on OK leds
+    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
+    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
+    GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
+    GPIO_InitStructure.GPIO_Pin = etRRGpioSDA.pin;
+    GPIO_Init(etRRGpioSDA.port, &GPIO_InitStructure);
+    GPIO_InitStructure.GPIO_Pin = etRRGpioSCL.pin;
+    GPIO_Init(etRRGpioSCL.port, &GPIO_InitStructure);
+    // Turn on OK LEDs.
+    GPIO_ResetBits(etRRGpioSDA.port, etRRGpioSDA.pin);
+    GPIO_ResetBits(etRRGpioSCL.port, etRRGpioSCL.pin);
+  }
+
+  return status;
+}
+
+
+static bool exptestRRTestAllPins(bool test)
+{
+  int i;
+  bool status = true;
+
+  for (i = 0; i < ET_NBR_PINS; i++)
+  {
+    if (!exptestRRTestPin(&etRRGpioIn[i], test))
+    {
+      status = false;
+    }
+  }
+
+  return status;
+}
+
+static bool exptestRRTestPin(EtGpio *etPin, bool test)
+{
+  if (test == GPIO_ReadInputDataBit(etPin->port, etPin->pin))
+  {
+    return true;
+  }
+  else
+  {
+    DEBUG_PRINT("Pin:%s != %d [FAIL]\n", etPin->name, test);
+    return false;
+  }
+}
+
+static const DeckDriver exptestRR_deck = {
+  .vid = 0xBC,
+  .pid = 0xFE,
+  .name = "bcExpTestRR",
+
+  .usedGpio = 0,               // FIXME: Edit the used GPIOs
+
+  .test = exptestRRRun,
+};
+
+DECK_DRIVER(exptestRR_deck);

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/test/uart1test.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/>.
+ *
+ * uart2test.c - Uart echo implementation to test uart.
+ */
+#define DEBUG_MODULE "U1T"
+
+#include <stdint.h>
+#include <string.h>
+
+#include "FreeRTOS.h"
+#include "task.h"
+
+#include "stm32fxxx.h"
+#include "system.h"
+#include "config.h"
+#include "debug.h"
+#include "deck.h"
+#include "uart1.h"
+
+//Hardware configuration
+static bool isInit;
+
+void uart1testTask(void* arg);
+
+static void uart1testInit(DeckInfo *info)
+{
+  if(isInit)
+    return;
+
+  uart1Init(115200);
+
+  xTaskCreate(uart1testTask, UART1_TEST_TASK_NAME, configMINIMAL_STACK_SIZE, NULL, UART1_TEST_TASK_PRI, NULL);
+
+  isInit = true;
+}
+
+static bool uart1testTest()
+{
+  bool status = true;
+
+  if(!isInit)
+    return false;
+
+  return status;
+}
+
+void uart1testTask(void* arg)
+{
+  systemWaitStart();
+
+  while (1)
+  {
+    char c;
+    uart1Getchar(&c);
+    consolePutchar(c);
+    uart1Putchar(c);
+    //uart1SendDataDmaBlocking(36, (uint8_t *)"Testing UART1 DMA and it is working\n");
+  }
+}
+
+static const DeckDriver uart1test_deck = {
+//  .vid = 0xBC,
+//  .pid = 0x08,
+  .name = "bcUart1Test",
+
+  .usedPeriph = 0,
+  .usedGpio = 0,
+  .init = uart1testInit,
+  .test = uart1testTest,
+};
+
+DECK_DRIVER(uart1test_deck);

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/test/uart2test.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/>.
+ *
+ * uart2test.c - Uart echo implementation to test uart.
+ */
+#define DEBUG_MODULE "U1T"
+
+#include <stdint.h>
+#include <string.h>
+
+#include "FreeRTOS.h"
+#include "task.h"
+
+#include "stm32fxxx.h"
+#include "system.h"
+#include "config.h"
+#include "debug.h"
+#include "deck.h"
+#include "uart2.h"
+
+//Hardware configuration
+static bool isInit;
+
+void uart2testTask(void* arg);
+
+static void uart2testInit(DeckInfo *info)
+{
+  if(isInit)
+    return;
+
+  uart2Init(115200);
+
+  xTaskCreate(uart2testTask, UART2_TEST_TASK_NAME, configMINIMAL_STACK_SIZE, NULL, UART2_TEST_TASK_PRI, NULL);
+
+  isInit = true;
+}
+
+static bool uart2testTest()
+{
+  bool status = true;
+
+  if(!isInit)
+    return false;
+
+  return status;
+}
+
+void uart2testTask(void* arg)
+{
+  systemWaitStart();
+
+  while (1)
+  {
+    char c;
+    uart2Getchar(&c);
+    consolePutchar(c);
+    uart2Putchar(c);
+    //uart2SendDataDmaBlocking(36, (uint8_t *)"Testing UART2 DMA and it is working\n");
+  }
+}
+
+static const DeckDriver uart2test_deck = {
+//  .vid = 0xBC,
+//  .pid = 0x08,
+  .name = "bcUart2Test",
+
+  .usedPeriph = 0,
+  .usedGpio = 0,
+  .init = uart2testInit,
+  .test = uart2testTest,
+};
+
+DECK_DRIVER(uart2test_deck);

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/test/uarttest.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/>.
+ *
+ * uartTest.c - Testing of the deck uarts.
+ *
+ * Connect deck UART1_TX to UART2_RX and UART2_TX to UART1_RX
+ *
+ */
+#define DEBUG_MODULE "UART-TEST"
+
+
+#include <stdint.h>
+
+#include "FreeRTOS.h"
+#include "task.h"
+
+#include "stm32fxxx.h"
+#include "config.h"
+#include "debug.h"
+#include "deck.h"
+#include "uart1.h"
+#include "uart2.h"
+
+
+static const char testString[] = "ABC123";
+
+static bool uarttestRun()
+{
+  bool status = true;
+  uint8_t testChar;
+
+  uart1Init(9600);
+  uart2Init(9600);
+
+  for (int i = 0; i < sizeof(testString) && status; i++)
+  {
+    uart1Putchar(testString[i]);
+    uart2GetDataWithDefaultTimeout(&testChar);
+    if (testChar != testString[i])
+    {
+      DEBUG_PRINT(" Uart1->Uart2 [FAIL]\n");
+      status = false;
+    }
+
+    uart2Putchar(testString[i]);
+    uart1GetDataWithDefaultTimeout(&testChar);
+    if (testChar != testString[i])
+    {
+      DEBUG_PRINT(" Uart2->Uart1 [FAIL]\n");
+      status = false;
+    }
+  }
+
+  if (status)
+  {
+    DEBUG_PRINT("Read/write test [OK]\n");
+  }
+
+  return status;
+}
+
+static const DeckDriver uarttest_deck = {
+  .name = "bcUartTest",
+
+  .usedPeriph = 0,
+  .usedGpio = 0,
+
+  .test = uarttestRun,
+};
+
+DECK_DRIVER(uarttest_deck);

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/usddeck.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.
+ *
+ * usddeck.c: micro SD deck driver. Implements logging to micro SD card.
+ */
+
+#define DEBUG_MODULE "uSD"
+
+#include <stdint.h>
+#include <string.h>
+#include <stdlib.h>
+#include <ctype.h>
+#include "stm32fxxx.h"
+
+#include "FreeRTOS.h"
+#include "task.h"
+#include "timers.h"
+#include "semphr.h"
+
+#include "ff.h"
+#include "diskio.h"
+#include "fatfs_sd.h"
+
+#include "deck.h"
+#include "usddeck.h"
+#include "system.h"
+#include "sensors.h"
+#include "debug.h"
+#include "led.h"
+
+#include "statsCnt.h"
+#include "log.h"
+#include "param.h"
+#include "crc32.h"
+#include "static_mem.h"
+#include "mem.h"
+#include "eventtrigger.h"
+
+// Hardware defines
+#ifdef USDDECK_USE_ALT_PINS_AND_SPI
+#include "deck_spi3.h"
+#define USD_CS_PIN    DECK_GPIO_RX2
+
+#define SPI_BEGIN               spi3Begin
+#define USD_SPI_BAUDRATE_2MHZ   SPI3_BAUDRATE_2MHZ
+#define USD_SPI_BAUDRATE_21MHZ  SPI3_BAUDRATE_21MHZ
+#define SPI_EXCHANGE            spi3Exchange
+#define SPI_BEGIN_TRANSACTION   spi3BeginTransaction
+#define SPI_END_TRANSACTION     spi3EndTransaction
+
+
+#else
+#include "deck_spi.h"
+#define USD_CS_PIN    DECK_GPIO_IO4
+
+#define SPI_BEGIN               spiBegin
+#define USD_SPI_BAUDRATE_2MHZ   SPI_BAUDRATE_2MHZ
+#define USD_SPI_BAUDRATE_21MHZ  SPI_BAUDRATE_21MHZ
+#define SPI_EXCHANGE            spiExchange
+#define SPI_BEGIN_TRANSACTION   spiBeginTransaction
+#define SPI_END_TRANSACTION     spiEndTransaction
+#endif
+
+#define MAX_USD_LOG_VARIABLES_PER_EVENT   (20)
+#define MAX_USD_LOG_EVENTS                (20)
+#define FIXED_FREQUENCY_EVENT_ID          (0xFFFF)
+#define FIXED_FREQUENCY_EVENT_NAME        "fixedFrequency"
+
+typedef struct usdLogEventConfig_s {
+  uint16_t eventId;
+  uint8_t numVars;
+  uint16_t numBytes;
+  logVarId_t varIds[MAX_USD_LOG_VARIABLES_PER_EVENT];
+} usdLogEventConfig_t;
+
+typedef struct usdLogConfig_s {
+  char filename[13];
+  uint16_t frequency;
+  uint16_t bufferSize;
+  bool enableOnStartup;
+  enum usddeckLoggingMode_e mode;
+
+  uint32_t numEventConfigs;
+  usdLogEventConfig_t eventConfigs[MAX_USD_LOG_EVENTS];
+  uint8_t fixedFrequencyEventIdx;
+} usdLogConfig_t;
+
+typedef struct usdLogStats_s {
+  uint32_t eventsRequested;
+  uint32_t eventsWritten;
+} usdLogStats_t;
+
+// Ring buffer
+typedef struct ringBuffer_s {
+  uint8_t* buffer;        // pointer to buffer
+  uint16_t capacity;      // total capacity of buffer
+  uint16_t size;          // used size of buffer
+  uint8_t* readPtr;       // pointer for read/pop
+  uint8_t* writePtr;      // pointer for write/push
+  uint16_t popSize;       // size for ongoing pop operation
+} ringBuffer_t;
+
+void ringBuffer_init(ringBuffer_t* b, uint8_t *buffer, uint16_t capacity)
+{
+  b->buffer = buffer;
+  b->capacity = capacity;
+  b->size = 0;
+  b->readPtr = buffer;
+  b->writePtr = buffer;
+  b->popSize = 0;
+}
+
+void ringBuffer_reset(ringBuffer_t *b)
+{
+  b->size = 0;
+  b->readPtr = b->buffer;
+  b->writePtr = b->buffer;
+  b->popSize = 0;
+}
+
+uint16_t ringBuffer_availableSpace(const ringBuffer_t* b)
+{
+  return b->capacity - b->size;
+}
+
+bool ringBuffer_push(ringBuffer_t* b, const void* data, uint16_t size)
+{
+  if (ringBuffer_availableSpace(b) < size) {
+    return false;
+  }
+  const uint8_t* dataTyped = (const uint8_t*)data;
+  for (uint16_t i = 0; i < size; ++i) {
+    *(b->writePtr) = dataTyped[i];
+    ++b->writePtr;
+    if (b->writePtr ==  b->buffer + b->capacity) {
+      b->writePtr = b->buffer;
+    }
+  }
+  b->size += size;
+  return true;
+}
+
+bool ringBuffer_pop_start(ringBuffer_t* b, const uint8_t** buf, uint16_t* size)
+{
+  if (b->size == 0) {
+    return false;
+  }
+
+  *buf = b->readPtr;
+  if (b->writePtr > b->readPtr) {
+    // writer did not wrap around yet
+    *size = b->writePtr - b->readPtr;
+    b->readPtr = b->writePtr;
+  } else {
+    // wrap around -> read until end of buffer, only
+    *size = b->buffer + b->capacity - b->readPtr;
+    b->readPtr = b->buffer;
+  }
+  b->popSize = *size;
+  return true;
+}
+
+void ringBuffer_pop_done(ringBuffer_t *b)
+{
+  b->size -= b->popSize;
+  b->popSize = 0;
+}
+
+// FATFS low lever driver functions.
+static void initSpi(void);
+static void setSlowSpiMode(void);
+static void setFastSpiMode(void);
+static BYTE xchgSpi(BYTE dat);
+static void rcvrSpiMulti(BYTE *buff, UINT btr);
+static void xmitSpiMulti(const BYTE *buff, UINT btx);
+static void csHigh(BYTE doDummyClock);
+static void csLow(void);
+static void delayMs(UINT ms);
+
+static void usdLogTask(void* prm);
+static void usdWriteTask(void* prm);
+
+static STATS_CNT_RATE_DEFINE(spiWriteRate, 1000);
+static STATS_CNT_RATE_DEFINE(spiReadRate, 1000);
+static STATS_CNT_RATE_DEFINE(fatWriteRate, 1000);
+
+static usdLogConfig_t usdLogConfig;
+static usdLogStats_t usdLogStats;
+
+static BYTE exchangeBuff[512];
+static uint16_t spiSpeed;
+
+#ifdef USD_RUN_DISKIO_FUNCTION_TESTS
+DWORD workBuff[512];  /* 2048 byte working buffer */
+#endif
+
+//Fatfs object
+static FATFS FatFs;
+//File object
+static FIL logFile;
+static SemaphoreHandle_t logFileMutex;
+
+static SemaphoreHandle_t logBufferMutex;
+static ringBuffer_t logBuffer;
+static TaskHandle_t xHandleWriteTask;
+
+static bool enableLogging;
+static uint32_t lastFileSize = 0;
+static crc32Context_t crcContext;
+
+static xTimerHandle timer;
+static void usdTimer(xTimerHandle timer);
+
+
+// Handling from the memory module
+static uint32_t handleMemGetSize(void) { return usddeckFileSize(); }
+static bool handleMemRead(const uint32_t memAddr, const uint8_t readLen, uint8_t* buffer);
+static const MemoryHandlerDef_t memDef = {
+  .type = MEM_TYPE_USD,
+  .getSize = handleMemGetSize,
+  .read = handleMemRead,
+  .write = 0, // Write not supported
+};
+
+
+// Low lever driver functions
+static sdSpiContext_t sdSpiContext =
+    {
+        .initSpi = initSpi,
+        .setSlowSpiMode = setSlowSpiMode,
+        .setFastSpiMode = setFastSpiMode,
+        .xchgSpi = xchgSpi,
+        .rcvrSpiMulti = rcvrSpiMulti,
+        .xmitSpiMulti = xmitSpiMulti,
+        .csLow = csLow,
+        .csHigh = csHigh,
+        .delayMs = delayMs,
+
+        .stat = STA_NOINIT,
+        .timer1 = 0,
+        .timer2 = 0
+    };
+
+
+/*-----------------------------------------------------------------------*/
+/* FATFS SPI controls (Platform dependent)                               */
+/*-----------------------------------------------------------------------*/
+
+/* Initialize MMC interface */
+static void initSpi(void)
+{
+  SPI_BEGIN();   /* Enable SPI function */
+  spiSpeed = USD_SPI_BAUDRATE_2MHZ;
+
+  pinMode(USD_CS_PIN, OUTPUT);
+  digitalWrite(USD_CS_PIN, 1);
+
+  // FIXME: DELAY of 10ms?
+}
+
+static void setSlowSpiMode(void)
+{
+  spiSpeed = USD_SPI_BAUDRATE_2MHZ;
+}
+
+static void setFastSpiMode(void)
+{
+  spiSpeed = USD_SPI_BAUDRATE_21MHZ;
+}
+
+/* Exchange a byte */
+static BYTE xchgSpi(BYTE dat)
+{
+  BYTE receive;
+
+  STATS_CNT_RATE_EVENT(&spiReadRate);
+  STATS_CNT_RATE_EVENT(&spiWriteRate);
+
+  SPI_EXCHANGE(1, &dat, &receive);
+  return (BYTE)receive;
+}
+
+/* Receive multiple byte */
+static void rcvrSpiMulti(BYTE *buff, UINT btr)
+{
+  STATS_CNT_RATE_MULTI_EVENT(&spiReadRate, btr);
+
+  memset(exchangeBuff, 0xFFFFFFFF, btr);
+  SPI_EXCHANGE(btr, exchangeBuff, buff);
+}
+
+/* Send multiple byte */
+static void xmitSpiMulti(const BYTE *buff, UINT btx)
+{
+  STATS_CNT_RATE_MULTI_EVENT(&spiWriteRate, btx);
+
+  SPI_EXCHANGE(btx, buff, exchangeBuff);
+}
+
+static void csHigh(BYTE doDummyClock)
+{
+  digitalWrite(USD_CS_PIN, 1);
+
+  // Dummy clock (force DO hi-z for multiple slave SPI)
+  // Moved here from fatfs_sd.c to handle bus release
+  if (doDummyClock) {
+    xchgSpi(0xFF);
+  }
+
+  SPI_END_TRANSACTION();
+}
+
+static void csLow(void)
+{
+  SPI_BEGIN_TRANSACTION(spiSpeed);
+  digitalWrite(USD_CS_PIN, 0);
+}
+
+static void delayMs(UINT ms)
+{
+  vTaskDelay(M2T(ms));
+}
+
+/* FatFS Disk Interface */
+DSTATUS disk_initialize(BYTE pdrv)
+{
+    return SD_disk_initialize(&sdSpiContext);
+}
+
+DSTATUS disk_status(BYTE pdrv)
+{
+  return SD_disk_status(&sdSpiContext);
+}
+
+DRESULT disk_read(BYTE pdrv, BYTE *buff, LBA_t sector, UINT count)
+{
+  return SD_disk_read(buff, sector, count, &sdSpiContext);
+}
+
+DRESULT disk_write(BYTE pdrv, const BYTE *buff, LBA_t sector, UINT count)
+{
+  return SD_disk_write(buff, sector, count, &sdSpiContext);
+}
+
+DRESULT disk_ioctl(BYTE pdrv, BYTE cmd, void *buff)
+{
+  return SD_disk_ioctl(cmd, buff, &sdSpiContext);
+}
+
+/*-----------------------------------------------------------------------*/
+/* Get time for fatfs for files                                          */
+/*-----------------------------------------------------------------------*/
+__attribute__((weak)) DWORD get_fattime(void)
+{
+  /* Returns current time packed into a DWORD variable */
+  return ((DWORD)(2016 - 1980) << 25) /* Year 2016 */
+         | ((DWORD)1 << 21)           /* Month 1 */
+         | ((DWORD)1 << 16)           /* Mday 1 */
+         | ((DWORD)0 << 11)           /* Hour 0 */
+         | ((DWORD)0 << 5)            /* Min 0 */
+         | ((DWORD)0 >> 1);           /* Sec 0 */
+}
+
+/********** FS helper function ***************/
+
+// reads a line and returns the string without any whitespace/comment
+//  * comments are indicated by #
+//  * a line ending is marked by \n
+//  * only up to "len" will be read
+TCHAR* f_gets_without_comments (
+  TCHAR* buff,  /* Pointer to the string buffer to read */
+  int len,    /* Size of string buffer (characters) */
+  FIL* fp     /* Pointer to the file object */
+)
+{
+  int n = 0;
+  TCHAR c, *p = buff;
+  UINT rc;
+  bool isComment = false;
+
+  while (n < len - 1) { /* Read characters until buffer gets filled */
+    f_read(fp, &c, 1, &rc);
+    if (rc != 1) {
+      break;
+    }
+    if (c == '\n') {
+      break;   /* Break on EOL */
+    }
+    if (isspace((int)c)) {
+      continue; /* Strip whitespace */
+    }
+    if (c == '#') {
+      isComment = true; /* keep reading until end of line */
+    }
+    if (!isComment) {
+      *p++ = c;
+      n++;
+    }
+  }
+  *p = 0;
+  return n ? buff : 0;      /* When no data read (eof or error), return with error. */
+}
+
+
+/*********** Deck driver initialization ***************/
+
+static bool isInit = false;
+static bool initSuccess = false;
+
+static void usdInit(DeckInfo *info)
+{
+  if (!isInit) {
+    memoryRegisterHandler(&memDef);
+
+    logFileMutex = xSemaphoreCreateMutex();
+    logBufferMutex = xSemaphoreCreateMutex();
+    /* try to mount drives before creating the tasks */
+    if (f_mount(&FatFs, "", 1) == FR_OK) {
+      DEBUG_PRINT("mount SD-Card [OK].\n");
+
+      /* create usd-log task */
+      xTaskCreate(usdLogTask, USDLOG_TASK_NAME,
+                  USDLOG_TASK_STACKSIZE, NULL,
+                  USDLOG_TASK_PRI, NULL);
+    } else {
+      DEBUG_PRINT("mount SD-Card [FAIL].\n");
+    }
+  }
+  isInit = true;
+}
+
+static void usddeckWriteEventData(const usdLogEventConfig_t* cfg, const uint8_t* payload, uint8_t payloadSize)
+{
+  uint64_t ticks = usecTimestamp();
+
+  if (!enableLogging) {
+    return;
+  }
+
+  ++usdLogStats.eventsRequested;
+
+  xSemaphoreTake(logBufferMutex, portMAX_DELAY);
+
+  // trigger writing once there is some data
+  if (logBuffer.size > 0 && xHandleWriteTask) {
+    vTaskResume(xHandleWriteTask);
+  }
+
+  int dataSize = sizeof(cfg->eventId) + sizeof(ticks) + payloadSize + cfg->numBytes;
+
+  // only write if we have enough space
+  if (ringBuffer_availableSpace(&logBuffer) >= dataSize) {
+    /* write data into buffer */
+    uint16_t event_id = cfg->eventId;
+    ringBuffer_push(&logBuffer, &event_id, sizeof(event_id));
+    ringBuffer_push(&logBuffer, &ticks, sizeof(ticks));
+    if (payloadSize) {
+      ringBuffer_push(&logBuffer, payload, payloadSize);
+    }
+
+    for (int i = 0; i < cfg->numVars; ++i) {
+      logVarId_t varid = cfg->varIds[i];
+      switch (logGetType(varid)) {
+      case LOG_UINT8:
+      case LOG_INT8:
+        ringBuffer_push(&logBuffer, logGetAddress(varid), sizeof(uint8_t));
+        break;
+      case LOG_UINT16:
+      case LOG_INT16:
+        ringBuffer_push(&logBuffer, logGetAddress(varid), sizeof(uint16_t));
+        break;
+      case LOG_UINT32:
+      case LOG_INT32:
+      case LOG_FLOAT:
+        ringBuffer_push(&logBuffer, logGetAddress(varid), sizeof(uint32_t));
+        break;
+      default:
+        ASSERT(false);
+        break;
+      }
+    }
+    ++usdLogStats.eventsWritten;
+  }
+  xSemaphoreGive(logBufferMutex);
+}
+
+static void usddeckEventtriggerCallback(const eventtrigger *event)
+{
+  uint16_t eventId = eventtriggerGetId(event);
+  for (uint8_t i = 0; i < usdLogConfig.numEventConfigs; ++i) {
+    if (usdLogConfig.eventConfigs[i].eventId == eventId) {
+      usddeckWriteEventData(&usdLogConfig.eventConfigs[i], event->payload, event->payloadSize);
+      break;
+    }
+  }
+}
+
+static void usdLogTask(void* prm)
+{
+  TickType_t lastWakeTime = xTaskGetTickCount();
+
+  DEBUG_PRINT("wait for sensors\n");
+
+  systemWaitStart();
+  /* wait until sensor calibration is done
+   * (memory of bias calculation buffer is free again) */
+  while(!sensorsAreCalibrated()) {
+    vTaskDelayUntil(&lastWakeTime, F2T(10));
+  }
+
+  // loop to break out in case of errors
+  while (true) {
+    /* open config file */
+    // loop to break out in case of errors
+    while (f_open(&logFile, "config.txt", FA_READ) == FR_OK) {
+      /* try to read configuration */
+      char readBuffer[32];
+      char* endptr;
+
+      // version
+      TCHAR* line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
+      if (!line) break;
+      int version = strtol(line, &endptr, 10);
+      if (version != 1) break;
+      // buffer size
+      line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
+      if (!line) break;
+      usdLogConfig.bufferSize = strtol(line, &endptr, 10);
+      // file name
+      line = f_gets_without_comments(usdLogConfig.filename, sizeof(usdLogConfig.filename), &logFile);
+      if (!line) break;
+
+      int l = strlen(usdLogConfig.filename);
+      if (l > sizeof(usdLogConfig.filename) - 3) {
+        l = sizeof(usdLogConfig.filename) - 3;
+      }
+      usdLogConfig.filename[l] = '0';
+      usdLogConfig.filename[l+1] = '0';
+      usdLogConfig.filename[l+2] = 0;
+
+      // enable on startup
+      line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
+      if (!line) break;
+      usdLogConfig.enableOnStartup = strtol(line, &endptr, 10);
+
+      // loop over event triggers "on:<name>"
+      usdLogConfig.numEventConfigs = 0;
+      usdLogConfig.fixedFrequencyEventIdx = MAX_USD_LOG_EVENTS;
+      usdLogConfig.frequency = 10; // use non-zero default value for task loop below
+      usdLogEventConfig_t *cfg = &usdLogConfig.eventConfigs[0];
+      const char* eventName = 0;
+      line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
+      while (line) {
+        if (strncmp(line, "on:", 3) == 0) {
+          // special mode for non-event-based logging
+          if (strcmp(&line[3], FIXED_FREQUENCY_EVENT_NAME) == 0) {
+            // frequency
+            line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
+            if (!line) break;
+            usdLogConfig.frequency = strtol(line, &endptr, 10);
+            // mode
+            line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
+            if (!line) break;
+            usdLogConfig.mode = strtol(line, &endptr, 10);
+            cfg->eventId = FIXED_FREQUENCY_EVENT_ID;
+            eventName = FIXED_FREQUENCY_EVENT_NAME;
+            usdLogConfig.fixedFrequencyEventIdx = usdLogConfig.numEventConfigs;
+          } else {
+            // handle event triggers
+            const eventtrigger *et = eventtriggerGetByName(&line[3]);
+            if (et) {
+              cfg->eventId = eventtriggerGetId(et);
+              eventName = et->name;
+            } else {
+              DEBUG_PRINT("Unknown event %s\n", &line[3]);
+              line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
+              continue;
+            }
+          }
+
+          // Add log variables
+          cfg->numVars = 0;
+          cfg->numBytes = 0;
+          while (true) {
+            line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
+            if (!line || strncmp(line, "on:", 3) == 0)
+              break;
+            char *group = line;
+            char *name = 0;
+            for (int i = 0; i < strlen(line); ++i) {
+              if (line[i] == '.') {
+                line[i] = 0;
+                name = &line[i + 1];
+                i = strlen(name);
+                break;
+              }
+            }
+            logVarId_t varid = logGetVarId(group, name);
+            if (!logVarIdIsValid(varid)) {
+              DEBUG_PRINT("Unknown log variable %s.%s\n", group, name);
+              continue;
+            }
+            if (cfg->numVars < MAX_USD_LOG_VARIABLES_PER_EVENT) {
+              cfg->varIds[cfg->numVars] = varid;
+              ++cfg->numVars;
+              cfg->numBytes += logVarSize(logGetType(varid));
+            } else {
+              DEBUG_PRINT("Skip log variable %s: %s.%s (out of storage)\n", eventName, group, name);
+              continue;
+            }
+          }
+          if (usdLogConfig.numEventConfigs < MAX_USD_LOG_EVENTS - 1) {
+            ++usdLogConfig.numEventConfigs;
+            cfg = &usdLogConfig.eventConfigs[usdLogConfig.numEventConfigs];
+          } else {
+            DEBUG_PRINT("Skip config after event %s (out of storage)\n", eventName);
+            break;
+          }
+        } else {
+          line = f_gets_without_comments(readBuffer, sizeof(readBuffer), &logFile);
+        }
+      }
+      f_close(&logFile);
+
+      eventtriggerRegisterCallback(eventtriggerHandler_USD, &usddeckEventtriggerCallback);
+
+      DEBUG_PRINT("Config read [OK].\n");
+      // DEBUG_PRINT("Frequency: %d Hz. Buffer size: %d\n",
+      //             usdLogConfig.frequency, usdLogConfig.bufferSize);
+      // DEBUG_PRINT("enOnStartup: %d. mode: %d\n", usdLogConfig.enableOnStartup, usdLogConfig.mode);
+      // DEBUG_PRINT("slots: %d, %d\n", usdLogConfig.numSlots, usdLogConfig.numBytes);
+      initSuccess = true;
+      break;
+    }
+
+    if (!initSuccess) {
+      DEBUG_PRINT("Config read [FAIL].\n");
+      break;
+    }
+
+    /* allocate memory for buffer */
+    DEBUG_PRINT("malloc buffer %d bytes ", usdLogConfig.bufferSize);
+    // vTaskDelay(10); // small delay to allow debug message to be send
+    uint8_t* logBufferData = pvPortMalloc(usdLogConfig.bufferSize);
+    if (logBufferData) {
+      DEBUG_PRINT("[OK].\n");
+    } else {
+      DEBUG_PRINT("[FAIL].\n");
+      break;
+    }
+    ringBuffer_init(&logBuffer, logBufferData, usdLogConfig.bufferSize);
+
+    /* create queue to hand over pointer to usdLogData */
+    // usdLogQueue = xQueueCreate(usdLogConfig.queueSize, sizeof(uint8_t*));
+
+    xHandleWriteTask = 0;
+    enableLogging = usdLogConfig.enableOnStartup; // enable logging if desired
+
+    /* create usd-write task */
+    xTaskCreate(usdWriteTask, USDWRITE_TASK_NAME,
+                USDWRITE_TASK_STACKSIZE, 0,
+                USDWRITE_TASK_PRI, &xHandleWriteTask);
+
+    bool lastEnableLogging = enableLogging;
+    while(1) {
+      vTaskDelayUntil(&lastWakeTime, F2T(usdLogConfig.frequency));
+
+      // if logging was just disabled, resume the writer task to give up mutex
+      if (!enableLogging && lastEnableLogging != enableLogging) {
+        vTaskResume(xHandleWriteTask);
+      }
+
+      if (enableLogging && usdLogConfig.mode == usddeckLoggingMode_Asynchronous) {
+        usddeckTriggerLogging();
+      }
+      lastEnableLogging = enableLogging;
+    }
+  }
+
+  /* something went wrong */
+  vTaskDelete(NULL);
+}
+
+bool usddeckLoggingEnabled(void)
+{
+  return enableLogging;
+}
+
+enum usddeckLoggingMode_e usddeckLoggingMode(void)
+{
+  return usdLogConfig.mode;
+}
+
+int usddeckFrequency(void)
+{
+  return usdLogConfig.frequency;
+}
+
+void usddeckTriggerLogging(void)
+{
+  if (usdLogConfig.fixedFrequencyEventIdx < MAX_USD_LOG_EVENTS) {
+    usddeckWriteEventData(&usdLogConfig.eventConfigs[usdLogConfig.fixedFrequencyEventIdx], 0, 0);
+  }
+}
+
+// returns size of current file if logging is stopped (0 otherwise)
+uint32_t usddeckFileSize(void)
+{
+  return lastFileSize;
+}
+
+// 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)
+{
+  bool result = false;
+  if (initSuccess && xSemaphoreTake(logFileMutex, 0) == pdTRUE) {
+    if (f_open(&logFile, usdLogConfig.filename, FA_READ) == FR_OK) {
+      if (f_lseek(&logFile, offset) == FR_OK) {
+        UINT bytesRead;
+        FRESULT r = f_read(&logFile, buffer, length, &bytesRead);
+        f_close(&logFile);
+        if (r == FR_OK && bytesRead == length) {
+          result = true;
+        }
+      } else {
+        f_close(&logFile);
+      }
+    }
+    xSemaphoreGive(logFileMutex);
+  }
+  return result;
+}
+
+static bool handleMemRead(const uint32_t memAddr, const uint8_t readLen, uint8_t* buffer) {
+  bool result = false;
+
+  if (memAddr + readLen <= usddeckFileSize()) {
+    if (usddeckRead(memAddr, buffer, readLen)) {
+      result = true;
+    }
+  }
+
+  return result;
+}
+
+static void usdWriteData(const void *data, size_t size)
+{
+  UINT bytesWritten;
+  FRESULT status = f_write(&logFile, data, size, &bytesWritten);
+  ASSERT(status == FR_OK);
+  crc32Update(&crcContext, data, size);
+  STATS_CNT_RATE_MULTI_EVENT(&fatWriteRate, bytesWritten);
+}
+
+static void usdWriteTask(void* prm)
+{
+  /* create and start timer for card control timing */
+  timer = xTimerCreate("usdTimer", M2T(SD_DISK_TIMER_PERIOD_MS),
+                       pdTRUE, NULL, usdTimer);
+  xTimerStart(timer, 0);
+
+  vTaskDelay(M2T(50));
+
+  while (true) {
+    vTaskSuspend(NULL);
+    if (enableLogging) {
+      // reset stats
+      usdLogStats.eventsRequested = 0;
+      usdLogStats.eventsWritten = 0;
+
+      // reset the buffer
+      xSemaphoreTake(logBufferMutex, portMAX_DELAY);
+      ringBuffer_reset(&logBuffer);
+      xSemaphoreGive(logBufferMutex);
+
+      xSemaphoreTake(logFileMutex, portMAX_DELAY);
+      lastFileSize = 0;
+
+      /* look for existing files and use first not existent combination
+       * of two chars */
+      {
+        FILINFO fno;
+        uint8_t NUL = 0;
+        while(usdLogConfig.filename[NUL] != '\0') {
+          NUL++;
+        }
+        while (f_stat(usdLogConfig.filename, &fno) == FR_OK) {
+          /* increase file */
+          switch(usdLogConfig.filename[NUL-1]) {
+            case '9':
+              usdLogConfig.filename[NUL-1] = '0';
+              usdLogConfig.filename[NUL-2]++;
+              break;
+            default:
+              usdLogConfig.filename[NUL-1]++;
+          }
+        }
+      }
+
+      /* try to create file */
+      if (f_open(&logFile, usdLogConfig.filename, FA_CREATE_ALWAYS | FA_WRITE)
+          == FR_OK) {
+
+        DEBUG_PRINT("Logging to: %s\n", usdLogConfig.filename);
+
+        // iniatialize crc
+        crc32ContextInit(&crcContext);
+
+        // write header
+        uint8_t magic = 0xBC;
+        usdWriteData(&magic, sizeof(magic));
+
+        uint16_t version = 2;
+        usdWriteData(&version, sizeof(version));
+
+        uint16_t numEventTypes = usdLogConfig.numEventConfigs;
+        usdWriteData(&numEventTypes, sizeof(numEventTypes));
+
+        for (int i = 0; i < numEventTypes; ++i) {
+          usdLogEventConfig_t* cfg = &usdLogConfig.eventConfigs[i];
+          const eventtrigger *et = eventtriggerGetById(cfg->eventId);
+          uint16_t numVariables = cfg->numVars;
+
+          usdWriteData(&cfg->eventId, sizeof(cfg->eventId));
+
+          if (cfg->eventId == FIXED_FREQUENCY_EVENT_ID) {
+            usdWriteData(FIXED_FREQUENCY_EVENT_NAME, strlen(FIXED_FREQUENCY_EVENT_NAME) + 1);
+          } else {
+            usdWriteData(et->name, strlen(et->name) + 1);
+            numVariables += et->numPayloadVariables;
+          }
+          usdWriteData(&numVariables, sizeof(numVariables));
+          if (et) {
+            for (int j = 0; j < et->numPayloadVariables; ++j) {
+              usdWriteData(et->payloadDesc[j].name, strlen(et->payloadDesc[j].name));
+              usdWriteData("(", 1);
+              char typeChar;
+              switch (et->payloadDesc[j].type)
+              {
+              case eventtriggerType_uint8:
+                typeChar = 'B';
+                break;
+              case eventtriggerType_int8:
+                typeChar = 'b';
+                break;
+              case eventtriggerType_uint16:
+                typeChar = 'H';
+                break;
+              case eventtriggerType_int16:
+                typeChar = 'h';
+                break;
+              case eventtriggerType_uint32:
+                typeChar = 'I';
+                break;
+              case eventtriggerType_int32:
+                typeChar = 'i';
+                break;
+              case eventtriggerType_float:
+                typeChar = 'f';
+                break;
+              case eventtrigerType_fp16:
+                typeChar = 'e';
+                break;
+              default:
+                ASSERT(false);
+              }
+              usdWriteData(&typeChar, 1);
+              usdWriteData(")", 2);
+            }
+          }
+          for (int j = 0; j < cfg->numVars; ++j) {
+            char *group;
+            char *name;
+            logVarId_t varid = cfg->varIds[j];
+            logGetGroupAndName(varid, &group, &name);
+            usdWriteData(group, strlen(group));
+            usdWriteData(".", 1);
+            usdWriteData(name, strlen(name));
+            usdWriteData("(", 1);
+            char typeChar;
+            switch (logGetType(varid)) {
+            case LOG_UINT8:
+              typeChar = 'B';
+              break;
+            case LOG_INT8:
+              typeChar = 'b';
+              break;
+            case LOG_UINT16:
+              typeChar = 'H';
+              break;
+            case LOG_INT16:
+              typeChar = 'h';
+              break;
+            case LOG_UINT32:
+              typeChar = 'I';
+              break;
+            case LOG_INT32:
+              typeChar = 'i';
+              break;
+            case LOG_FLOAT:
+              typeChar = 'f';
+              break;
+            default:
+              ASSERT(false);
+            }
+            usdWriteData(&typeChar, 1);
+            usdWriteData(")", 2);
+          }
+        }
+
+        while (enableLogging) {
+          /* sleep */
+          vTaskSuspend(NULL);
+
+          // check if we have anything to write
+          xSemaphoreTake(logBufferMutex, portMAX_DELAY);
+          const uint8_t* buf;
+          uint16_t size;
+          bool hasData = ringBuffer_pop_start(&logBuffer, &buf, &size);
+          xSemaphoreGive(logBufferMutex);
+
+          // execute the actual write operation
+          if (hasData) {
+            usdWriteData(buf, size);
+
+            xSemaphoreTake(logBufferMutex, portMAX_DELAY);
+            ringBuffer_pop_done(&logBuffer);
+            xSemaphoreGive(logBufferMutex);
+          }
+        }
+        // write everything that's still in the buffer
+        xSemaphoreTake(logBufferMutex, portMAX_DELAY);
+        while (true) {
+          const uint8_t *buf;
+          uint16_t size;
+          bool hasData = ringBuffer_pop_start(&logBuffer, &buf, &size);
+          if (hasData) {
+            usdWriteData(buf, size);
+            ringBuffer_pop_done(&logBuffer);
+          } else {
+            break;
+          }
+        }
+        xSemaphoreGive(logBufferMutex);
+
+        // write CRC
+        uint32_t crcValue = crc32Out(&crcContext);
+        usdWriteData(&crcValue, sizeof(crcValue));
+
+        // close file
+        f_close(&logFile);
+
+        // Update file size for fast query
+        FILINFO info;
+        if (f_stat(usdLogConfig.filename, &info) == FR_OK) {
+          lastFileSize = info.fsize;
+        }
+
+        DEBUG_PRINT("Wrote %ld B to: %s (%ld of %ld events)\n",
+          lastFileSize,
+          usdLogConfig.filename,
+          usdLogStats.eventsWritten,
+          usdLogStats.eventsRequested);
+
+        xSemaphoreGive(logFileMutex);
+      } else {
+        f_mount(NULL, "", 0);
+        DEBUG_PRINT("Failed to open file: %s\n", usdLogConfig.filename);
+        break;
+      }
+    }
+  }
+  /* something went wrong */
+  xHandleWriteTask = 0;
+  vTaskDelete(NULL);
+}
+
+static bool usdTest()
+{
+  if (!isInit) {
+    DEBUG_PRINT("Error while initializing uSD deck\n");
+  }
+#ifdef USD_RUN_DISKIO_FUNCTION_TESTS
+  int result;
+  extern int test_diskio (BYTE pdrv, UINT ncyc, DWORD* buff, UINT sz_buff);
+
+  result = test_diskio(0, 1, (DWORD*)&workBuff, sizeof(workBuff));
+  if (result) {
+    DEBUG_PRINT("(result=%d)\nFatFs diskio functions test [FAIL].\n", result);
+    isInit = false;
+  }
+  else {
+    DEBUG_PRINT("FatFs diskio functions test [OK].\n");
+  }
+#endif
+
+  return isInit;
+}
+
+static void usdTimer(xTimerHandle timer)
+{
+  SD_disk_timerproc(&sdSpiContext);
+}
+
+static const DeckDriver usd_deck = {
+    .vid = 0xBC,
+    .pid = 0x08,
+    .name = "bcUSD",
+    .usedGpio = DECK_USING_MISO|DECK_USING_MOSI|DECK_USING_SCK|DECK_USING_IO_4,
+    .usedPeriph = DECK_USING_SPI,
+    .init = usdInit,
+    .test = usdTest,
+};
+
+DECK_DRIVER(usd_deck);
+
+PARAM_GROUP_START(deck)
+
+/**
+ * @brief Nonzero if [SD-card deck](%https://store.bitcraze.io/collections/decks/products/sd-card-deck) is attached
+*/
+PARAM_ADD_CORE(PARAM_UINT8 | PARAM_RONLY, bcUSD, &isInit)
+
+PARAM_GROUP_STOP(deck)
+
+/**
+ * The micro SD card deck is used for on-board logging of data to a micro SD card.
+ */
+PARAM_GROUP_START(usd)
+/**
+ * @brief Non zero if logging is possible, 0 indicates there might be a problem with the logging configuration.
+ */
+PARAM_ADD_CORE(PARAM_UINT8 | PARAM_RONLY, canLog, &initSuccess)
+
+/**
+ * @brief Controls if logging to the SD-card is active. Set to 1 to start logging, set to 0 to stop logging (default).
+ */
+PARAM_ADD_CORE(PARAM_UINT8, logging, &enableLogging) /* use to start/stop logging*/
+PARAM_GROUP_STOP(usd)
+
+/**
+ * Micro-SD related log variables for debug purposes mainly.
+ */
+LOG_GROUP_START(usd)
+/**
+ * @brief SPI write rate (includes overhead) [bytes/s]
+ */
+STATS_CNT_RATE_LOG_ADD(spiWrBps, &spiWriteRate)
+/**
+ * @brief SPI read rate (includes overhead) [bytes/s]
+ */
+STATS_CNT_RATE_LOG_ADD(spiReBps, &spiReadRate)
+/**
+ * @brief Data write rate to the SD card [bytes/s]
+ */
+STATS_CNT_RATE_LOG_ADD(fatWrBps, &fatWriteRate)
+LOG_GROUP_STOP(usd)

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/zranger.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/>.
+ *
+ * vl53l0x.c: Time-of-flight distance sensor driver
+ */
+
+#define DEBUG_MODULE "VLX"
+
+#include "FreeRTOS.h"
+#include "task.h"
+
+#include "deck.h"
+#include "system.h"
+#include "debug.h"
+#include "log.h"
+#include "param.h"
+#include "range.h"
+#include "static_mem.h"
+
+#include "i2cdev.h"
+#include "zranger.h"
+#include "vl53l0x.h"
+
+#include "cf_math.h"
+
+// Measurement noise model
+static const float expPointA = 1.0f;
+static const float expStdA = 0.0025f; // STD at elevation expPointA [m]
+static const float expPointB = 1.3f;
+static const float expStdB = 0.2f;    // STD at elevation expPointB [m]
+static float expCoeff;
+
+#define RANGE_OUTLIER_LIMIT 3000 // the measured range is in [mm]
+
+static uint16_t range_last = 0;
+
+static bool isInit;
+
+static VL53L0xDev dev;
+
+void zRangerInit(DeckInfo* info)
+{
+  if (isInit)
+    return;
+
+  vl53l0xInit(&dev, I2C1_DEV, true);
+
+  xTaskCreate(zRangerTask, ZRANGER_TASK_NAME, ZRANGER_TASK_STACKSIZE, NULL, ZRANGER_TASK_PRI, NULL);
+
+  // pre-compute constant in the measurement noise model for kalman
+  expCoeff = logf(expStdB / expStdA) / (expPointB - expPointA);
+
+  isInit = true;
+}
+
+bool zRangerTest(void)
+{
+  bool testStatus;
+
+  if (!isInit)
+    return false;
+
+  testStatus  = vl53l0xTestConnection(&dev);
+
+  return testStatus;
+}
+
+void zRangerTask(void* arg)
+{
+  systemWaitStart();
+  TickType_t xLastWakeTime;
+
+  vl53l0xSetVcselPulsePeriod(&dev, VcselPeriodPreRange, 18);
+  vl53l0xSetVcselPulsePeriod(&dev, VcselPeriodFinalRange, 14);
+  vl53l0xStartContinuous(&dev, 0);
+
+  xLastWakeTime = xTaskGetTickCount();
+
+  while (1) {
+    vTaskDelayUntil(&xLastWakeTime, M2T(dev.measurement_timing_budget_ms));
+
+    range_last = vl53l0xReadRangeContinuousMillimeters(&dev);
+    rangeSet(rangeDown, range_last / 1000.0f);
+
+    // check if range is feasible and push into the estimator
+    // the sensor should not be able to measure >3 [m], and outliers typically
+    // occur as >8 [m] measurements
+    if (range_last < RANGE_OUTLIER_LIMIT) {
+      float distance = (float)range_last * 0.001f; // Scale from [mm] to [m]
+      float stdDev = expStdA * (1.0f  + expf( expCoeff * (distance - expPointA)));
+      rangeEnqueueDownRangeInEstimator(distance, stdDev, xTaskGetTickCount());
+    }
+  }
+}
+
+static const DeckDriver zranger_deck = {
+  .vid = 0xBC,
+  .pid = 0x09,
+  .name = "bcZRanger",
+  .usedGpio = 0x0C,
+
+  .init = zRangerInit,
+  .test = zRangerTest,
+};
+
+DECK_DRIVER(zranger_deck);
+
+PARAM_GROUP_START(deck)
+
+/**
+ * @brief Nonzero if [Z-ranger deck](%https://store.bitcraze.io/collections/decks/products/z-ranger-deck) is attached
+*/
+PARAM_ADD_CORE(PARAM_UINT8 | PARAM_RONLY, bcZRanger, &isInit)
+
+PARAM_GROUP_STOP(deck)

FlyPi/crazyflie-firmware-2021.06/src/deck/drivers/src/zranger2.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/>.
+ *
+ * vl53l0x.c: Time-of-flight distance sensor driver
+ */
+
+#define DEBUG_MODULE "ZR2"
+
+#include "FreeRTOS.h"
+#include "task.h"
+
+#include "config.h"
+#include "deck.h"
+#include "system.h"
+#include "debug.h"
+#include "log.h"
+#include "param.h"
+#include "range.h"
+#include "static_mem.h"
+
+#include "i2cdev.h"
+#include "zranger2.h"
+#include "vl53l1x.h"
+
+#include "cf_math.h"
+
+// Measurement noise model
+static const float expPointA = 2.5f;
+static const float expStdA = 0.0025f; // STD at elevation expPointA [m]
+static const float expPointB = 4.0f;
+static const float expStdB = 0.2f;    // STD at elevation expPointB [m]
+static float expCoeff;
+
+#define RANGE_OUTLIER_LIMIT 5000 // the measured range is in [mm]
+
+static uint16_t range_last = 0;
+
+static bool isInit;
+
+NO_DMA_CCM_SAFE_ZERO_INIT static VL53L1_Dev_t dev;
+
+static uint16_t zRanger2GetMeasurementAndRestart(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;
+}
+
+void zRanger2Init(DeckInfo* info)
+{
+  if (isInit)
+    return;
+
+  if (vl53l1xInit(&dev, I2C1_DEV))
+  {
+      DEBUG_PRINT("Z-down sensor [OK]\n");
+  }
+  else
+  {
+    DEBUG_PRINT("Z-down sensor [FAIL]\n");
+    return;
+  }
+
+  xTaskCreate(zRanger2Task, ZRANGER2_TASK_NAME, ZRANGER2_TASK_STACKSIZE, NULL, ZRANGER2_TASK_PRI, NULL);
+
+  // pre-compute constant in the measurement noise model for kalman
+  expCoeff = logf(expStdB / expStdA) / (expPointB - expPointA);
+
+  isInit = true;
+}
+
+bool zRanger2Test(void)
+{
+  if (!isInit)
+    return false;
+
+  return true;
+}
+
+void zRanger2Task(void* arg)
+{
+  TickType_t lastWakeTime;
+
+  systemWaitStart();
+
+  // Restart sensor
+  VL53L1_StopMeasurement(&dev);
+  VL53L1_SetDistanceMode(&dev, VL53L1_DISTANCEMODE_MEDIUM);
+  VL53L1_SetMeasurementTimingBudgetMicroSeconds(&dev, 25000);
+
+  VL53L1_StartMeasurement(&dev);
+
+  lastWakeTime = xTaskGetTickCount();
+
+  while (1) {
+    vTaskDelayUntil(&lastWakeTime, M2T(25));
+
+    range_last = zRanger2GetMeasurementAndRestart(&dev);
+    rangeSet(rangeDown, range_last / 1000.0f);
+
+    // check if range is feasible and push into the estimator
+    // the sensor should not be able to measure >5 [m], and outliers typically
+    // occur as >8 [m] measurements
+    if (range_last < RANGE_OUTLIER_LIMIT) {
+      float distance = (float)range_last * 0.001f; // Scale from [mm] to [m]
+      float stdDev = expStdA * (1.0f  + expf( expCoeff * (distance - expPointA)));
+      rangeEnqueueDownRangeInEstimator(distance, stdDev, xTaskGetTickCount());
+    }
+  }
+}
+
+static const DeckDriver zranger2_deck = {
+  .vid = 0xBC,
+  .pid = 0x0E,
+  .name = "bcZRanger2",
+  .usedGpio = 0x0C,
+
+  .init = zRanger2Init,
+  .test = zRanger2Test,
+};
+
+DECK_DRIVER(zranger2_deck);
+
+PARAM_GROUP_START(deck)
+
+/**
+ * @brief Nonzero if [Z-ranger deck v2](%https://store.bitcraze.io/collections/decks/products/z-ranger-deck-v2) is attached
+ */
+PARAM_ADD_CORE(PARAM_UINT8 | PARAM_RONLY, bcZRanger2, &isInit)
+
+PARAM_GROUP_STOP(deck)

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

+// Includes all the deck C API headers
+
+#ifndef __DECK_H__
+#define __DECK_H__
+
+/* Core: handles initialisation, discovery and drivers */
+#include "deck_core.h"
+
+/* Deck APIs */
+#include "deck_constants.h"
+#include "deck_digital.h"
+#include "deck_analog.h"
+#include "deck_spi.h"
+
+#endif

FlyPi/crazyflie-firmware-2021.06/src/deck/interface/deck_analog.h

+/*
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 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/>.
+ *
+ * deck_analog.h - Arduino-compatible analog input API
+ */
+
+#ifndef __DECK_ANALOG_H__
+#define __DECK_ANALOG_H__
+
+#include <stdint.h>
+#include "deck_constants.h"
+
+/* Voltage reference types for the analogReference() function. */
+#define DEFAULT 0
+#define VREF    3.0
+
+void adcInit(void);
+
+uint16_t analogRead(const deckPin_t pin);
+
+void analogReference(uint8_t type);
+
+void analogReadResolution(uint8_t bits);
+
+/*
+ * Read the voltage on a deck pin.
+ * @param[in] pin   deck pin to measure.
+ * @return          voltage in volts
+ */
+float analogReadVoltage(const deckPin_t pin);
+
+#endif

FlyPi/crazyflie-firmware-2021.06/src/deck/interface/deck_constants.h

+/*
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 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/>.
+ *
+ * deck_contants.h - Arduino-compatible constant definition
+ */
+
+#ifndef __DECK_CONSTANTS_H__
+#define __DECK_CONSTANTS_H__
+
+// For true and false
+#include <stdbool.h>
+
+#include "stm32fxxx.h"
+
+#define LOW 0x0
+#define HIGH 0x1
+
+#define INPUT           0x0
+#define OUTPUT          0x1
+#define INPUT_PULLUP    0x2
+#define INPUT_PULLDOWN  0x3
+
+/*
+ * The Deck port GPIO pins, as named by the deck port / expansion port / expansion breakout documentation on bitcraze.io.
+ * Sequenced according to the deckGPIOMapping struct, so that these can be used for lookup in the struct.
+ */
+
+// Type used to identify a pin in the deck API.
+// id is used as an index in the deckGPIOMapping array
+typedef struct {uint8_t id;} deckPin_t;
+
+extern const deckPin_t DECK_GPIO_RX1;
+extern const deckPin_t DECK_GPIO_TX1;
+extern const deckPin_t DECK_GPIO_SDA;
+extern const deckPin_t DECK_GPIO_SCL;
+extern const deckPin_t DECK_GPIO_IO1;
+extern const deckPin_t DECK_GPIO_IO2;
+extern const deckPin_t DECK_GPIO_IO3;
+extern const deckPin_t DECK_GPIO_IO4;
+extern const deckPin_t DECK_GPIO_TX2;
+extern const deckPin_t DECK_GPIO_RX2;
+extern const deckPin_t DECK_GPIO_SCK;
+extern const deckPin_t DECK_GPIO_MISO;
+extern const deckPin_t DECK_GPIO_MOSI;
+
+
+typedef const struct {
+  uint32_t periph;
+  GPIO_TypeDef* port;
+  uint16_t pin;
+  int8_t adcCh; /* -1 means no ADC available for this pin. */
+} deckGPIOMapping_t;
+
+extern deckGPIOMapping_t deckGPIOMapping[];
+
+#endif

FlyPi/crazyflie-firmware-2021.06/src/deck/interface/deck_core.h

+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-2012 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * deck_core.h - Definitions and interface to handle deck init and drivers
+ *               Implementation is in deck.c, deck_info.c and deck_drivers.c
+ */
+
+#ifndef __DECK_CODE_H__
+#define __DECK_CODE_H__
+
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "estimator.h"
+
+/* Maximum number of decks that can be enumerated */
+#define DECK_MAX_COUNT 4
+
+/* Main functions to init and test the decks, called during system initialisation */
+void deckInit(void);
+bool deckTest(void);
+
+/***** Driver TOC definitions ******/
+
+/* Used peripherals */
+#define DECK_USING_UART1   (1<<0)
+#define DECK_USING_UART2   (1<<1)
+#define DECK_USING_SPI     (1<<2)
+#define DECK_USING_TIMER3  (1<<3)
+#define DECK_USING_TIMER5  (1<<4)
+#define DECK_USING_TIMER14 (1<<5)
+
+/* Used GPIO */
+#define DECK_USING_PC11 (1<<0)
+#define DECK_USING_RX1  (1<<0)
+#define DECK_USING_PC10 (1<<1)
+#define DECK_USING_TX1  (1<<1)
+#define DECK_USING_PB7  (1<<2)
+#define DECK_USING_SDA  (1<<2)
+#define DECK_USING_PB6  (1<<3)
+#define DECK_USING_SCL  (1<<3)
+#define DECK_USING_PB8  (1<<4)
+#define DECK_USING_IO_1 (1<<4)
+#define DECK_USING_PB5  (1<<5)
+#define DECK_USING_IO_2 (1<<5)
+#define DECK_USING_PB4  (1<<6)
+#define DECK_USING_IO_3 (1<<6)
+#define DECK_USING_PC12 (1<<7)
+#define DECK_USING_IO_4 (1<<7)
+#define DECK_USING_PA2  (1<<8)
+#define DECK_USING_TX2  (1<<8)
+#define DECK_USING_PA3  (1<<9)
+#define DECK_USING_RX2  (1<<9)
+#define DECK_USING_PA5  (1<<10)
+#define DECK_USING_SCK  (1<<10)
+#define DECK_USING_PA6  (1<<11)
+#define DECK_USING_MISO (1<<11)
+#define DECK_USING_PA7  (1<<12)
+#define DECK_USING_MOSI (1<<12)
+
+struct deckInfo_s;
+struct deckFwUpdate_s;
+
+/* Structure definition and registering macro */
+typedef struct deck_driver {
+  /* Identification of the deck (written in the board) */
+  uint8_t vid;
+  uint8_t pid;
+  char *name;
+
+  /* Periphreal and Gpio used _dirrectly_ by the driver */
+  uint32_t usedPeriph;
+  uint32_t usedGpio;
+
+  /* Required system properties */
+  StateEstimatorType requiredEstimator;
+  bool requiredLowInterferenceRadioMode;
+
+  // Deck memory access definition
+  const struct deckMemDef_s* memoryDef;
+
+  /* Init and test functions */
+  void (*init)(struct deckInfo_s *);
+  bool (*test)(void);
+} DeckDriver;
+
+#define DECK_DRIVER(NAME) const struct deck_driver * driver_##NAME __attribute__((section(".deckDriver." #NAME), used)) = &(NAME)
+
+/****** Deck_info *******/
+
+#define DECK_INFO_HEADER_ID 0xeb
+#define DECK_INFO_HEADER_SIZE 7
+#define DECK_INFO_TLV_VERSION 0
+#define DECK_INFO_TLV_VERSION_POS 8
+#define DECK_INFO_TLV_LENGTH_POS 9
+#define DECK_INFO_TLV_DATA_POS 10
+
+typedef struct {
+  uint8_t *data;
+  int length;
+} TlvArea;
+
+typedef struct deckInfo_s {
+
+  union {
+    struct {
+      uint8_t header;
+      uint32_t usedPins;
+      uint8_t vid;
+      uint8_t pid;
+      uint8_t crc;
+
+      uint8_t rawTlv[104];
+    } __attribute__((packed));
+
+    uint8_t raw[112];
+  };
+
+  TlvArea tlv;
+  const DeckDriver *driver;
+} DeckInfo;
+
+/**
+ * @brief Definition of function that is called when a block of a new firmware is uploaded to the deck.
+ * The upload will be done in small but continouse pieces.
+ * @param address: Address where the buffer should be written. The start of the firmware is at address 0.
+ * @param len: Buffer length
+ * @param buffer: Buffer to write in the firmware memory
+ *
+ * @return True if the buffer could be written successully, false otherwise (if the deck if not in bootloader
+ *         mode for example)
+ */
+typedef bool (deckMemoryWrite)(const uint32_t vAddr, const uint8_t len, const uint8_t* buffer);
+
+/**
+ * @brief Definition of function to read the firmware
+ *
+ * @param addr: Address where the data should be read. The start of the firmware is at address 0.
+ * @param len: Length to read.
+ * @param buffer: Buffer where to output the data
+ *
+ * @return True if the buffer could be read successully, false otherwise (if the deck if not in bootloader
+ *         mode for example)
+ */
+typedef bool (deckMemoryRead)(const uint32_t vAddr, const uint8_t len, uint8_t* buffer);
+
+#define DECK_MEMORY_MASK_STARTED 1
+#define DECK_MEMORY_MASK_UPGRADE_REQUIRED 2
+#define DECK_MEMORY_MASK_BOOT_LOADER_ACTIVE 4
+
+/**
+ * @brief Definition of function to query a deck for properties related to memory
+ *
+ * @return bitfield using the DECK_MEMORY_MASK_XXX definitions
+ */
+typedef uint8_t (deckMemoryProperties)();
+
+/**
+ * @brief This struct defines the firmware required by the deck and the function
+ * to use to flash new firmware to the deck.
+ */
+typedef struct deckMemDef_s {
+  // Functions that will be called to read or write to the deck memory.
+  deckMemoryWrite* write;
+  deckMemoryRead* read;
+
+  // Function to query properties of the deck memory
+  deckMemoryProperties* properties;
+
+  // True if the deck supports FW upgrades
+  bool supportsUpgrade;
+
+  // Definition of the required firmware for the deck (if supported)
+  uint32_t requiredHash;
+  // TOOD krri rename to length?
+  uint32_t requiredSize;
+
+} DeckMemDef_t;
+
+int deckCount(void);
+
+DeckInfo * deckInfo(int i);
+
+/* Key/value area handling */
+bool deckTlvHasElement(TlvArea *tlv, int type);
+
+int deckTlvGetString(TlvArea *tlv, int type, char *string, int maxLength);
+
+char* deckTlvGetBuffer(TlvArea *tlv, int type, int *length);
+
+void deckTlvGetTlv(TlvArea *tlv, int type, TlvArea *output);
+
+/* Defined Types */
+#define DECK_INFO_NAME 1
+#define DECK_INFO_REVISION 2
+#define DECK_INFO_CUSTOMDATA 3
+
+/***** Drivers introspection API ******/
+
+/* Returns the number of driver registered */
+int deckDriverCount();
+
+/* Returns one driver definition */
+const struct deck_driver* deckGetDriver(int i);
+
+/* Find driver by pid/vid */
+const struct deck_driver* deckFindDriverByVidPid(uint8_t vid, uint8_t pid);
+
+/*find driver by name */
+const struct deck_driver* deckFindDriverByName(char* name);
+
+StateEstimatorType deckGetRequiredEstimator();
+
+bool deckGetRequiredLowInterferenceRadioMode();
+
+#endif //__DECK_CODE_H__

FlyPi/crazyflie-firmware-2021.06/src/deck/interface/deck_digital.h

+/*
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 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/>.
+ *
+ * deck_digital.h - Arduino-compatible digital I/O API
+ */
+
+#ifndef __DECK_DIGITAL_H__
+#define __DECK_DIGITAL_H__
+
+#include <stdint.h>
+#include "deck_constants.h"
+
+void pinMode(const deckPin_t pin, const uint32_t mode);
+
+void digitalWrite(const deckPin_t pin, const uint32_t val);
+
+int digitalRead(const deckPin_t pin);
+
+#endif

FlyPi/crazyflie-firmware-2021.06/src/deck/interface/deck_memory.h

+/**
+ * ,---------,       ____  _ __
+ * |  ,-^-,  |      / __ )(_) /_______________ _____  ___
+ * | (  O  ) |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * | / ,--´  |    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *    +------`   /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2021 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ *
+ * deck_memory.h - interface for reading/writing deck memory and updating
+ *                 firmware and binary data on decks, using the memory
+ *                 subsystem.
+ *
+ */
+
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+void deckMemoryInit();

FlyPi/crazyflie-firmware-2021.06/src/deck/interface/deck_spi.h

+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 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/>.
+ *
+ * deck_spi.h - Deck-API SPI communication header
+ */
+#ifndef SPI_H_
+#define SPI_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+
+// Based on 84MHz peripheral clock
+#define SPI_BAUDRATE_21MHZ  SPI_BaudRatePrescaler_4     // 21MHz
+#define SPI_BAUDRATE_12MHZ  SPI_BaudRatePrescaler_8     // 11.5MHz
+#define SPI_BAUDRATE_6MHZ   SPI_BaudRatePrescaler_16    // 5.25MHz
+#define SPI_BAUDRATE_3MHZ   SPI_BaudRatePrescaler_32    // 2.625MHz
+#define SPI_BAUDRATE_2MHZ   SPI_BaudRatePrescaler_64    // 1.3125MHz
+
+/**
+ * Initialize the SPI.
+ */
+void spiBegin(void);
+void spiBeginTransaction(uint16_t baudRatePrescaler);
+void spiEndTransaction();
+
+/* Send the data_tx buffer and receive into the data_rx buffer */
+bool spiExchange(size_t length, const uint8_t *data_tx, uint8_t *data_rx);
+
+#endif /* SPI_H_ */

FlyPi/crazyflie-firmware-2021.06/src/deck/interface/deck_spi3.h

+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 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/>.
+ *
+ * deck_spi3.h - Deck-API SPI3 communication header
+ */
+#ifndef SPI3_H_
+#define SPI3_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+
+#define SPI3_BAUDRATE_21MHZ  SPI_BaudRatePrescaler_2     // 21MHz
+#define SPI3_BAUDRATE_12MHZ  SPI_BaudRatePrescaler_4     // 11.5MHz
+#define SPI3_BAUDRATE_6MHZ   SPI_BaudRatePrescaler_8    // 5.25MHz
+#define SPI3_BAUDRATE_3MHZ   SPI_BaudRatePrescaler_16    // 2.625MHz
+#define SPI3_BAUDRATE_2MHZ   SPI_BaudRatePrescaler_32    // 1.3125MHz
+
+/**
+ * Initialize the SPI.
+ */
+void spi3Begin(void);
+void spi3BeginTransaction(uint16_t baudRatePrescaler);
+void spi3EndTransaction();
+
+/* Send the data_tx buffer and receive into the data_rx buffer */
+bool spi3Exchange(size_t length, const uint8_t *data_tx, uint8_t *data_rx);
+
+#endif /* SPI3_H_ */